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 #define LOG_LEVEL   AFSK_LOG_LEVEL
  50 #define LOG_FORMAT  AFSK_LOG_FORMAT
  51 #include <cfg/log.h>
  52
  53 #include <cpu/power.h>
  54 #include <cpu/pgm.h>
  55 #include <struct/fifobuf.h>
  56
  57 #include <string.h> /* memset */
  58
  59 #define PHASE_BIT    8
  60 #define PHASE_INC    1
  61
  62 #define PHASE_MAX    (SAMPLEPERBIT * PHASE_BIT)
  63 #define PHASE_THRES  (PHASE_MAX / 2) // - PHASE_BIT / 2)
  64
  65 // Modulator constants
  66 #define MARK_FREQ  1200
  67 #define MARK_INC   (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)MARK_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
  68
  69 #define SPACE_FREQ 2200
  70 #define SPACE_INC  (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)SPACE_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
  71
  72 //Ensure sample rate is a multiple of bit rate
  73 STATIC_ASSERT(!(CONFIG_AFSK_DAC_SAMPLERATE % BITRATE));
  74
  75 #define DAC_SAMPLEPERBIT (CONFIG_AFSK_DAC_SAMPLERATE / BITRATE)
  76
  77 /**
  78  * Sine table for the first quarter of wave.
  79  * The rest of the wave is computed from this first quarter.
  80  * This table is used to generate the modulated data.
  81  */
  82 static const uint8_t PROGMEM sin_table[] =
  83 {
  84         128, 129, 131, 132, 134, 135, 137, 138, 140, 142, 143, 145, 146, 148, 149, 151,
  85         152, 154, 155, 157, 158, 160, 162, 163, 165, 166, 167, 169, 170, 172, 173, 175,
  86         176, 178, 179, 181, 182, 183, 185, 186, 188, 189, 190, 192, 193, 194, 196, 197,
  87         198, 200, 201, 202, 203, 205, 206, 207, 208, 210, 211, 212, 213, 214, 215, 217,
  88         218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233,
  89         234, 234, 235, 236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245,
  90         245, 246, 246, 247, 248, 248, 249, 249, 250, 250, 250, 251, 251, 252, 252, 252,
  91         253, 253, 253, 253, 254, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255,
  92 };
  93
  94 #define SIN_LEN 512 ///< Full wave length
  95
  96 STATIC_ASSERT(sizeof(sin_table) == SIN_LEN / 4);
  97
  98
  99 /**
 100  * Given the index, this function computes the correct sine sample
 101  * based only on the first quarter of wave.
 102  */
 103 INLINE uint8_t sin_sample(uint16_t idx)
 104 {
 105         ASSERT(idx < SIN_LEN);
 106         uint16_t new_idx = idx % (SIN_LEN / 2);
 107         new_idx = (new_idx >= (SIN_LEN / 4)) ? (SIN_LEN / 2 - new_idx - 1) : new_idx;
 108
 109         #if CPU_HARVARD
 110                 uint8_t data = pgm_read_char(&sin_table[new_idx]);
 111         #else
 112                 uint8_t data = sin_table[new_idx];
 113         #endif
 114
 115         return (idx >= (SIN_LEN / 2)) ? (255 - data) : data;
 116 }
 117
 118
 119 #define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
 120 #define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)
 121
 122 /**
 123  * High-Level Data Link Control parsing function.
 124  * Parse bitstream in order to find characters.
 125  *
 126  * \param hdlc HDLC context.
 127  * \param bit  current bit to be parsed.
 128  * \param fifo FIFO buffer used to push characters.
 129  *
 130  * \return true if all is ok, false if the fifo is full.
 131  */
 132 static bool hdlc_parse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo)
 133 {
 134         bool ret = true;
 135
 136         hdlc->demod_bits <<= 1;
 137         hdlc->demod_bits |= bit ? 1 : 0;
 138
 139         /* HDLC Flag */
 140         if (hdlc->demod_bits == HDLC_FLAG)
 141         {
 142                 if (!fifo_isfull(fifo))
 143                 {
 144                         fifo_push(fifo, HDLC_FLAG);
 145                         hdlc->rxstart = true;
 146                 }
 147                 else
 148                 {
 149                         ret = false;
 150                         hdlc->rxstart = false;
 151                 }
 152
 153                 hdlc->currchar = 0;
 154                 hdlc->bit_idx = 0;
 155                 return ret;
 156         }
 157
 158         /* Reset */
 159         if ((hdlc->demod_bits & HDLC_RESET) == HDLC_RESET)
 160         {
 161                 hdlc->rxstart = false;
 162                 return ret;
 163         }
 164
 165         if (!hdlc->rxstart)
 166                 return ret;
 167
 168         /* Stuffed bit */
 169         if ((hdlc->demod_bits & 0x3f) == 0x3e)
 170                 return ret;
 171
 172         if (hdlc->demod_bits & 0x01)
 173                 hdlc->currchar |= 0x80;
 174
 175         if (++hdlc->bit_idx >= 8)
 176         {
 177                 if ((hdlc->currchar == HDLC_FLAG
 178                         || hdlc->currchar == HDLC_RESET
 179                         || hdlc->currchar == AX25_ESC))
 180                 {
 181                         if (!fifo_isfull(fifo))
 182                                 fifo_push(fifo, AX25_ESC);
 183                         else
 184                         {
 185                                 hdlc->rxstart = false;
 186                                 ret = false;
 187                         }
 188                 }
 189
 190                 if (!fifo_isfull(fifo))
 191                         fifo_push(fifo, hdlc->currchar);
 192                 else
 193                 {
 194                         hdlc->rxstart = false;
 195                         ret = false;
 196                 }
 197
 198                 hdlc->currchar = 0;
 199                 hdlc->bit_idx = 0;
 200         }
 201         else
 202                 hdlc->currchar >>= 1;
 203
 204         return ret;
 205 }
 206
 207
 208 /**
 209  * ADC ISR callback.
 210  * This function has to be called by the ADC ISR when a sample of the configured
 211  * channel is available.
 212  * \param af Afsk context to operate on.
 213  * \param curr_sample current sample from the ADC.
 214  */
 215 void afsk_adc_isr(Afsk *af, int8_t curr_sample)
 216 {
 217         AFSK_STROBE_ON();
 218
 219         /*
 220          * Frequency discriminator and LP IIR filter.
 221          * This filter is designed to work
 222          * at the given sample rate and bit rate.
 223          */
 224         STATIC_ASSERT(SAMPLERATE == 9600);
 225         STATIC_ASSERT(BITRATE == 1200);
 226
 227         /*
 228          * Frequency discrimination is achieved by simply multiplying
 229          * the sample with a delayed sample of (samples per bit) / 2.
 230          * Then the signal is lowpass filtered with a first order,
 231          * 600 Hz filter. The filter implementation is selectable
 232          * through the CONFIG_AFSK_FILTER config variable.
 233          */
 234
 235         af->iir_x[0] = af->iir_x[1];
 236
 237         #if (CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH)
 238                 af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
 239                 //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 6.027339492;
 240         #elif (CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV)
 241                 af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
 242                 //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 3.558147322;
 243         #else
 244                 #error Filter type not found!
 245         #endif
 246
 247         af->iir_y[0] = af->iir_y[1];
 248
 249         #if CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH
 250                 /*
 251                  * This strange sum + shift is an optimization for af->iir_y[0] * 0.668.
 252                  * iir * 0.668 ~= (iir * 21) / 32 =
 253                  * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
 254                  * = iir / 2 + iir / 8 + iir / 32 =
 255                  * = iir >> 1 + iir >> 3 + iir >> 5
 256                  */
 257                 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);
 258                 //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.6681786379;
 259         #elif CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV
 260                 /*
 261                  * This should be (af->iir_y[0] * 0.438) but
 262                  * (af->iir_y[0] >> 1) is a faster approximation :-)
 263                  */
 264                 af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + (af->iir_y[0] >> 1);
 265                 //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.4379097269;
 266         #endif
 267
 268         /* Save this sampled bit in a delay line */
 269         af->sampled_bits <<= 1;
 270         af->sampled_bits |= (af->iir_y[1] > 0) ? 1 : 0;
 271
 272         /* Store current ADC sample in the af->delay_fifo */
 273         fifo_push(&af->delay_fifo, curr_sample);
 274
 275         /* If there is an edge, adjust phase sampling */
 276         if (EDGE_FOUND(af->sampled_bits))
 277         {
 278                 if (af->curr_phase < PHASE_THRES)
 279                         af->curr_phase += PHASE_INC;
 280                 else
 281                         af->curr_phase -= PHASE_INC;
 282         }
 283         af->curr_phase += PHASE_BIT;
 284
 285         /* sample the bit */
 286         if (af->curr_phase >= PHASE_MAX)
 287         {
 288                 af->curr_phase %= PHASE_MAX;
 289
 290                 /* Shift 1 position in the shift register of the found bits */
 291                 af->found_bits <<= 1;
 292
 293                 /*
 294                  * Determine bit value by reading the last 3 sampled bits.
 295                  * If the number of ones is two or greater, the bit value is a 1,
 296                  * otherwise is a 0.
 297                  * This algorithm presumes that there are 8 samples per bit.
 298                  */
 299                 STATIC_ASSERT(SAMPLEPERBIT == 8);
 300                 uint8_t bits = af->sampled_bits & 0x07;
 301                 if (bits == 0x07 // 111, 3 bits set to 1
 302                  || bits == 0x06 // 110, 2 bits
 303                  || bits == 0x05 // 101, 2 bits
 304                  || bits == 0x03 // 011, 2 bits
 305                 )
 306                         af->found_bits |= 1;
 307
 308                 /*
 309                  * NRZI coding: if 2 consecutive bits have the same value
 310                  * a 1 is received, otherwise it's a 0.
 311                  */
 312                 if (!hdlc_parse(&af->hdlc, !EDGE_FOUND(af->found_bits), &af->rx_fifo))
 313                         af->status |= AFSK_RXFIFO_OVERRUN;
 314         }
 315
 316
 317         AFSK_STROBE_OFF();
 318 }
 319
 320 static void afsk_txStart(Afsk *af)
 321 {
 322         if (!af->sending)
 323         {
 324                 af->phase_inc = MARK_INC;
 325                 af->phase_acc = 0;
 326                 af->stuff_cnt = 0;
 327                 af->sending = true;
 328                 af->preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
 329                 AFSK_DAC_IRQ_START(af->dac_ch);
 330         }
 331         ATOMIC(af->trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
 332 }
 333
 334 #define BIT_STUFF_LEN 5
 335
 336 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 337
 338 /**
 339  * DAC ISR callback.
 340  * This function has to be called by the DAC ISR when a sample of the configured
 341  * channel has been converted out.
 342  *
 343  * \param af Afsk context to operate on.
 344  *
 345  * \return The next DAC output sample.
 346  */
 347 uint8_t afsk_dac_isr(Afsk *af)
 348 {
 349         AFSK_STROBE_ON();
 350
 351         /* Check if we are at a start of a sample cycle */
 352         if (af->sample_count == 0)
 353         {
 354                 if (af->tx_bit == 0)
 355                 {
 356                         /* We have just finished transimitting a char, get a new one. */
 357                         if (fifo_isempty(&af->tx_fifo) && af->trailer_len == 0)
 358                         {
 359                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 360                                 af->sending = false;
 361                                 AFSK_STROBE_OFF();
 362                                 return 0;
 363                         }
 364                         else
 365                         {
 366                                 /*
 367                                  * If we have just finished sending an unstuffed byte,
 368                                  * reset bitstuff counter.
 369                                  */
 370                                 if (!af->bit_stuff)
 371                                         af->stuff_cnt = 0;
 372
 373                                 af->bit_stuff = true;
 374
 375                                 /*
 376                                  * Handle preamble and trailer
 377                                  */
 378                                 if (af->preamble_len == 0)
 379                                 {
 380                                         if (fifo_isempty(&af->tx_fifo))
 381                                         {
 382                                                 af->trailer_len--;
 383                                                 af->curr_out = HDLC_FLAG;
 384                                         }
 385                                         else
 386                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 387                                 }
 388                                 else
 389                                 {
 390                                         af->preamble_len--;
 391                                         af->curr_out = HDLC_FLAG;
 392                                 }
 393
 394                                 /* Handle char escape */
 395                                 if (af->curr_out == AX25_ESC)
 396                                 {
 397                                         if (fifo_isempty(&af->tx_fifo))
 398                                         {
 399                                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 400                                                 af->sending = false;
 401                                                 AFSK_STROBE_OFF();
 402                                                 return 0;
 403                                         }
 404                                         else
 405                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 406                                 }
 407                                 else if (af->curr_out == HDLC_FLAG || af->curr_out == HDLC_RESET)
 408                                         /* If these chars are not escaped disable bit stuffing */
 409                                         af->bit_stuff = false;
 410                         }
 411                         /* Start with LSB mask */
 412                         af->tx_bit = 0x01;
 413                 }
 414
 415                 /* check for bit stuffing */
 416                 if (af->bit_stuff && af->stuff_cnt >= BIT_STUFF_LEN)
 417                 {
 418                         /* If there are more than 5 ones in a row insert a 0 */
 419                         af->stuff_cnt = 0;
 420                         /* switch tone */
 421                         af->phase_inc = SWITCH_TONE(af->phase_inc);
 422                 }
 423                 else
 424                 {
 425                         /*
 426                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 427                          * unchanged; with a 0, there will be a change in the tone.
 428                          */
 429                         if (af->curr_out & af->tx_bit)
 430                         {
 431                                 /*
 432                                  * Transmit a 1:
 433                                  * - Stay on the previous tone
 434                                  * - Increase bit stuff counter
 435                                  */
 436                                 af->stuff_cnt++;
 437                         }
 438                         else
 439                         {
 440                                 /*
 441                                  * Transmit a 0:
 442                                  * - Reset bit stuff counter
 443                                  * - Switch tone
 444                                  */
 445                                 af->stuff_cnt = 0;
 446                                 af->phase_inc = SWITCH_TONE(af->phase_inc);
 447                         }
 448
 449                         /* Go to the next bit */
 450                         af->tx_bit <<= 1;
 451                 }
 452                 af->sample_count = DAC_SAMPLEPERBIT;
 453         }
 454
 455         /* Get new sample and put it out on the DAC */
 456         af->phase_acc += af->phase_inc;
 457         af->phase_acc %= SIN_LEN;
 458
 459         af->sample_count--;
 460         AFSK_STROBE_OFF();
 461         return sin_sample(af->phase_acc);
 462 }
 463
 464
 465 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
 466 {
 467         Afsk *af = AFSK_CAST(fd);
 468         uint8_t *buf = (uint8_t *)_buf;
 469
 470         #if CONFIG_AFSK_RXTIMEOUT == 0
 471         while (size-- && !fifo_isempty_locked(&af->rx_fifo))
 472         #else
 473         while (size--)
 474         #endif
 475         {
 476                 #if CONFIG_AFSK_RXTIMEOUT != -1
 477                 ticks_t start = timer_clock();
 478                 #endif
 479
 480                 while (fifo_isempty_locked(&af->rx_fifo));
 481                 {
 482                         cpu_relax();
 483                         #if CONFIG_AFSK_RXTIMEOUT != -1
 484                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 485                                 return buf - (uint8_t *)_buf;
 486                         #endif
 487                 }
 488
 489                 *buf++ = fifo_pop_locked(&af->rx_fifo);
 490         }
 491
 492         return buf - (uint8_t *)_buf;
 493 }
 494
 495 static size_t afsk_write(KFile *fd, const void *_buf, size_t size)
 496 {
 497         Afsk *af = AFSK_CAST(fd);
 498         const uint8_t *buf = (const uint8_t *)_buf;
 499
 500         while (size--)
 501         {
 502                 while (fifo_isfull_locked(&af->tx_fifo))
 503                         cpu_relax();
 504
 505                 fifo_push_locked(&af->tx_fifo, *buf++);
 506                 afsk_txStart(af);
 507         }
 508
 509         return buf - (const uint8_t *)_buf;
 510 }
 511
 512 static int afsk_flush(KFile *fd)
 513 {
 514         Afsk *af = AFSK_CAST(fd);
 515         while (af->sending)
 516                 cpu_relax();
 517         return 0;
 518 }
 519
 520 static int afsk_error(KFile *fd)
 521 {
 522         Afsk *af = AFSK_CAST(fd);
 523         int err;
 524
 525         ATOMIC(err = af->status);
 526         return err;
 527 }
 528
 529 static void afsk_clearerr(KFile *fd)
 530 {
 531         Afsk *af = AFSK_CAST(fd);
 532         ATOMIC(af->status = 0);
 533 }
 534
 535
 536 /**
 537  * Initialize an AFSK1200 modem.
 538  * \param af Afsk context to operate on.
 539  * \param adc_ch  ADC channel used by the demodulator.
 540  * \param dac_ch  DAC channel used by the modulator.
 541  */
 542 void afsk_init(Afsk *af, int adc_ch, int dac_ch)
 543 {
 544         #if CONFIG_AFSK_RXTIMEOUT != -1
 545         MOD_CHECK(timer);
 546         #endif
 547         memset(af, 0, sizeof(*af));
 548         af->adc_ch = adc_ch;
 549         af->dac_ch = dac_ch;
 550
 551         fifo_init(&af->delay_fifo, (uint8_t *)af->delay_buf, sizeof(af->delay_buf));
 552         fifo_init(&af->rx_fifo, af->rx_buf, sizeof(af->rx_buf));
 553
 554         /* Fill sample FIFO with 0 */
 555         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 556                 fifo_push(&af->delay_fifo, 0);
 557
 558         fifo_init(&af->tx_fifo, af->tx_buf, sizeof(af->tx_buf));
 559
 560         AFSK_ADC_INIT(adc_ch, af);
 561         AFSK_DAC_INIT(dac_ch, af);
 562         AFSK_STROBE_INIT();
 563         LOG_INFO("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 564
 565         DB(af->fd._type = KFT_AFSK);
 566         af->fd.write = afsk_write;
 567         af->fd.read = afsk_read;
 568         af->fd.flush = afsk_flush;
 569         af->fd.error = afsk_error;
 570         af->fd.clearerr = afsk_clearerr;
 571         af->phase_inc = MARK_INC;
 572 }