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_locked(&rx_fifo))
 175                 {
 176                         fifo_push(&rx_fifo, HDLC_FLAG);
 177                         hdlc_rxstart = true;
 178                 }
 179                 else
 180                         hdlc_rxstart = false;
 181                 hdlc_currchar = 0;
 182                 hdlc_bit_idx = 0;
 183                 return;
 184         }
 185
 186         /* Reset */
 187         if ((demod_bits & HDLC_RESET) == HDLC_RESET)
 188         {
 189                 hdlc_rxstart = false;
 190                 return;
 191         }
 192
 193         if (!hdlc_rxstart)
 194                 return;
 195
 196         /* Stuffed bit */
 197         if ((demod_bits & 0x3f) == 0x3e)
 198                 return;
 199
 200         if (demod_bits & 0x01)
 201                 hdlc_currchar |= 0x80;
 202
 203         if (++hdlc_bit_idx >= 8)
 204         {
 205                 if ((hdlc_currchar == HDLC_FLAG
 206                         || hdlc_currchar == HDLC_RESET
 207                         || hdlc_currchar == AX25_ESC)
 208                         && !fifo_isfull_locked(&rx_fifo))
 209                         fifo_push(&rx_fifo, AX25_ESC);
 210                 else
 211                         hdlc_rxstart = false;
 212
 213                 if (!fifo_isfull_locked(&rx_fifo))
 214                         fifo_push(&rx_fifo, hdlc_currchar);
 215                 else
 216                         hdlc_rxstart = false;
 217
 218                 hdlc_currchar = 0;
 219                 hdlc_bit_idx = 0;
 220                 return;
 221         }
 222
 223         hdlc_currchar >>= 1;
 224 }
 225
 226 DEFINE_AFSK_ADC_ISR()
 227 {
 228         AFSK_STROBE_ON();
 229         int8_t curr_sample = AFSK_READ_ADC();
 230
 231         /*
 232          * Frequency discriminator and LP IIR filter.
 233          * This filter is designed to work
 234          * at the given sample rate and bit rate.
 235          */
 236         STATIC_ASSERT(SAMPLERATE == 9600);
 237         STATIC_ASSERT(BITRATE == 1200);
 238
 239         /*
 240          * Frequency discrimination is achieved by simply multiplying
 241          * the sample with a delayed sample of (bits per sample) / 2.
 242          * Then the signal is lowpass filtered with a first order, 600 Hz
 243          * Butterworth filter.
 244          */
 245
 246         iir_x[0] = iir_x[1];
 247         iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
 248         iir_y[0] = iir_y[1];
 249
 250         /*
 251          * This strange sum + shift is an optimization for 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         iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
 258
 259         /* Save this sampled bit in a delay line */
 260         sampled_bits <<= 1;
 261         sampled_bits |= (iir_y[1] > 0) ? 1 : 0;
 262
 263         /* Store current ADC sample in the delay_fifo */
 264         fifo_push(&delay_fifo, curr_sample);
 265
 266         /* If there is an edge, adjust phase sampling */
 267         if (EDGE_FOUND(sampled_bits))
 268         {
 269                 if (curr_phase < PHASE_THRES)
 270                         curr_phase += PHASE_INC;
 271                 else
 272                         curr_phase -= PHASE_INC;
 273         }
 274         curr_phase += PHASE_BIT;
 275
 276         /* sample the bit */
 277         if (curr_phase >= PHASE_MAX)
 278         {
 279                 curr_phase %= PHASE_MAX;
 280
 281                 /* Shift 1 position in the shift register of the found bits */
 282                 found_bits <<= 1;
 283
 284                 /*
 285                  * TODO: maybe a better algorithm to find the sample bit
 286                  * other than reading the last one.
 287                  */
 288                 found_bits |= sampled_bits & 1;
 289
 290                 /*
 291                  * NRZI coding: if 2 consecutive bits have the same value
 292                  * a 1 is received, otherwise it's a 0.
 293                  */
 294                 hdlc_parse(!EDGE_FOUND(found_bits));
 295         }
 296
 297
 298         AFSK_STROBE_OFF();
 299         AFSK_ADC_IRQ_END();
 300 }
 301
 302 /* True while modem sends data */
 303 static volatile bool sending;
 304
 305 static void afsk_txStart(void)
 306 {
 307         if (!sending)
 308         {
 309                 phase_inc = MARK_INC;
 310                 phase_acc = 0;
 311                 stuff_cnt = 0;
 312                 sending = true;
 313                 AFSK_DAC_IRQ_START();
 314         }
 315 }
 316
 317 #define BIT_STUFF_LEN 5
 318
 319 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 320
 321 DEFINE_AFSK_DAC_ISR()
 322 {
 323         /* Check if we are at a start of a sample cycle */
 324         if (sample_count == 0)
 325         {
 326                 if (tx_bit == 0)
 327                 {
 328                         /* We have just finished transimitting a char, get a new one. */
 329                         if (fifo_isempty(&tx_fifo))
 330                         {
 331                                 AFSK_DAC_IRQ_STOP();
 332                                 sending = false;
 333                                 AFSK_DAC_IRQ_END();
 334                                 return;
 335                         }
 336                         else
 337                         {
 338                                 /*
 339                                  * If we have just finished sending an unstuffed byte,
 340                                  * reset bitstuff counter.
 341                                  */
 342                                 if (!bit_stuff)
 343                                         stuff_cnt = 0;
 344
 345                                 bit_stuff = true;
 346                                 curr_out = fifo_pop(&tx_fifo);
 347
 348                                 /* Handle char escape */
 349                                 if (curr_out == AX25_ESC)
 350                                 {
 351                                         if (fifo_isempty(&tx_fifo))
 352                                         {
 353                                                 AFSK_DAC_IRQ_STOP();
 354                                                 sending = false;
 355                                                 AFSK_DAC_IRQ_END();
 356                                                 return;
 357                                         }
 358                                         else
 359                                                 curr_out = fifo_pop(&tx_fifo);
 360                                 }
 361                                 else if (curr_out == HDLC_FLAG || curr_out == HDLC_RESET)
 362                                         /* If these chars are not escaped disable bit stuffing */
 363                                         bit_stuff = false;
 364                         }
 365                         /* Start with LSB mask */
 366                         tx_bit = 0x01;
 367                 }
 368
 369                 /* check for bit stuffing */
 370                 if (bit_stuff && stuff_cnt >= BIT_STUFF_LEN)
 371                 {
 372                         /* If there are more than 5 ones in a row insert a 0 */
 373                         stuff_cnt = 0;
 374                         /* switch tone */
 375                         phase_inc = SWITCH_TONE(phase_inc);
 376                 }
 377                 else
 378                 {
 379                         /*
 380                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 381                          * unchanged; with a 0, there will be a change in the tone.
 382                          */
 383                         if (curr_out & tx_bit)
 384                         {
 385                                 /*
 386                                  * Transmit a 1:
 387                                  * - Stay on the previous tone
 388                                  * - Increace bit stuff count
 389                                  */
 390                                 stuff_cnt++;
 391                         }
 392                         else
 393                         {
 394                                 /*
 395                                  * Transmit a 0:
 396                                  * - Reset bit stuff count
 397                                  * - Switch tone
 398                                  */
 399                                 stuff_cnt = 0;
 400                                 phase_inc = SWITCH_TONE(phase_inc);
 401                         }
 402
 403                         /* Go to the next bit */
 404                         tx_bit <<= 1;
 405                 }
 406                 sample_count = DAC_SAMPLEPERBIT;
 407         }
 408
 409         /* Get new sample and put it out on the DAC */
 410         phase_acc += phase_inc;
 411         phase_acc %= SIN_LEN;
 412
 413         AFSK_SET_DAC(sin_sample(phase_acc));
 414         sample_count--;
 415         AFSK_DAC_IRQ_END();
 416 }
 417
 418
 419 static size_t afsk_read(UNUSED_ARG(KFile *, fd), void *_buf, size_t size)
 420 {
 421         uint8_t *buf = (uint8_t *)_buf;
 422
 423         #if CONFIG_AFSK_RXTIMEOUT == 0
 424         while (size-- && !fifo_isempty_locked(&rx_fifo))
 425         #else
 426         while (size--)
 427         #endif
 428         {
 429                 #if CONFIG_AFSK_RXTIMEOUT != -1
 430                 ticks_t start = timer_clock();
 431                 #endif
 432
 433                 do
 434                 {
 435                         cpu_relax();
 436                         #if CONFIG_AFSK_RXTIMEOUT != -1
 437                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 438                                 return buf - (uint8_t *)_buf;
 439                         #endif
 440                 }
 441                 while (fifo_isempty_locked(&rx_fifo));
 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 }