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(Afsk *af, bool bit)
 113 {
 114         af->hdlc_demod_bits <<= 1;
 115         af->hdlc_demod_bits |= bit ? 1 : 0;
 116
 117         /* HDLC Flag */
 118         if (af->hdlc_demod_bits == HDLC_FLAG)
 119         {
 120                 if (!fifo_isfull(&af->rx_fifo))
 121                 {
 122                         fifo_push(&af->rx_fifo, HDLC_FLAG);
 123                         af->hdlc_rxstart = true;
 124                 }
 125                 else
 126                         af->hdlc_rxstart = false;
 127
 128                 af->hdlc_currchar = 0;
 129                 af->hdlc_bit_idx = 0;
 130                 return;
 131         }
 132
 133         /* Reset */
 134         if ((af->hdlc_demod_bits & HDLC_RESET) == HDLC_RESET)
 135         {
 136                 af->hdlc_rxstart = false;
 137                 return;
 138         }
 139
 140         if (!af->hdlc_rxstart)
 141                 return;
 142
 143         /* Stuffed bit */
 144         if ((af->hdlc_demod_bits & 0x3f) == 0x3e)
 145                 return;
 146
 147         if (af->hdlc_demod_bits & 0x01)
 148                 af->hdlc_currchar |= 0x80;
 149
 150         if (++af->hdlc_bit_idx >= 8)
 151         {
 152                 if ((af->hdlc_currchar == HDLC_FLAG
 153                         || af->hdlc_currchar == HDLC_RESET
 154                         || af->hdlc_currchar == AX25_ESC))
 155                 {
 156                         if (!fifo_isfull(&af->rx_fifo))
 157                                 fifo_push(&af->rx_fifo, AX25_ESC);
 158                         else
 159                                 af->hdlc_rxstart = false;
 160                 }
 161
 162                 if (!fifo_isfull(&af->rx_fifo))
 163                         fifo_push(&af->rx_fifo, af->hdlc_currchar);
 164                 else
 165                         af->hdlc_rxstart = false;
 166
 167                 af->hdlc_currchar = 0;
 168                 af->hdlc_bit_idx = 0;
 169                 return;
 170         }
 171
 172         af->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                  */
 258                 uint8_t bits = af->sampled_bits & 0x07;
 259                 if (bits == 0x07 // 111, 3 bits set to 1
 260                  || bits == 0x06 // 110, 2 bits
 261                  || bits == 0x05 // 101, 2 bits
 262                  || bits == 0x03 // 011, 2 bits
 263                 )
 264                         af->found_bits |= 1;
 265
 266                 /*
 267                  * NRZI coding: if 2 consecutive bits have the same value
 268                  * a 1 is received, otherwise it's a 0.
 269                  */
 270                 hdlc_parse(af, !EDGE_FOUND(af->found_bits));
 271         }
 272
 273
 274         AFSK_STROBE_OFF();
 275         AFSK_ADC_IRQ_END();
 276 }
 277
 278 static void afsk_txStart(Afsk *af)
 279 {
 280         if (!af->sending)
 281         {
 282                 af->phase_inc = MARK_INC;
 283                 af->phase_acc = 0;
 284                 af->stuff_cnt = 0;
 285                 af->sending = true;
 286                 af->preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
 287                 AFSK_DAC_IRQ_START();
 288         }
 289         ATOMIC(af->trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
 290 }
 291
 292 #define BIT_STUFF_LEN 5
 293
 294 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 295
 296 void afsk_dac_isr(Afsk *af)
 297 {
 298         /* Check if we are at a start of a sample cycle */
 299         if (af->sample_count == 0)
 300         {
 301                 if (af->tx_bit == 0)
 302                 {
 303                         /* We have just finished transimitting a char, get a new one. */
 304                         if (fifo_isempty(&af->tx_fifo) && af->trailer_len == 0)
 305                         {
 306                                 AFSK_DAC_IRQ_STOP();
 307                                 af->sending = false;
 308                                 AFSK_DAC_IRQ_END();
 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();
 347                                                 af->sending = false;
 348                                                 AFSK_DAC_IRQ_END();
 349                                                 return;
 350                                         }
 351                                         else
 352                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 353                                 }
 354                                 else if (af->curr_out == HDLC_FLAG || af->curr_out == HDLC_RESET)
 355                                         /* If these chars are not escaped disable bit stuffing */
 356                                         af->bit_stuff = false;
 357                         }
 358                         /* Start with LSB mask */
 359                         af->tx_bit = 0x01;
 360                 }
 361
 362                 /* check for bit stuffing */
 363                 if (af->bit_stuff && af->stuff_cnt >= BIT_STUFF_LEN)
 364                 {
 365                         /* If there are more than 5 ones in a row insert a 0 */
 366                         af->stuff_cnt = 0;
 367                         /* switch tone */
 368                         af->phase_inc = SWITCH_TONE(af->phase_inc);
 369                 }
 370                 else
 371                 {
 372                         /*
 373                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 374                          * unchanged; with a 0, there will be a change in the tone.
 375                          */
 376                         if (af->curr_out & af->tx_bit)
 377                         {
 378                                 /*
 379                                  * Transmit a 1:
 380                                  * - Stay on the previous tone
 381                                  * - Increace bit stuff count
 382                                  */
 383                                 af->stuff_cnt++;
 384                         }
 385                         else
 386                         {
 387                                 /*
 388                                  * Transmit a 0:
 389                                  * - Reset bit stuff count
 390                                  * - Switch tone
 391                                  */
 392                                 af->stuff_cnt = 0;
 393                                 af->phase_inc = SWITCH_TONE(af->phase_inc);
 394                         }
 395
 396                         /* Go to the next bit */
 397                         af->tx_bit <<= 1;
 398                 }
 399                 af->sample_count = DAC_SAMPLEPERBIT;
 400         }
 401
 402         /* Get new sample and put it out on the DAC */
 403         af->phase_acc += af->phase_inc;
 404         af->phase_acc %= SIN_LEN;
 405
 406         AFSK_SET_DAC(sin_sample(af->phase_acc));
 407         af->sample_count--;
 408         AFSK_DAC_IRQ_END();
 409 }
 410
 411
 412 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
 413 {
 414         Afsk *af = AFSK_CAST(fd);
 415         uint8_t *buf = (uint8_t *)_buf;
 416
 417         #if CONFIG_AFSK_RXTIMEOUT == 0
 418         while (size-- && !fifo_isempty_locked(&af->rx_fifo))
 419         #else
 420         while (size--)
 421         #endif
 422         {
 423                 #if CONFIG_AFSK_RXTIMEOUT != -1
 424                 ticks_t start = timer_clock();
 425                 #endif
 426
 427                 while (fifo_isempty_locked(&af->rx_fifo));
 428                 {
 429                         cpu_relax();
 430                         #if CONFIG_AFSK_RXTIMEOUT != -1
 431                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 432                                 return buf - (uint8_t *)_buf;
 433                         #endif
 434                 }
 435
 436                 *buf++ = fifo_pop_locked(&af->rx_fifo);
 437         }
 438
 439         return buf - (uint8_t *)_buf;
 440 }
 441
 442 static size_t afsk_write(KFile *fd, const void *_buf, size_t size)
 443 {
 444         Afsk *af = AFSK_CAST(fd);
 445         const uint8_t *buf = (const uint8_t *)_buf;
 446
 447         while (size--)
 448         {
 449                 while (fifo_isfull_locked(&af->tx_fifo))
 450                         cpu_relax();
 451
 452                 fifo_push_locked(&af->tx_fifo, *buf++);
 453                 afsk_txStart(af);
 454         }
 455
 456         return buf - (const uint8_t *)_buf;
 457 }
 458
 459 static int afsk_flush(KFile *fd)
 460 {
 461         Afsk *af = AFSK_CAST(fd);
 462         while (af->sending)
 463                 cpu_relax();
 464         return 0;
 465 }
 466
 467
 468 void afsk_init(Afsk *af)
 469 {
 470         #if CONFIG_AFSK_RXTIMEOUT != -1
 471         MOD_CHECK(timer);
 472         #endif
 473         memset(af, 0, sizeof(*af));
 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();
 485         AFSK_STROBE_INIT();
 486         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 487
 488         DB(af->fd._type = KFT_AFSK);
 489         af->fd.write = afsk_write;
 490         af->fd.read = afsk_read;
 491         af->fd.flush = afsk_flush;
 492         af->phase_inc = MARK_INC;
 493 }