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 // Demodulator constants
  55 #define SAMPLERATE 9600
  56 #define BITRATE    1200
  57
  58 #define SAMPLEPERBIT (SAMPLERATE / BITRATE)
  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 /** Current sample of bit for output data. */
  79 static uint8_t sample_count;
  80
  81 /** Current character to be modulated */
  82 static uint8_t curr_out;
  83
  84 /** Mask of current modulated bit */
  85 static uint8_t tx_bit;
  86
  87 /** True if bit stuff is allowed, false otherwise */
  88 static bool bit_stuff;
  89
  90 /** Counter for bit stuffing */
  91 static uint8_t stuff_cnt;
  92
  93 /**
  94  * Sine table for the first quarter of wave.
  95  * The rest of the wave is computed from this first quarter.
  96  * This table is used to generate the modulated data.
  97  */
  98 static const uint8_t sin_table[] =
  99 {
 100         //TODO put in flash!
 101         128, 129, 131, 132, 134, 135, 137, 138, 140, 142, 143, 145, 146, 148, 149, 151,
 102         152, 154, 155, 157, 158, 160, 162, 163, 165, 166, 167, 169, 170, 172, 173, 175,
 103         176, 178, 179, 181, 182, 183, 185, 186, 188, 189, 190, 192, 193, 194, 196, 197,
 104         198, 200, 201, 202, 203, 205, 206, 207, 208, 210, 211, 212, 213, 214, 215, 217,
 105         218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233,
 106         234, 234, 235, 236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245,
 107         245, 246, 246, 247, 248, 248, 249, 249, 250, 250, 250, 251, 251, 252, 252, 252,
 108         253, 253, 253, 253, 254, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255,
 109 };
 110
 111 #define SIN_LEN 512 ///< Full wave length
 112
 113 STATIC_ASSERT(sizeof(sin_table) == SIN_LEN / 4);
 114
 115 /**
 116  * DDS phase accumulator for generating modulated data.
 117  */
 118 static uint16_t phase_acc;
 119
 120 /** Current phase increment for current modulated bit */
 121 static uint16_t phase_inc = MARK_INC;
 122
 123
 124 /**
 125  * Given the index, this function computes the correct sine sample
 126  * based only on the first quarter of wave.
 127  */
 128 INLINE uint8_t sin_sample(uint16_t idx)
 129 {
 130         ASSERT(idx < SIN_LEN);
 131         uint16_t new_idx = idx % (SIN_LEN / 2);
 132         new_idx = (new_idx >= (SIN_LEN / 4)) ? (SIN_LEN / 2 - new_idx - 1) : new_idx;
 133         return (idx >= (SIN_LEN / 2)) ? (255 - sin_table[new_idx]) : sin_table[new_idx];
 134 }
 135
 136
 137 static FIFOBuffer delay_fifo;
 138
 139 /**
 140  * Buffer for delay FIFO.
 141  * The 1 is added because the FIFO macros need
 142  * 1 byte more to handle a buffer (SAMPLEPERBIT / 2) bytes long.
 143  */
 144 static int8_t delay_buf[SAMPLEPERBIT / 2 + 1];
 145
 146 static FIFOBuffer rx_fifo;
 147 static uint8_t rx_buf[CONFIG_AFSK_RX_BUFLEN];
 148
 149 static FIFOBuffer tx_fifo;
 150 static uint8_t tx_buf[CONFIG_AFSK_TX_BUFLEN];
 151
 152 static int16_t iir_x[2];
 153 static int16_t iir_y[2];
 154 static uint8_t sampled_bits;
 155 static uint8_t found_bits;
 156 static uint8_t demod_bits;
 157 static int8_t curr_phase;
 158
 159 static bool hdlc_rxstart;
 160 static uint8_t hdlc_currchar;
 161 static uint8_t hdlc_bit_idx;
 162
 163 #define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
 164 #define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)
 165
 166 static void hdlc_parse(bool bit)
 167 {
 168         demod_bits <<= 1;
 169         demod_bits |= bit ? 1 : 0;
 170
 171         /* HDLC Flag */
 172         if (demod_bits == HDLC_FLAG)
 173         {
 174                 if (!fifo_isfull(&rx_fifo))
 175                 {
 176                         fifo_push(&rx_fifo, HDLC_FLAG);
 177                         hdlc_rxstart = true;
 178                 }
 179                 else
 180                         hdlc_rxstart = false;
 181
 182                 hdlc_currchar = 0;
 183                 hdlc_bit_idx = 0;
 184                 return;
 185         }
 186
 187         /* Reset */
 188         if ((demod_bits & HDLC_RESET) == HDLC_RESET)
 189         {
 190                 hdlc_rxstart = false;
 191                 return;
 192         }
 193
 194         if (!hdlc_rxstart)
 195                 return;
 196
 197         /* Stuffed bit */
 198         if ((demod_bits & 0x3f) == 0x3e)
 199                 return;
 200
 201         if (demod_bits & 0x01)
 202                 hdlc_currchar |= 0x80;
 203
 204         if (++hdlc_bit_idx >= 8)
 205         {
 206                 if ((hdlc_currchar == HDLC_FLAG
 207                         || hdlc_currchar == HDLC_RESET
 208                         || hdlc_currchar == AX25_ESC)
 209                         && !fifo_isfull(&rx_fifo))
 210                         fifo_push(&rx_fifo, AX25_ESC);
 211                 else
 212                         hdlc_rxstart = false;
 213
 214                 if (!fifo_isfull(&rx_fifo))
 215                         fifo_push(&rx_fifo, hdlc_currchar);
 216                 else
 217                         hdlc_rxstart = false;
 218
 219                 hdlc_currchar = 0;
 220                 hdlc_bit_idx = 0;
 221                 return;
 222         }
 223
 224         hdlc_currchar >>= 1;
 225 }
 226
 227 DEFINE_AFSK_ADC_ISR()
 228 {
 229         AFSK_STROBE_ON();
 230         int8_t curr_sample = AFSK_READ_ADC();
 231
 232         /*
 233          * Frequency discriminator and LP IIR filter.
 234          * This filter is designed to work
 235          * at the given sample rate and bit rate.
 236          */
 237         STATIC_ASSERT(SAMPLERATE == 9600);
 238         STATIC_ASSERT(BITRATE == 1200);
 239
 240         /*
 241          * Frequency discrimination is achieved by simply multiplying
 242          * the sample with a delayed sample of (bits per sample) / 2.
 243          * Then the signal is lowpass filtered with a first order, 600 Hz
 244          * Butterworth filter.
 245          */
 246
 247         iir_x[0] = iir_x[1];
 248         iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
 249         iir_y[0] = iir_y[1];
 250
 251         /*
 252          * This strange sum + shift is an optimization for iir_y[0] * 0.668.
 253          * iir * 0.668 ~= (iir * 21) / 32 =
 254          * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
 255          * = iir / 2 + iir / 8 + iir / 32 =
 256          * = iir >> 1 + iir >> 3 + iir >> 5
 257          */
 258         iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
 259
 260         /* Save this sampled bit in a delay line */
 261         sampled_bits <<= 1;
 262         sampled_bits |= (iir_y[1] > 0) ? 1 : 0;
 263
 264         /* Store current ADC sample in the delay_fifo */
 265         fifo_push(&delay_fifo, curr_sample);
 266
 267         /* If there is an edge, adjust phase sampling */
 268         if (EDGE_FOUND(sampled_bits))
 269         {
 270                 if (curr_phase < PHASE_THRES)
 271                         curr_phase += PHASE_INC;
 272                 else
 273                         curr_phase -= PHASE_INC;
 274         }
 275         curr_phase += PHASE_BIT;
 276
 277         /* sample the bit */
 278         if (curr_phase >= PHASE_MAX)
 279         {
 280                 curr_phase %= PHASE_MAX;
 281
 282                 /* Shift 1 position in the shift register of the found bits */
 283                 found_bits <<= 1;
 284
 285                 /*
 286                  * TODO: maybe a better algorithm to find the sample bit
 287                  * other than reading the last one.
 288                  */
 289                 found_bits |= sampled_bits & 1;
 290
 291                 /*
 292                  * NRZI coding: if 2 consecutive bits have the same value
 293                  * a 1 is received, otherwise it's a 0.
 294                  */
 295                 hdlc_parse(!EDGE_FOUND(found_bits));
 296         }
 297
 298
 299         AFSK_STROBE_OFF();
 300         AFSK_ADC_IRQ_END();
 301 }
 302
 303 /* True while modem sends data */
 304 static volatile bool sending;
 305
 306 static void afsk_txStart(void)
 307 {
 308         if (!sending)
 309         {
 310                 phase_inc = MARK_INC;
 311                 phase_acc = 0;
 312                 stuff_cnt = 0;
 313                 sending = true;
 314                 AFSK_DAC_IRQ_START();
 315         }
 316 }
 317
 318 #define BIT_STUFF_LEN 5
 319
 320 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 321
 322 DEFINE_AFSK_DAC_ISR()
 323 {
 324         /* Check if we are at a start of a sample cycle */
 325         if (sample_count == 0)
 326         {
 327                 if (tx_bit == 0)
 328                 {
 329                         /* We have just finished transimitting a char, get a new one. */
 330                         if (fifo_isempty(&tx_fifo))
 331                         {
 332                                 AFSK_DAC_IRQ_STOP();
 333                                 sending = false;
 334                                 AFSK_DAC_IRQ_END();
 335                                 return;
 336                         }
 337                         else
 338                         {
 339                                 /*
 340                                  * If we have just finished sending an unstuffed byte,
 341                                  * reset bitstuff counter.
 342                                  */
 343                                 if (!bit_stuff)
 344                                         stuff_cnt = 0;
 345
 346                                 bit_stuff = true;
 347                                 curr_out = fifo_pop(&tx_fifo);
 348
 349                                 /* Handle char escape */
 350                                 if (curr_out == AX25_ESC)
 351                                 {
 352                                         if (fifo_isempty(&tx_fifo))
 353                                         {
 354                                                 AFSK_DAC_IRQ_STOP();
 355                                                 sending = false;
 356                                                 AFSK_DAC_IRQ_END();
 357                                                 return;
 358                                         }
 359                                         else
 360                                                 curr_out = fifo_pop(&tx_fifo);
 361                                 }
 362                                 else if (curr_out == HDLC_FLAG || curr_out == HDLC_RESET)
 363                                         /* If these chars are not escaped disable bit stuffing */
 364                                         bit_stuff = false;
 365                         }
 366                         /* Start with LSB mask */
 367                         tx_bit = 0x01;
 368                 }
 369
 370                 /* check for bit stuffing */
 371                 if (bit_stuff && stuff_cnt >= BIT_STUFF_LEN)
 372                 {
 373                         /* If there are more than 5 ones in a row insert a 0 */
 374                         stuff_cnt = 0;
 375                         /* switch tone */
 376                         phase_inc = SWITCH_TONE(phase_inc);
 377                 }
 378                 else
 379                 {
 380                         /*
 381                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 382                          * unchanged; with a 0, there will be a change in the tone.
 383                          */
 384                         if (curr_out & tx_bit)
 385                         {
 386                                 /*
 387                                  * Transmit a 1:
 388                                  * - Stay on the previous tone
 389                                  * - Increace bit stuff count
 390                                  */
 391                                 stuff_cnt++;
 392                         }
 393                         else
 394                         {
 395                                 /*
 396                                  * Transmit a 0:
 397                                  * - Reset bit stuff count
 398                                  * - Switch tone
 399                                  */
 400                                 stuff_cnt = 0;
 401                                 phase_inc = SWITCH_TONE(phase_inc);
 402                         }
 403
 404                         /* Go to the next bit */
 405                         tx_bit <<= 1;
 406                 }
 407                 sample_count = DAC_SAMPLEPERBIT;
 408         }
 409
 410         /* Get new sample and put it out on the DAC */
 411         phase_acc += phase_inc;
 412         phase_acc %= SIN_LEN;
 413
 414         AFSK_SET_DAC(sin_sample(phase_acc));
 415         sample_count--;
 416         AFSK_DAC_IRQ_END();
 417 }
 418
 419
 420 static size_t afsk_read(UNUSED_ARG(KFile *, fd), void *_buf, size_t size)
 421 {
 422         uint8_t *buf = (uint8_t *)_buf;
 423
 424         #if CONFIG_AFSK_RXTIMEOUT == 0
 425         while (size-- && !fifo_isempty_locked(&rx_fifo))
 426         #else
 427         while (size--)
 428         #endif
 429         {
 430                 #if CONFIG_AFSK_RXTIMEOUT != -1
 431                 ticks_t start = timer_clock();
 432                 #endif
 433
 434                 while (fifo_isempty_locked(&rx_fifo));
 435                 {
 436                         cpu_relax();
 437                         #if CONFIG_AFSK_RXTIMEOUT != -1
 438                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 439                                 return buf - (uint8_t *)_buf;
 440                         #endif
 441                 }
 442
 443                 *buf++ = fifo_pop_locked(&rx_fifo);
 444         }
 445
 446         return buf - (uint8_t *)_buf;
 447 }
 448
 449 static size_t afsk_write(UNUSED_ARG(KFile *, fd), const void *_buf, size_t size)
 450 {
 451         const uint8_t *buf = (const uint8_t *)_buf;
 452
 453         while (size--)
 454         {
 455                 while (fifo_isfull_locked(&tx_fifo))
 456                         cpu_relax();
 457
 458                 fifo_push_locked(&tx_fifo, *buf++);
 459                 afsk_txStart();
 460         }
 461
 462         return buf - (const uint8_t *)_buf;
 463 }
 464
 465 static int afsk_flush(UNUSED_ARG(KFile *, fd))
 466 {
 467         while (sending)
 468                 cpu_relax();
 469         return 0;
 470 }
 471
 472
 473 void afsk_init(Afsk *af)
 474 {
 475         #if CONFIG_AFSK_RXTIMEOUT != -1
 476         MOD_CHECK(timer);
 477         #endif
 478
 479         fifo_init(&delay_fifo, (uint8_t *)delay_buf, sizeof(delay_buf));
 480         fifo_init(&rx_fifo, rx_buf, sizeof(rx_buf));
 481
 482         /* Fill sample FIFO with 0 */
 483         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 484                 fifo_push(&delay_fifo, 0);
 485
 486         fifo_init(&tx_fifo, tx_buf, sizeof(tx_buf));
 487
 488         AFSK_ADC_INIT();
 489         AFSK_STROBE_INIT();
 490         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 491
 492         memset(af, 0, sizeof(*af));
 493         DB(af->fd._type = KFT_AFSK);
 494         af->fd.write = afsk_write;
 495         af->fd.read = afsk_read;
 496         af->fd.flush = afsk_flush;
 497 }