bertos/net/afsk.c

   1 /**
   2  * \file
   3  * <!--
   4  * This file is part of BeRTOS.
   5  *
   6  * Bertos is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  19  *
  20  * As a special exception, you may use this file as part of a free software
  21  * library without restriction.  Specifically, if other files instantiate
  22  * templates or use macros or inline functions from this file, or you compile
  23  * this file and link it with other files to produce an executable, this
  24  * file does not by itself cause the resulting executable to be covered by
  25  * the GNU General Public License.  This exception does not however
  26  * invalidate any other reasons why the executable file might be covered by
  27  * the GNU General Public License.
  28  *
  29  * Copyright 2008 Develer S.r.l. (http://www.develer.com/)
  30  *
  31  * -->
  32  *
  33  * \brief AFSK1200 modem.
  34  *
  35  * \version $Id$
  36  * \author Francesco Sacchi <asterix@develer.com>
  37  */
  38
  39 #include "afsk.h"
  40 #include <net/ax25.h>
  41
  42 #include "cfg/cfg_afsk.h"
  43 #include "hw/hw_afsk.h"
  44
  45 #include <drv/timer.h>
  46
  47 #include <cfg/module.h>
  48
  49 #include <cpu/power.h>
  50 #include <struct/fifobuf.h>
  51
  52 #include <string.h> /* memset */
  53
  54 #define PHASE_BIT    8
  55 #define PHASE_INC    1
  56
  57 #define PHASE_MAX    (SAMPLEPERBIT * PHASE_BIT)
  58 #define PHASE_THRES  (PHASE_MAX / 2) // - PHASE_BIT / 2)
  59
  60 // Modulator constants
  61 #define MARK_FREQ  1200
  62 #define MARK_INC   (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)MARK_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
  63
  64 #define SPACE_FREQ 2200
  65 #define SPACE_INC  (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)SPACE_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
  66
  67 //Ensure sample rate is a multiple of bit rate
  68 STATIC_ASSERT(!(CONFIG_AFSK_DAC_SAMPLERATE % BITRATE));
  69
  70 #define DAC_SAMPLEPERBIT (CONFIG_AFSK_DAC_SAMPLERATE / BITRATE)
  71
  72 /**
  73  * Sine table for the first quarter of wave.
  74  * The rest of the wave is computed from this first quarter.
  75  * This table is used to generate the modulated data.
  76  */
  77 static const uint8_t sin_table[] =
  78 {
  79         //TODO put in flash!
  80         128, 129, 131, 132, 134, 135, 137, 138, 140, 142, 143, 145, 146, 148, 149, 151,
  81         152, 154, 155, 157, 158, 160, 162, 163, 165, 166, 167, 169, 170, 172, 173, 175,
  82         176, 178, 179, 181, 182, 183, 185, 186, 188, 189, 190, 192, 193, 194, 196, 197,
  83         198, 200, 201, 202, 203, 205, 206, 207, 208, 210, 211, 212, 213, 214, 215, 217,
  84         218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233,
  85         234, 234, 235, 236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245,
  86         245, 246, 246, 247, 248, 248, 249, 249, 250, 250, 250, 251, 251, 252, 252, 252,
  87         253, 253, 253, 253, 254, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255,
  88 };
  89
  90 #define SIN_LEN 512 ///< Full wave length
  91
  92 STATIC_ASSERT(sizeof(sin_table) == SIN_LEN / 4);
  93
  94
  95 /**
  96  * Given the index, this function computes the correct sine sample
  97  * based only on the first quarter of wave.
  98  */
  99 INLINE uint8_t sin_sample(uint16_t idx)
 100 {
 101         ASSERT(idx < SIN_LEN);
 102         uint16_t new_idx = idx % (SIN_LEN / 2);
 103         new_idx = (new_idx >= (SIN_LEN / 4)) ? (SIN_LEN / 2 - new_idx - 1) : new_idx;
 104         return (idx >= (SIN_LEN / 2)) ? (255 - sin_table[new_idx]) : sin_table[new_idx];
 105 }
 106
 107
 108 #define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
 109 #define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)
 110
 111
 112 static void hdlc_parse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo)
 113 {
 114         hdlc->demod_bits <<= 1;
 115         hdlc->demod_bits |= bit ? 1 : 0;
 116
 117         /* HDLC Flag */
 118         if (hdlc->demod_bits == HDLC_FLAG)
 119         {
 120                 if (!fifo_isfull(fifo))
 121                 {
 122                         fifo_push(fifo, HDLC_FLAG);
 123                         hdlc->rxstart = true;
 124                 }
 125                 else
 126                         hdlc->rxstart = false;
 127
 128                 hdlc->currchar = 0;
 129                 hdlc->bit_idx = 0;
 130                 return;
 131         }
 132
 133         /* Reset */
 134         if ((hdlc->demod_bits & HDLC_RESET) == HDLC_RESET)
 135         {
 136                 hdlc->rxstart = false;
 137                 return;
 138         }
 139
 140         if (!hdlc->rxstart)
 141                 return;
 142
 143         /* Stuffed bit */
 144         if ((hdlc->demod_bits & 0x3f) == 0x3e)
 145                 return;
 146
 147         if (hdlc->demod_bits & 0x01)
 148                 hdlc->currchar |= 0x80;
 149
 150         if (++hdlc->bit_idx >= 8)
 151         {
 152                 if ((hdlc->currchar == HDLC_FLAG
 153                         || hdlc->currchar == HDLC_RESET
 154                         || hdlc->currchar == AX25_ESC))
 155                 {
 156                         if (!fifo_isfull(fifo))
 157                                 fifo_push(fifo, AX25_ESC);
 158                         else
 159                                 hdlc->rxstart = false;
 160                 }
 161
 162                 if (!fifo_isfull(fifo))
 163                         fifo_push(fifo, hdlc->currchar);
 164                 else
 165                         hdlc->rxstart = false;
 166
 167                 hdlc->currchar = 0;
 168                 hdlc->bit_idx = 0;
 169                 return;
 170         }
 171
 172         hdlc->currchar >>= 1;
 173 }
 174
 175
 176 /**
 177  * ADC ISR callback.
 178  * This function has to be called by the ADC ISR when a sample of the configured
 179  * channel is available.
 180  * \param af Afsk context to operate one (\see Afsk).
 181  * \param curr_sample current sample from the ADC.
 182  */
 183 void afsk_adc_isr(Afsk *af, int8_t curr_sample)
 184 {
 185         AFSK_STROBE_ON();
 186
 187         /*
 188          * Frequency discriminator and LP IIR filter.
 189          * This filter is designed to work
 190          * at the given sample rate and bit rate.
 191          */
 192         STATIC_ASSERT(SAMPLERATE == 9600);
 193         STATIC_ASSERT(BITRATE == 1200);
 194
 195         /*
 196          * Frequency discrimination is achieved by simply multiplying
 197          * the sample with a delayed sample of (samples per bit) / 2.
 198          * Then the signal is lowpass filtered with a first order,
 199          * 600 Hz filter. The filter implementation is selectable
 200          * through the CONFIG_AFSK_FILTER config variable.
 201          */
 202
 203         af->iir_x[0] = af->iir_x[1];
 204
 205         #if (CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH)
 206                 af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
 207                 //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 6.027339492;
 208         #elif (CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV)
 209                 af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
 210                 //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 3.558147322;
 211         #else
 212                 #error Filter type not found!
 213         #endif
 214
 215         af->iir_y[0] = af->iir_y[1];
 216
 217         #if CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH
 218                 /*
 219                  * This strange sum + shift is an optimization for af->iir_y[0] * 0.668.
 220                  * iir * 0.668 ~= (iir * 21) / 32 =
 221                  * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
 222                  * = iir / 2 + iir / 8 + iir / 32 =
 223                  * = iir >> 1 + iir >> 3 + iir >> 5
 224                  */
 225                 af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + (af->iir_y[0] >> 1) + (af->iir_y[0] >> 3) + (af->iir_y[0] >> 5);
 226                 //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.6681786379;
 227         #elif CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV
 228                 /*
 229                  * This should be (af->iir_y[0] * 0.438) but
 230                  * (af->iir_y[0] >> 1) is a faster approximation :-)
 231                  */
 232                 af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + (af->iir_y[0] >> 1);
 233                 //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.4379097269;
 234         #endif
 235
 236         /* Save this sampled bit in a delay line */
 237         af->sampled_bits <<= 1;
 238         af->sampled_bits |= (af->iir_y[1] > 0) ? 1 : 0;
 239
 240         /* Store current ADC sample in the af->delay_fifo */
 241         fifo_push(&af->delay_fifo, curr_sample);
 242
 243         /* If there is an edge, adjust phase sampling */
 244         if (EDGE_FOUND(af->sampled_bits))
 245         {
 246                 if (af->curr_phase < PHASE_THRES)
 247                         af->curr_phase += PHASE_INC;
 248                 else
 249                         af->curr_phase -= PHASE_INC;
 250         }
 251         af->curr_phase += PHASE_BIT;
 252
 253         /* sample the bit */
 254         if (af->curr_phase >= PHASE_MAX)
 255         {
 256                 af->curr_phase %= PHASE_MAX;
 257
 258                 /* Shift 1 position in the shift register of the found bits */
 259                 af->found_bits <<= 1;
 260
 261                 /*
 262                  * Determine bit value by reading the last 3 sampled bits.
 263                  * If the number of ones is two or greater, the bit value is a 1,
 264                  * otherwise is a 0.
 265                  * This algorithm presumes that there are 8 samples per bit.
 266                  */
 267                 STATIC_ASSERT(SAMPLEPERBIT == 8);
 268                 uint8_t bits = af->sampled_bits & 0x07;
 269                 if (bits == 0x07 // 111, 3 bits set to 1
 270                  || bits == 0x06 // 110, 2 bits
 271                  || bits == 0x05 // 101, 2 bits
 272                  || bits == 0x03 // 011, 2 bits
 273                 )
 274                         af->found_bits |= 1;
 275
 276                 /*
 277                  * NRZI coding: if 2 consecutive bits have the same value
 278                  * a 1 is received, otherwise it's a 0.
 279                  */
 280                 hdlc_parse(&af->hdlc, !EDGE_FOUND(af->found_bits), &af->rx_fifo);
 281         }
 282
 283
 284         AFSK_STROBE_OFF();
 285 }
 286
 287 static void afsk_txStart(Afsk *af)
 288 {
 289         if (!af->sending)
 290         {
 291                 af->phase_inc = MARK_INC;
 292                 af->phase_acc = 0;
 293                 af->stuff_cnt = 0;
 294                 af->sending = true;
 295                 af->preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
 296                 AFSK_DAC_IRQ_START(af->dac_ch);
 297         }
 298         ATOMIC(af->trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
 299 }
 300
 301 #define BIT_STUFF_LEN 5
 302
 303 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 304
 305 /**
 306  * DAC ISR callback.
 307  * This function has to be called by the DAC ISR when a sample of the configured
 308  * channel has been converted out.
 309  *
 310  * \param af Afsk context to operate one (\see Afsk).
 311  *
 312  * \note The next DAC output sample is supplied by the Afsk driver through calling
 313  *        the AFSK_DAC_SET() callback.
 314  */
 315 void afsk_dac_isr(Afsk *af)
 316 {
 317         /* Check if we are at a start of a sample cycle */
 318         if (af->sample_count == 0)
 319         {
 320                 if (af->tx_bit == 0)
 321                 {
 322                         /* We have just finished transimitting a char, get a new one. */
 323                         if (fifo_isempty(&af->tx_fifo) && af->trailer_len == 0)
 324                         {
 325                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 326                                 af->sending = false;
 327                                 return;
 328                         }
 329                         else
 330                         {
 331                                 /*
 332                                  * If we have just finished af->sending an unstuffed byte,
 333                                  * reset bitstuff counter.
 334                                  */
 335                                 if (!af->bit_stuff)
 336                                         af->stuff_cnt = 0;
 337
 338                                 af->bit_stuff = true;
 339
 340                                 /*
 341                                  * Handle preamble and trailer
 342                                  */
 343                                 if (af->preamble_len == 0)
 344                                 {
 345                                         if (fifo_isempty(&af->tx_fifo))
 346                                         {
 347                                                 af->trailer_len--;
 348                                                 af->curr_out = HDLC_FLAG;
 349                                         }
 350                                         else
 351                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 352                                 }
 353                                 else
 354                                 {
 355                                         af->preamble_len--;
 356                                         af->curr_out = HDLC_FLAG;
 357                                 }
 358
 359                                 /* Handle char escape */
 360                                 if (af->curr_out == AX25_ESC)
 361                                 {
 362                                         if (fifo_isempty(&af->tx_fifo))
 363                                         {
 364                                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 365                                                 af->sending = false;
 366                                                 return;
 367                                         }
 368                                         else
 369                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 370                                 }
 371                                 else if (af->curr_out == HDLC_FLAG || af->curr_out == HDLC_RESET)
 372                                         /* If these chars are not escaped disable bit stuffing */
 373                                         af->bit_stuff = false;
 374                         }
 375                         /* Start with LSB mask */
 376                         af->tx_bit = 0x01;
 377                 }
 378
 379                 /* check for bit stuffing */
 380                 if (af->bit_stuff && af->stuff_cnt >= BIT_STUFF_LEN)
 381                 {
 382                         /* If there are more than 5 ones in a row insert a 0 */
 383                         af->stuff_cnt = 0;
 384                         /* switch tone */
 385                         af->phase_inc = SWITCH_TONE(af->phase_inc);
 386                 }
 387                 else
 388                 {
 389                         /*
 390                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 391                          * unchanged; with a 0, there will be a change in the tone.
 392                          */
 393                         if (af->curr_out & af->tx_bit)
 394                         {
 395                                 /*
 396                                  * Transmit a 1:
 397                                  * - Stay on the previous tone
 398                                  * - Increace bit stuff count
 399                                  */
 400                                 af->stuff_cnt++;
 401                         }
 402                         else
 403                         {
 404                                 /*
 405                                  * Transmit a 0:
 406                                  * - Reset bit stuff count
 407                                  * - Switch tone
 408                                  */
 409                                 af->stuff_cnt = 0;
 410                                 af->phase_inc = SWITCH_TONE(af->phase_inc);
 411                         }
 412
 413                         /* Go to the next bit */
 414                         af->tx_bit <<= 1;
 415                 }
 416                 af->sample_count = DAC_SAMPLEPERBIT;
 417         }
 418
 419         /* Get new sample and put it out on the DAC */
 420         af->phase_acc += af->phase_inc;
 421         af->phase_acc %= SIN_LEN;
 422
 423         AFSK_DAC_SET(af->dac_ch, sin_sample(af->phase_acc));
 424         af->sample_count--;
 425 }
 426
 427
 428 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
 429 {
 430         Afsk *af = AFSK_CAST(fd);
 431         uint8_t *buf = (uint8_t *)_buf;
 432
 433         #if CONFIG_AFSK_RXTIMEOUT == 0
 434         while (size-- && !fifo_isempty_locked(&af->rx_fifo))
 435         #else
 436         while (size--)
 437         #endif
 438         {
 439                 #if CONFIG_AFSK_RXTIMEOUT != -1
 440                 ticks_t start = timer_clock();
 441                 #endif
 442
 443                 while (fifo_isempty_locked(&af->rx_fifo));
 444                 {
 445                         cpu_relax();
 446                         #if CONFIG_AFSK_RXTIMEOUT != -1
 447                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 448                                 return buf - (uint8_t *)_buf;
 449                         #endif
 450                 }
 451
 452                 *buf++ = fifo_pop_locked(&af->rx_fifo);
 453         }
 454
 455         return buf - (uint8_t *)_buf;
 456 }
 457
 458 static size_t afsk_write(KFile *fd, const void *_buf, size_t size)
 459 {
 460         Afsk *af = AFSK_CAST(fd);
 461         const uint8_t *buf = (const uint8_t *)_buf;
 462
 463         while (size--)
 464         {
 465                 while (fifo_isfull_locked(&af->tx_fifo))
 466                         cpu_relax();
 467
 468                 fifo_push_locked(&af->tx_fifo, *buf++);
 469                 afsk_txStart(af);
 470         }
 471
 472         return buf - (const uint8_t *)_buf;
 473 }
 474
 475 static int afsk_flush(KFile *fd)
 476 {
 477         Afsk *af = AFSK_CAST(fd);
 478         while (af->sending)
 479                 cpu_relax();
 480         return 0;
 481 }
 482
 483
 484 /**
 485  * Initialize an AFSK1200 modem.
 486  * \param af Afsk context to operate one (\see Afsk).
 487  * \param adc_ch  ADC channel used by the demodulator.
 488  * \param dac_ch  DAC channel used by the modulator.
 489  */
 490 void afsk_init(Afsk *af, int adc_ch, int dac_ch)
 491 {
 492         #if CONFIG_AFSK_RXTIMEOUT != -1
 493         MOD_CHECK(timer);
 494         #endif
 495         memset(af, 0, sizeof(*af));
 496         af->adc_ch = adc_ch;
 497         af->dac_ch = dac_ch;
 498
 499         fifo_init(&af->delay_fifo, (uint8_t *)af->delay_buf, sizeof(af->delay_buf));
 500         fifo_init(&af->rx_fifo, af->rx_buf, sizeof(af->rx_buf));
 501
 502         /* Fill sample FIFO with 0 */
 503         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 504                 fifo_push(&af->delay_fifo, 0);
 505
 506         fifo_init(&af->tx_fifo, af->tx_buf, sizeof(af->tx_buf));
 507
 508         AFSK_ADC_INIT(adc_ch, af);
 509         AFSK_DAC_INIT(dac_ch, af);
 510         AFSK_STROBE_INIT();
 511         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 512
 513         DB(af->fd._type = KFT_AFSK);
 514         af->fd.write = afsk_write;
 515         af->fd.read = afsk_read;
 516         af->fd.flush = afsk_flush;
 517         af->phase_inc = MARK_INC;
 518 }