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