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