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