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