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