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
 155 static uint8_t sampled_bits;
 156 static uint8_t found_bits;
 157 static uint8_t demod_bits;
 158 static int8_t curr_phase;
 159
 160 static bool hdlc_rxstart;
 161 static uint8_t hdlc_currchar;
 162 static uint8_t hdlc_bit_idx;
 163
 164 #define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
 165 #define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)
 166
 167 static uint16_t preamble_len;
 168 static uint16_t trailer_len;
 169
 170 static void hdlc_parse(bool bit)
 171 {
 172         demod_bits <<= 1;
 173         demod_bits |= bit ? 1 : 0;
 174
 175         /* HDLC Flag */
 176         if (demod_bits == HDLC_FLAG)
 177         {
 178                 if (!fifo_isfull(&rx_fifo))
 179                 {
 180                         fifo_push(&rx_fifo, HDLC_FLAG);
 181                         hdlc_rxstart = true;
 182                 }
 183                 else
 184                         hdlc_rxstart = false;
 185
 186                 hdlc_currchar = 0;
 187                 hdlc_bit_idx = 0;
 188                 return;
 189         }
 190
 191         /* Reset */
 192         if ((demod_bits & HDLC_RESET) == HDLC_RESET)
 193         {
 194                 hdlc_rxstart = false;
 195                 return;
 196         }
 197
 198         if (!hdlc_rxstart)
 199                 return;
 200
 201         /* Stuffed bit */
 202         if ((demod_bits & 0x3f) == 0x3e)
 203                 return;
 204
 205         if (demod_bits & 0x01)
 206                 hdlc_currchar |= 0x80;
 207
 208         if (++hdlc_bit_idx >= 8)
 209         {
 210                 if ((hdlc_currchar == HDLC_FLAG
 211                         || hdlc_currchar == HDLC_RESET
 212                         || hdlc_currchar == AX25_ESC))
 213                 {
 214                         if (!fifo_isfull(&rx_fifo))
 215                                 fifo_push(&rx_fifo, AX25_ESC);
 216                         else
 217                                 hdlc_rxstart = false;
 218                 }
 219
 220                 if (!fifo_isfull(&rx_fifo))
 221                         fifo_push(&rx_fifo, hdlc_currchar);
 222                 else
 223                         hdlc_rxstart = false;
 224
 225                 hdlc_currchar = 0;
 226                 hdlc_bit_idx = 0;
 227                 return;
 228         }
 229
 230         hdlc_currchar >>= 1;
 231 }
 232
 233 DEFINE_AFSK_ADC_ISR()
 234 {
 235         AFSK_STROBE_ON();
 236         int8_t curr_sample = AFSK_READ_ADC();
 237
 238         /*
 239          * Frequency discriminator and LP IIR filter.
 240          * This filter is designed to work
 241          * at the given sample rate and bit rate.
 242          */
 243         STATIC_ASSERT(SAMPLERATE == 9600);
 244         STATIC_ASSERT(BITRATE == 1200);
 245
 246         /*
 247          * Frequency discrimination is achieved by simply multiplying
 248          * the sample with a delayed sample of (samples per bit) / 2.
 249          * Then the signal is lowpass filtered with a first order,
 250          * 600 Hz filter. The filter implementation is selectable
 251          * through the CONFIG_AFSK_FILTER config variable.
 252          */
 253
 254         iir_x[0] = iir_x[1];
 255
 256         #if (CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH)
 257                 iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
 258                 //iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) / 6.027339492;
 259         #elif (CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV)
 260                 iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) >> 2;
 261                 //iir_x[1] = ((int8_t)fifo_pop(&delay_fifo) * curr_sample) / 3.558147322;
 262         #else
 263                 #error Filter type not found!
 264         #endif
 265
 266         iir_y[0] = iir_y[1];
 267
 268         #if CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH
 269                 /*
 270                  * This strange sum + shift is an optimization for iir_y[0] * 0.668.
 271                  * iir * 0.668 ~= (iir * 21) / 32 =
 272                  * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
 273                  * = iir / 2 + iir / 8 + iir / 32 =
 274                  * = iir >> 1 + iir >> 3 + iir >> 5
 275                  */
 276                 iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1) + (iir_y[0] >> 3) + (iir_y[0] >> 5);
 277                 //iir_y[1] = iir_x[0] + iir_x[1] + iir_y[0] * 0.6681786379;
 278         #elif CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV
 279                 /*
 280                  * This should be (iir_y[0] * 0.438) but
 281                  * (iir_y[0] >> 1) is a faster approximation :-)
 282                  */
 283                 iir_y[1] = iir_x[0] + iir_x[1] + (iir_y[0] >> 1);
 284                 //iir_y[1] = iir_x[0] + iir_x[1] + iir_y[0] * 0.4379097269;
 285         #endif
 286
 287         /* Save this sampled bit in a delay line */
 288         sampled_bits <<= 1;
 289         sampled_bits |= (iir_y[1] > 0) ? 1 : 0;
 290
 291         /* Store current ADC sample in the delay_fifo */
 292         fifo_push(&delay_fifo, curr_sample);
 293
 294         /* If there is an edge, adjust phase sampling */
 295         if (EDGE_FOUND(sampled_bits))
 296         {
 297                 if (curr_phase < PHASE_THRES)
 298                         curr_phase += PHASE_INC;
 299                 else
 300                         curr_phase -= PHASE_INC;
 301         }
 302         curr_phase += PHASE_BIT;
 303
 304         /* sample the bit */
 305         if (curr_phase >= PHASE_MAX)
 306         {
 307                 curr_phase %= PHASE_MAX;
 308
 309                 /* Shift 1 position in the shift register of the found bits */
 310                 found_bits <<= 1;
 311
 312                 /*
 313                  * Determine bit value by reading the last 3 sampled bits.
 314                  * If the number of ones is two or greater, the bit value is a 1,
 315                  * otherwise is a 0.
 316                  */
 317                 uint8_t bits = sampled_bits & 0x07;
 318                 if (bits == 0x07 // 111, 3 bits set to 1
 319                  || bits == 0x06 // 110, 2 bits
 320                  || bits == 0x05 // 101, 2 bits
 321                  || bits == 0x03 // 011, 2 bits
 322                 )
 323                         found_bits |= 1;
 324
 325                 /*
 326                  * NRZI coding: if 2 consecutive bits have the same value
 327                  * a 1 is received, otherwise it's a 0.
 328                  */
 329                 hdlc_parse(!EDGE_FOUND(found_bits));
 330         }
 331
 332
 333         AFSK_STROBE_OFF();
 334         AFSK_ADC_IRQ_END();
 335 }
 336
 337 /* True while modem sends data */
 338 static volatile bool sending;
 339
 340 static void afsk_txStart(void)
 341 {
 342         if (!sending)
 343         {
 344                 phase_inc = MARK_INC;
 345                 phase_acc = 0;
 346                 stuff_cnt = 0;
 347                 sending = true;
 348                 preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
 349                 AFSK_DAC_IRQ_START();
 350         }
 351         ATOMIC(trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
 352 }
 353
 354 #define BIT_STUFF_LEN 5
 355
 356 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
 357
 358 DEFINE_AFSK_DAC_ISR()
 359 {
 360         /* Check if we are at a start of a sample cycle */
 361         if (sample_count == 0)
 362         {
 363                 if (tx_bit == 0)
 364                 {
 365                         /* We have just finished transimitting a char, get a new one. */
 366                         if (fifo_isempty(&tx_fifo) && trailer_len == 0)
 367                         {
 368                                 AFSK_DAC_IRQ_STOP();
 369                                 sending = false;
 370                                 AFSK_DAC_IRQ_END();
 371                                 return;
 372                         }
 373                         else
 374                         {
 375                                 /*
 376                                  * If we have just finished sending an unstuffed byte,
 377                                  * reset bitstuff counter.
 378                                  */
 379                                 if (!bit_stuff)
 380                                         stuff_cnt = 0;
 381
 382                                 bit_stuff = true;
 383
 384                                 /*
 385                                  * Handle preamble and trailer
 386                                  */
 387                                 if (preamble_len == 0)
 388                                 {
 389                                         if (fifo_isempty(&tx_fifo))
 390                                         {
 391                                                 trailer_len--;
 392                                                 curr_out = HDLC_FLAG;
 393                                         }
 394                                         else
 395                                                 curr_out = fifo_pop(&tx_fifo);
 396                                 }
 397                                 else
 398                                 {
 399                                         preamble_len--;
 400                                         curr_out = HDLC_FLAG;
 401                                 }
 402
 403                                 /* Handle char escape */
 404                                 if (curr_out == AX25_ESC)
 405                                 {
 406                                         if (fifo_isempty(&tx_fifo))
 407                                         {
 408                                                 AFSK_DAC_IRQ_STOP();
 409                                                 sending = false;
 410                                                 AFSK_DAC_IRQ_END();
 411                                                 return;
 412                                         }
 413                                         else
 414                                                 curr_out = fifo_pop(&tx_fifo);
 415                                 }
 416                                 else if (curr_out == HDLC_FLAG || curr_out == HDLC_RESET)
 417                                         /* If these chars are not escaped disable bit stuffing */
 418                                         bit_stuff = false;
 419                         }
 420                         /* Start with LSB mask */
 421                         tx_bit = 0x01;
 422                 }
 423
 424                 /* check for bit stuffing */
 425                 if (bit_stuff && stuff_cnt >= BIT_STUFF_LEN)
 426                 {
 427                         /* If there are more than 5 ones in a row insert a 0 */
 428                         stuff_cnt = 0;
 429                         /* switch tone */
 430                         phase_inc = SWITCH_TONE(phase_inc);
 431                 }
 432                 else
 433                 {
 434                         /*
 435                          * NRZI: if we want to transmit a 1 the modulated frequency will stay
 436                          * unchanged; with a 0, there will be a change in the tone.
 437                          */
 438                         if (curr_out & tx_bit)
 439                         {
 440                                 /*
 441                                  * Transmit a 1:
 442                                  * - Stay on the previous tone
 443                                  * - Increace bit stuff count
 444                                  */
 445                                 stuff_cnt++;
 446                         }
 447                         else
 448                         {
 449                                 /*
 450                                  * Transmit a 0:
 451                                  * - Reset bit stuff count
 452                                  * - Switch tone
 453                                  */
 454                                 stuff_cnt = 0;
 455                                 phase_inc = SWITCH_TONE(phase_inc);
 456                         }
 457
 458                         /* Go to the next bit */
 459                         tx_bit <<= 1;
 460                 }
 461                 sample_count = DAC_SAMPLEPERBIT;
 462         }
 463
 464         /* Get new sample and put it out on the DAC */
 465         phase_acc += phase_inc;
 466         phase_acc %= SIN_LEN;
 467
 468         AFSK_SET_DAC(sin_sample(phase_acc));
 469         sample_count--;
 470         AFSK_DAC_IRQ_END();
 471 }
 472
 473
 474 static size_t afsk_read(UNUSED_ARG(KFile *, fd), void *_buf, size_t size)
 475 {
 476         uint8_t *buf = (uint8_t *)_buf;
 477
 478         #if CONFIG_AFSK_RXTIMEOUT == 0
 479         while (size-- && !fifo_isempty_locked(&rx_fifo))
 480         #else
 481         while (size--)
 482         #endif
 483         {
 484                 #if CONFIG_AFSK_RXTIMEOUT != -1
 485                 ticks_t start = timer_clock();
 486                 #endif
 487
 488                 while (fifo_isempty_locked(&rx_fifo));
 489                 {
 490                         cpu_relax();
 491                         #if CONFIG_AFSK_RXTIMEOUT != -1
 492                         if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
 493                                 return buf - (uint8_t *)_buf;
 494                         #endif
 495                 }
 496
 497                 *buf++ = fifo_pop_locked(&rx_fifo);
 498         }
 499
 500         return buf - (uint8_t *)_buf;
 501 }
 502
 503 static size_t afsk_write(UNUSED_ARG(KFile *, fd), const void *_buf, size_t size)
 504 {
 505         const uint8_t *buf = (const uint8_t *)_buf;
 506
 507         while (size--)
 508         {
 509                 while (fifo_isfull_locked(&tx_fifo))
 510                         cpu_relax();
 511
 512                 fifo_push_locked(&tx_fifo, *buf++);
 513                 afsk_txStart();
 514         }
 515
 516         return buf - (const uint8_t *)_buf;
 517 }
 518
 519 static int afsk_flush(UNUSED_ARG(KFile *, fd))
 520 {
 521         while (sending)
 522                 cpu_relax();
 523         return 0;
 524 }
 525
 526
 527 void afsk_init(Afsk *af)
 528 {
 529         #if CONFIG_AFSK_RXTIMEOUT != -1
 530         MOD_CHECK(timer);
 531         #endif
 532
 533         fifo_init(&delay_fifo, (uint8_t *)delay_buf, sizeof(delay_buf));
 534         fifo_init(&rx_fifo, rx_buf, sizeof(rx_buf));
 535
 536         /* Fill sample FIFO with 0 */
 537         for (int i = 0; i < SAMPLEPERBIT / 2; i++)
 538                 fifo_push(&delay_fifo, 0);
 539
 540         fifo_init(&tx_fifo, tx_buf, sizeof(tx_buf));
 541
 542         AFSK_ADC_INIT();
 543         AFSK_STROBE_INIT();
 544         kprintf("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
 545
 546         memset(af, 0, sizeof(*af));
 547         DB(af->fd._type = KFT_AFSK);
 548         af->fd.write = afsk_write;
 549         af->fd.read = afsk_read;
 550         af->fd.flush = afsk_flush;
 551 }