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                 {
 210                         if (!fifo_isfull(&rx_fifo))
 211                                 fifo_push(&rx_fifo, AX25_ESC);
 212                         else
 213                                 hdlc_rxstart = false;
 214                 }
 215
 216                 if (!fifo_isfull(&rx_fifo))
 217                         fifo_push(&rx_fifo, hdlc_currchar);
 218                 else
 219                         hdlc_rxstart = false;
 220
 221                 hdlc_currchar = 0;
 222                 hdlc_bit_idx = 0;
 223                 return;
 224         }
 225
 226         hdlc_currchar >>= 1;
 227 }
 228
 229 DEFINE_AFSK_ADC_ISR()
 230 {
 231         AFSK_STROBE_ON();
 232         int8_t curr_sample = AFSK_READ_ADC();
 233
 234         /*
 235          * Frequency discriminator and LP IIR filter.
 236          * This filter is designed to work
 237          * at the given sample rate and bit rate.
 238          */
 239         STATIC_ASSERT(SAMPLERATE == 9600);
 240         STATIC_ASSERT(BITRATE == 1200);
 241
 242         /*
 243          * Frequency discrimination is achieved by simply multiplying
 244          * the sample with a delayed sample of (bits per sample) / 2.
 245          * Then the signal is lowpass filtered with a first order, 600 Hz
 246          * Butterworth filter.
 247          */
 248
 249         iir_x[0] = iir_x[1];
 250         iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
 251         iir_y[0] = iir_y[1];
 252
 253         /*
 254          * This strange sum + shift is an optimization for iir_y[0] * 0.668.
 255          * iir * 0.668 ~= (iir * 21) / 32 =
 256          * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
 257          * = iir / 2 + iir / 8 + iir / 32 =
 258          * = iir >> 1 + iir >> 3 + iir >> 5
 259          */
 260         iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
 261
 262         /* Save this sampled bit in a delay line */
 263         sampled_bits <<= 1;
 264         sampled_bits |= (iir_y[1] > 0) ? 1 : 0;
 265
 266         /* Store current ADC sample in the delay_fifo */
 267         fifo_push(&delay_fifo, curr_sample);
 268
 269         /* If there is an edge, adjust phase sampling */
 270         if (EDGE_FOUND(sampled_bits))
 271         {
 272                 if (curr_phase < PHASE_THRES)
 273                         curr_phase += PHASE_INC;
 274                 else
 275                         curr_phase -= PHASE_INC;
 276         }
 277         curr_phase += PHASE_BIT;
 278
 279         /* sample the bit */
 280         if (curr_phase >= PHASE_MAX)
 281         {
 282                 curr_phase %= PHASE_MAX;
 283
 284                 /* Shift 1 position in the shift register of the found bits */
 285                 found_bits <<= 1;
 286
 287                 /*
 288                  * TODO: maybe a better algorithm to find the sample bit
 289                  * other than reading the last one.
 290                  */
 291                 found_bits |= sampled_bits & 1;
 292
 293                 /*
 294                  * NRZI coding: if 2 consecutive bits have the same value
 295                  * a 1 is received, otherwise it's a 0.
 296                  */
 297                 hdlc_parse(!EDGE_FOUND(found_bits));
 298         }
 299
 300
 301         AFSK_STROBE_OFF();
 302         AFSK_ADC_IRQ_END();
 303 }
 304
 305 /* True while modem sends data */
 306 static volatile bool sending;
 307
 308 static void afsk_txStart(void)
 309 {
 310         if (!sending)
 311         {
 312                 phase_inc = MARK_INC;
 313                 phase_acc = 0;
 314                 stuff_cnt = 0;
 315                 sending = true;
 316                 AFSK_DAC_IRQ_START();
 317         }
 318 }
 319
 320 #define BIT_STUFF_LEN 5
 321
 322 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 323
 324 DEFINE_AFSK_DAC_ISR()
 325 {
 326         /* Check if we are at a start of a sample cycle */
 327         if (sample_count == 0)
 328         {
 329                 if (tx_bit == 0)
 330                 {
 331                         /* We have just finished transimitting a char, get a new one. */
 332                         if (fifo_isempty(&tx_fifo))
 333                         {
 334                                 AFSK_DAC_IRQ_STOP();
 335                                 sending = false;
 336                                 AFSK_DAC_IRQ_END();
 337                                 return;
 338                         }
 339                         else
 340                         {
 341                                 /*
 342                                  * If we have just finished sending an unstuffed byte,
 343                                  * reset bitstuff counter.
 344                                  */
 345                                 if (!bit_stuff)
 346                                         stuff_cnt = 0;
 347
 348                                 bit_stuff = true;
 349                                 curr_out = fifo_pop(&tx_fifo);
 350
 351                                 /* Handle char escape */
 352                                 if (curr_out == AX25_ESC)
 353                                 {
 354                                         if (fifo_isempty(&tx_fifo))
 355                                         {
 356                                                 AFSK_DAC_IRQ_STOP();
 357                                                 sending = false;
 358                                                 AFSK_DAC_IRQ_END();
 359                                                 return;
 360                                         }
 361                                         else
 362                                                 curr_out = fifo_pop(&tx_fifo);
 363                                 }
 364                                 else if (curr_out == HDLC_FLAG || curr_out == HDLC_RESET)
 365                                         /* If these chars are not escaped disable bit stuffing */
 366                                         bit_stuff = false;
 367                         }
 368                         /* Start with LSB mask */
 369                         tx_bit = 0x01;
 370                 }
 371
 372                 /* check for bit stuffing */
 373                 if (bit_stuff && stuff_cnt >= BIT_STUFF_LEN)
 374                 {
 375                         /* If there are more than 5 ones in a row insert a 0 */
 376                         stuff_cnt = 0;
 377                         /* switch tone */
 378                         phase_inc = SWITCH_TONE(phase_inc);
 379                 }
 380                 else
 381                 {
 382                         /*
 383                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 384                          * unchanged; with a 0, there will be a change in the tone.
 385                          */
 386                         if (curr_out & tx_bit)
 387                         {
 388                                 /*
 389                                  * Transmit a 1:
 390                                  * - Stay on the previous tone
 391                                  * - Increace bit stuff count
 392                                  */
 393                                 stuff_cnt++;
 394                         }
 395                         else
 396                         {
 397                                 /*
 398                                  * Transmit a 0:
 399                                  * - Reset bit stuff count
 400                                  * - Switch tone
 401                                  */
 402                                 stuff_cnt = 0;
 403                                 phase_inc = SWITCH_TONE(phase_inc);
 404                         }
 405
 406                         /* Go to the next bit */
 407                         tx_bit <<= 1;
 408                 }
 409                 sample_count = DAC_SAMPLEPERBIT;
 410         }
 411
 412         /* Get new sample and put it out on the DAC */
 413         phase_acc += phase_inc;
 414         phase_acc %= SIN_LEN;
 415
 416         AFSK_SET_DAC(sin_sample(phase_acc));
 417         sample_count--;
 418         AFSK_DAC_IRQ_END();
 419 }
 420
 421
 422 static size_t afsk_read(UNUSED_ARG(KFile *, fd), void *_buf, size_t size)
 423 {
 424         uint8_t *buf = (uint8_t *)_buf;
 425
 426         #if CONFIG_AFSK_RXTIMEOUT == 0
 427         while (size-- && !fifo_isempty_locked(&rx_fifo))
 428         #else
 429         while (size--)
 430         #endif
 431         {
 432                 #if CONFIG_AFSK_RXTIMEOUT != -1
 433                 ticks_t start = timer_clock();
 434                 #endif
 435
 436                 while (fifo_isempty_locked(&rx_fifo));
 437                 {
 438                         cpu_relax();
 439                         #if CONFIG_AFSK_RXTIMEOUT != -1
 440                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 441                                 return buf - (uint8_t *)_buf;
 442                         #endif
 443                 }
 444
 445                 *buf++ = fifo_pop_locked(&rx_fifo);
 446         }
 447
 448         return buf - (uint8_t *)_buf;
 449 }
 450
 451 static size_t afsk_write(UNUSED_ARG(KFile *, fd), const void *_buf, size_t size)
 452 {
 453         const uint8_t *buf = (const uint8_t *)_buf;
 454
 455         while (size--)
 456         {
 457                 while (fifo_isfull_locked(&tx_fifo))
 458                         cpu_relax();
 459
 460                 fifo_push_locked(&tx_fifo, *buf++);
 461                 afsk_txStart();
 462         }
 463
 464         return buf - (const uint8_t *)_buf;
 465 }
 466
 467 static int afsk_flush(UNUSED_ARG(KFile *, fd))
 468 {
 469         while (sending)
 470                 cpu_relax();
 471         return 0;
 472 }
 473
 474
 475 void afsk_init(Afsk *af)
 476 {
 477         #if CONFIG_AFSK_RXTIMEOUT != -1
 478         MOD_CHECK(timer);
 479         #endif
 480
 481         fifo_init(&delay_fifo, (uint8_t *)delay_buf, sizeof(delay_buf));
 482         fifo_init(&rx_fifo, rx_buf, sizeof(rx_buf));
 483
 484         /* Fill sample FIFO with 0 */
 485         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 486                 fifo_push(&delay_fifo, 0);
 487
 488         fifo_init(&tx_fifo, tx_buf, sizeof(tx_buf));
 489
 490         AFSK_ADC_INIT();
 491         AFSK_STROBE_INIT();
 492         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 493
 494         memset(af, 0, sizeof(*af));
 495         DB(af->fd._type = KFT_AFSK);
 496         af->fd.write = afsk_write;
 497         af->fd.read = afsk_read;
 498         af->fd.flush = afsk_flush;
 499 }