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 }
 276
 277 static void afsk_txStart(Afsk *af)
 278 {
 279         if (!af->sending)
 280         {
 281                 af->phase_inc = MARK_INC;
 282                 af->phase_acc = 0;
 283                 af->stuff_cnt = 0;
 284                 af->sending = true;
 285                 af->preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
 286                 AFSK_DAC_IRQ_START(af->dac_ch);
 287         }
 288         ATOMIC(af->trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
 289 }
 290
 291 #define BIT_STUFF_LEN 5
 292
 293 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 294
 295 void afsk_dac_isr(Afsk *af)
 296 {
 297         /* Check if we are at a start of a sample cycle */
 298         if (af->sample_count == 0)
 299         {
 300                 if (af->tx_bit == 0)
 301                 {
 302                         /* We have just finished transimitting a char, get a new one. */
 303                         if (fifo_isempty(&af->tx_fifo) && af->trailer_len == 0)
 304                         {
 305                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 306                                 af->sending = false;
 307                                 return;
 308                         }
 309                         else
 310                         {
 311                                 /*
 312                                  * If we have just finished af->sending an unstuffed byte,
 313                                  * reset bitstuff counter.
 314                                  */
 315                                 if (!af->bit_stuff)
 316                                         af->stuff_cnt = 0;
 317
 318                                 af->bit_stuff = true;
 319
 320                                 /*
 321                                  * Handle preamble and trailer
 322                                  */
 323                                 if (af->preamble_len == 0)
 324                                 {
 325                                         if (fifo_isempty(&af->tx_fifo))
 326                                         {
 327                                                 af->trailer_len--;
 328                                                 af->curr_out = HDLC_FLAG;
 329                                         }
 330                                         else
 331                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 332                                 }
 333                                 else
 334                                 {
 335                                         af->preamble_len--;
 336                                         af->curr_out = HDLC_FLAG;
 337                                 }
 338
 339                                 /* Handle char escape */
 340                                 if (af->curr_out == AX25_ESC)
 341                                 {
 342                                         if (fifo_isempty(&af->tx_fifo))
 343                                         {
 344                                                 AFSK_DAC_IRQ_STOP(af->dac_ch);
 345                                                 af->sending = false;
 346                                                 return;
 347                                         }
 348                                         else
 349                                                 af->curr_out = fifo_pop(&af->tx_fifo);
 350                                 }
 351                                 else if (af->curr_out == HDLC_FLAG || af->curr_out == HDLC_RESET)
 352                                         /* If these chars are not escaped disable bit stuffing */
 353                                         af->bit_stuff = false;
 354                         }
 355                         /* Start with LSB mask */
 356                         af->tx_bit = 0x01;
 357                 }
 358
 359                 /* check for bit stuffing */
 360                 if (af->bit_stuff && af->stuff_cnt >= BIT_STUFF_LEN)
 361                 {
 362                         /* If there are more than 5 ones in a row insert a 0 */
 363                         af->stuff_cnt = 0;
 364                         /* switch tone */
 365                         af->phase_inc = SWITCH_TONE(af->phase_inc);
 366                 }
 367                 else
 368                 {
 369                         /*
 370                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 371                          * unchanged; with a 0, there will be a change in the tone.
 372                          */
 373                         if (af->curr_out & af->tx_bit)
 374                         {
 375                                 /*
 376                                  * Transmit a 1:
 377                                  * - Stay on the previous tone
 378                                  * - Increace bit stuff count
 379                                  */
 380                                 af->stuff_cnt++;
 381                         }
 382                         else
 383                         {
 384                                 /*
 385                                  * Transmit a 0:
 386                                  * - Reset bit stuff count
 387                                  * - Switch tone
 388                                  */
 389                                 af->stuff_cnt = 0;
 390                                 af->phase_inc = SWITCH_TONE(af->phase_inc);
 391                         }
 392
 393                         /* Go to the next bit */
 394                         af->tx_bit <<= 1;
 395                 }
 396                 af->sample_count = DAC_SAMPLEPERBIT;
 397         }
 398
 399         /* Get new sample and put it out on the DAC */
 400         af->phase_acc += af->phase_inc;
 401         af->phase_acc %= SIN_LEN;
 402
 403         AFSK_DAC_SET(af->dac_ch, sin_sample(af->phase_acc));
 404         af->sample_count--;
 405 }
 406
 407
 408 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
 409 {
 410         Afsk *af = AFSK_CAST(fd);
 411         uint8_t *buf = (uint8_t *)_buf;
 412
 413         #if CONFIG_AFSK_RXTIMEOUT == 0
 414         while (size-- && !fifo_isempty_locked(&af->rx_fifo))
 415         #else
 416         while (size--)
 417         #endif
 418         {
 419                 #if CONFIG_AFSK_RXTIMEOUT != -1
 420                 ticks_t start = timer_clock();
 421                 #endif
 422
 423                 while (fifo_isempty_locked(&af->rx_fifo));
 424                 {
 425                         cpu_relax();
 426                         #if CONFIG_AFSK_RXTIMEOUT != -1
 427                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 428                                 return buf - (uint8_t *)_buf;
 429                         #endif
 430                 }
 431
 432                 *buf++ = fifo_pop_locked(&af->rx_fifo);
 433         }
 434
 435         return buf - (uint8_t *)_buf;
 436 }
 437
 438 static size_t afsk_write(KFile *fd, const void *_buf, size_t size)
 439 {
 440         Afsk *af = AFSK_CAST(fd);
 441         const uint8_t *buf = (const uint8_t *)_buf;
 442
 443         while (size--)
 444         {
 445                 while (fifo_isfull_locked(&af->tx_fifo))
 446                         cpu_relax();
 447
 448                 fifo_push_locked(&af->tx_fifo, *buf++);
 449                 afsk_txStart(af);
 450         }
 451
 452         return buf - (const uint8_t *)_buf;
 453 }
 454
 455 static int afsk_flush(KFile *fd)
 456 {
 457         Afsk *af = AFSK_CAST(fd);
 458         while (af->sending)
 459                 cpu_relax();
 460         return 0;
 461 }
 462
 463
 464 void afsk_init(Afsk *af, int adc_ch, int dac_ch)
 465 {
 466         #if CONFIG_AFSK_RXTIMEOUT != -1
 467         MOD_CHECK(timer);
 468         #endif
 469         memset(af, 0, sizeof(*af));
 470         af->adc_ch = adc_ch;
 471         af->dac_ch = dac_ch;
 472
 473         fifo_init(&af->delay_fifo, (uint8_t *)af->delay_buf, sizeof(af->delay_buf));
 474         fifo_init(&af->rx_fifo, af->rx_buf, sizeof(af->rx_buf));
 475
 476         /* Fill sample FIFO with 0 */
 477         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 478                 fifo_push(&af->delay_fifo, 0);
 479
 480         fifo_init(&af->tx_fifo, af->tx_buf, sizeof(af->tx_buf));
 481
 482         AFSK_ADC_INIT(adc_ch, af);
 483         AFSK_DAC_INIT(dac_ch, af);
 484         AFSK_STROBE_INIT();
 485         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 486
 487         DB(af->fd._type = KFT_AFSK);
 488         af->fd.write = afsk_write;
 489         af->fd.read = afsk_read;
 490         af->fd.flush = afsk_flush;
 491         af->phase_inc = MARK_INC;
 492 }