BeRTOS
afsk.c
Go to the documentation of this file.
00001 
00038 #include "afsk.h"
00039 #include <net/ax25.h>
00040 
00041 #include "cfg/cfg_afsk.h"
00042 #include "hw/hw_afsk.h"
00043 
00044 #include <drv/timer.h>
00045 
00046 #include <cfg/module.h>
00047 
00048 #define LOG_LEVEL   AFSK_LOG_LEVEL
00049 #define LOG_FORMAT  AFSK_LOG_FORMAT
00050 #include <cfg/log.h>
00051 
00052 #include <cpu/power.h>
00053 #include <cpu/pgm.h>
00054 #include <struct/fifobuf.h>
00055 
00056 #include <string.h> /* memset */
00057 
00058 #define PHASE_BIT    8
00059 #define PHASE_INC    1
00060 
00061 #define PHASE_MAX    (SAMPLEPERBIT * PHASE_BIT)
00062 #define PHASE_THRES  (PHASE_MAX / 2) // - PHASE_BIT / 2)
00063 
00064 // Modulator constants
00065 #define MARK_FREQ  1200
00066 #define MARK_INC   (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)MARK_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
00067 
00068 #define SPACE_FREQ 2200
00069 #define SPACE_INC  (uint16_t)(DIV_ROUND(SIN_LEN * (uint32_t)SPACE_FREQ, CONFIG_AFSK_DAC_SAMPLERATE))
00070 
00071 //Ensure sample rate is a multiple of bit rate
00072 STATIC_ASSERT(!(CONFIG_AFSK_DAC_SAMPLERATE % BITRATE));
00073 
00074 #define DAC_SAMPLEPERBIT (CONFIG_AFSK_DAC_SAMPLERATE / BITRATE)
00075 
00081 static const uint8_t PROGMEM sin_table[] =
00082 {
00083     128, 129, 131, 132, 134, 135, 137, 138, 140, 142, 143, 145, 146, 148, 149, 151,
00084     152, 154, 155, 157, 158, 160, 162, 163, 165, 166, 167, 169, 170, 172, 173, 175,
00085     176, 178, 179, 181, 182, 183, 185, 186, 188, 189, 190, 192, 193, 194, 196, 197,
00086     198, 200, 201, 202, 203, 205, 206, 207, 208, 210, 211, 212, 213, 214, 215, 217,
00087     218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233,
00088     234, 234, 235, 236, 237, 238, 238, 239, 240, 241, 241, 242, 243, 243, 244, 245,
00089     245, 246, 246, 247, 248, 248, 249, 249, 250, 250, 250, 251, 251, 252, 252, 252,
00090     253, 253, 253, 253, 254, 254, 254, 254, 254, 255, 255, 255, 255, 255, 255, 255,
00091 };
00092 
00093 #define SIN_LEN 512 ///< Full wave length
00094 
00095 STATIC_ASSERT(sizeof(sin_table) == SIN_LEN / 4);
00096 
00097 
00102 INLINE uint8_t sin_sample(uint16_t idx)
00103 {
00104     ASSERT(idx < SIN_LEN);
00105     uint16_t new_idx = idx % (SIN_LEN / 2);
00106     new_idx = (new_idx >= (SIN_LEN / 4)) ? (SIN_LEN / 2 - new_idx - 1) : new_idx;
00107 
00108     uint8_t data = pgm_read8(&sin_table[new_idx]);
00109 
00110     return (idx >= (SIN_LEN / 2)) ? (255 - data) : data;
00111 }
00112 
00113 
00114 #define BIT_DIFFER(bitline1, bitline2) (((bitline1) ^ (bitline2)) & 0x01)
00115 #define EDGE_FOUND(bitline)            BIT_DIFFER((bitline), (bitline) >> 1)
00116 
00127 static bool hdlc_parse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo)
00128 {
00129     bool ret = true;
00130 
00131     hdlc->demod_bits <<= 1;
00132     hdlc->demod_bits |= bit ? 1 : 0;
00133 
00134     /* HDLC Flag */
00135     if (hdlc->demod_bits == HDLC_FLAG)
00136     {
00137         if (!fifo_isfull(fifo))
00138         {
00139             fifo_push(fifo, HDLC_FLAG);
00140             hdlc->rxstart = true;
00141         }
00142         else
00143         {
00144             ret = false;
00145             hdlc->rxstart = false;
00146         }
00147 
00148         hdlc->currchar = 0;
00149         hdlc->bit_idx = 0;
00150         return ret;
00151     }
00152 
00153     /* Reset */
00154     if ((hdlc->demod_bits & HDLC_RESET) == HDLC_RESET)
00155     {
00156         hdlc->rxstart = false;
00157         return ret;
00158     }
00159 
00160     if (!hdlc->rxstart)
00161         return ret;
00162 
00163     /* Stuffed bit */
00164     if ((hdlc->demod_bits & 0x3f) == 0x3e)
00165         return ret;
00166 
00167     if (hdlc->demod_bits & 0x01)
00168         hdlc->currchar |= 0x80;
00169 
00170     if (++hdlc->bit_idx >= 8)
00171     {
00172         if ((hdlc->currchar == HDLC_FLAG
00173             || hdlc->currchar == HDLC_RESET
00174             || hdlc->currchar == AX25_ESC))
00175         {
00176             if (!fifo_isfull(fifo))
00177                 fifo_push(fifo, AX25_ESC);
00178             else
00179             {
00180                 hdlc->rxstart = false;
00181                 ret = false;
00182             }
00183         }
00184 
00185         if (!fifo_isfull(fifo))
00186             fifo_push(fifo, hdlc->currchar);
00187         else
00188         {
00189             hdlc->rxstart = false;
00190             ret = false;
00191         }
00192 
00193         hdlc->currchar = 0;
00194         hdlc->bit_idx = 0;
00195     }
00196     else
00197         hdlc->currchar >>= 1;
00198 
00199     return ret;
00200 }
00201 
00202 
00210 void afsk_adc_isr(Afsk *af, int8_t curr_sample)
00211 {
00212     AFSK_STROBE_ON();
00213 
00214     /*
00215      * Frequency discriminator and LP IIR filter.
00216      * This filter is designed to work
00217      * at the given sample rate and bit rate.
00218      */
00219     STATIC_ASSERT(SAMPLERATE == 9600);
00220     STATIC_ASSERT(BITRATE == 1200);
00221 
00222     /*
00223      * Frequency discrimination is achieved by simply multiplying
00224      * the sample with a delayed sample of (samples per bit) / 2.
00225      * Then the signal is lowpass filtered with a first order,
00226      * 600 Hz filter. The filter implementation is selectable
00227      * through the CONFIG_AFSK_FILTER config variable.
00228      */
00229 
00230     af->iir_x[0] = af->iir_x[1];
00231 
00232     #if (CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH)
00233         af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
00234         //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 6.027339492;
00235     #elif (CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV)
00236         af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) >> 2;
00237         //af->iir_x[1] = ((int8_t)fifo_pop(&af->delay_fifo) * curr_sample) / 3.558147322;
00238     #else
00239         #error Filter type not found!
00240     #endif
00241 
00242     af->iir_y[0] = af->iir_y[1];
00243 
00244     #if CONFIG_AFSK_FILTER == AFSK_BUTTERWORTH
00245         /*
00246          * This strange sum + shift is an optimization for af->iir_y[0] * 0.668.
00247          * iir * 0.668 ~= (iir * 21) / 32 =
00248          * = (iir * 16) / 32 + (iir * 4) / 32 + iir / 32 =
00249          * = iir / 2 + iir / 8 + iir / 32 =
00250          * = iir >> 1 + iir >> 3 + iir >> 5
00251          */
00252         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);
00253         //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.6681786379;
00254     #elif CONFIG_AFSK_FILTER == AFSK_CHEBYSHEV
00255         /*
00256          * This should be (af->iir_y[0] * 0.438) but
00257          * (af->iir_y[0] >> 1) is a faster approximation :-)
00258          */
00259         af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + (af->iir_y[0] >> 1);
00260         //af->iir_y[1] = af->iir_x[0] + af->iir_x[1] + af->iir_y[0] * 0.4379097269;
00261     #endif
00262 
00263     /* Save this sampled bit in a delay line */
00264     af->sampled_bits <<= 1;
00265     af->sampled_bits |= (af->iir_y[1] > 0) ? 1 : 0;
00266 
00267     /* Store current ADC sample in the af->delay_fifo */
00268     fifo_push(&af->delay_fifo, curr_sample);
00269 
00270     /* If there is an edge, adjust phase sampling */
00271     if (EDGE_FOUND(af->sampled_bits))
00272     {
00273         if (af->curr_phase < PHASE_THRES)
00274             af->curr_phase += PHASE_INC;
00275         else
00276             af->curr_phase -= PHASE_INC;
00277     }
00278     af->curr_phase += PHASE_BIT;
00279 
00280     /* sample the bit */
00281     if (af->curr_phase >= PHASE_MAX)
00282     {
00283         af->curr_phase %= PHASE_MAX;
00284 
00285         /* Shift 1 position in the shift register of the found bits */
00286         af->found_bits <<= 1;
00287 
00288         /*
00289          * Determine bit value by reading the last 3 sampled bits.
00290          * If the number of ones is two or greater, the bit value is a 1,
00291          * otherwise is a 0.
00292          * This algorithm presumes that there are 8 samples per bit.
00293          */
00294         STATIC_ASSERT(SAMPLEPERBIT == 8);
00295         uint8_t bits = af->sampled_bits & 0x07;
00296         if (bits == 0x07 // 111, 3 bits set to 1
00297          || bits == 0x06 // 110, 2 bits
00298          || bits == 0x05 // 101, 2 bits
00299          || bits == 0x03 // 011, 2 bits
00300         )
00301             af->found_bits |= 1;
00302 
00303         /*
00304          * NRZI coding: if 2 consecutive bits have the same value
00305          * a 1 is received, otherwise it's a 0.
00306          */
00307         if (!hdlc_parse(&af->hdlc, !EDGE_FOUND(af->found_bits), &af->rx_fifo))
00308             af->status |= AFSK_RXFIFO_OVERRUN;
00309     }
00310 
00311 
00312     AFSK_STROBE_OFF();
00313 }
00314 
00315 static void afsk_txStart(Afsk *af)
00316 {
00317     if (!af->sending)
00318     {
00319         af->phase_inc = MARK_INC;
00320         af->phase_acc = 0;
00321         af->stuff_cnt = 0;
00322         af->sending = true;
00323         af->preamble_len = DIV_ROUND(CONFIG_AFSK_PREAMBLE_LEN * BITRATE, 8000);
00324         AFSK_DAC_IRQ_START(af->dac_ch);
00325     }
00326     ATOMIC(af->trailer_len  = DIV_ROUND(CONFIG_AFSK_TRAILER_LEN  * BITRATE, 8000));
00327 }
00328 
00329 #define BIT_STUFF_LEN 5
00330 
00331 #define SWITCH_TONE(inc)  (((inc) == MARK_INC) ? SPACE_INC : MARK_INC)
00332 
00342 uint8_t afsk_dac_isr(Afsk *af)
00343 {
00344     AFSK_STROBE_ON();
00345 
00346     /* Check if we are at a start of a sample cycle */
00347     if (af->sample_count == 0)
00348     {
00349         if (af->tx_bit == 0)
00350         {
00351             /* We have just finished transimitting a char, get a new one. */
00352             if (fifo_isempty(&af->tx_fifo) && af->trailer_len == 0)
00353             {
00354                 AFSK_DAC_IRQ_STOP(af->dac_ch);
00355                 af->sending = false;
00356                 AFSK_STROBE_OFF();
00357                 return 0;
00358             }
00359             else
00360             {
00361                 /*
00362                  * If we have just finished sending an unstuffed byte,
00363                  * reset bitstuff counter.
00364                  */
00365                 if (!af->bit_stuff)
00366                     af->stuff_cnt = 0;
00367 
00368                 af->bit_stuff = true;
00369 
00370                 /*
00371                  * Handle preamble and trailer
00372                  */
00373                 if (af->preamble_len == 0)
00374                 {
00375                     if (fifo_isempty(&af->tx_fifo))
00376                     {
00377                         af->trailer_len--;
00378                         af->curr_out = HDLC_FLAG;
00379                     }
00380                     else
00381                         af->curr_out = fifo_pop(&af->tx_fifo);
00382                 }
00383                 else
00384                 {
00385                     af->preamble_len--;
00386                     af->curr_out = HDLC_FLAG;
00387                 }
00388 
00389                 /* Handle char escape */
00390                 if (af->curr_out == AX25_ESC)
00391                 {
00392                     if (fifo_isempty(&af->tx_fifo))
00393                     {
00394                         AFSK_DAC_IRQ_STOP(af->dac_ch);
00395                         af->sending = false;
00396                         AFSK_STROBE_OFF();
00397                         return 0;
00398                     }
00399                     else
00400                         af->curr_out = fifo_pop(&af->tx_fifo);
00401                 }
00402                 else if (af->curr_out == HDLC_FLAG || af->curr_out == HDLC_RESET)
00403                     /* If these chars are not escaped disable bit stuffing */
00404                     af->bit_stuff = false;
00405             }
00406             /* Start with LSB mask */
00407             af->tx_bit = 0x01;
00408         }
00409 
00410         /* check for bit stuffing */
00411         if (af->bit_stuff && af->stuff_cnt >= BIT_STUFF_LEN)
00412         {
00413             /* If there are more than 5 ones in a row insert a 0 */
00414             af->stuff_cnt = 0;
00415             /* switch tone */
00416             af->phase_inc = SWITCH_TONE(af->phase_inc);
00417         }
00418         else
00419         {
00420             /*
00421              * NRZI: if we want to transmit a 1 the modulated frequency will stay
00422              * unchanged; with a 0, there will be a change in the tone.
00423              */
00424             if (af->curr_out & af->tx_bit)
00425             {
00426                 /*
00427                  * Transmit a 1:
00428                  * - Stay on the previous tone
00429                  * - Increase bit stuff counter
00430                  */
00431                 af->stuff_cnt++;
00432             }
00433             else
00434             {
00435                 /*
00436                  * Transmit a 0:
00437                  * - Reset bit stuff counter
00438                  * - Switch tone
00439                  */
00440                 af->stuff_cnt = 0;
00441                 af->phase_inc = SWITCH_TONE(af->phase_inc);
00442             }
00443 
00444             /* Go to the next bit */
00445             af->tx_bit <<= 1;
00446         }
00447         af->sample_count = DAC_SAMPLEPERBIT;
00448     }
00449 
00450     /* Get new sample and put it out on the DAC */
00451     af->phase_acc += af->phase_inc;
00452     af->phase_acc %= SIN_LEN;
00453 
00454     af->sample_count--;
00455     AFSK_STROBE_OFF();
00456     return sin_sample(af->phase_acc);
00457 }
00458 
00459 
00460 static size_t afsk_read(KFile *fd, void *_buf, size_t size)
00461 {
00462     Afsk *af = AFSK_CAST(fd);
00463     uint8_t *buf = (uint8_t *)_buf;
00464 
00465     #if CONFIG_AFSK_RXTIMEOUT == 0
00466     while (size-- && !fifo_isempty_locked(&af->rx_fifo))
00467     #else
00468     while (size--)
00469     #endif
00470     {
00471         #if CONFIG_AFSK_RXTIMEOUT != -1
00472         ticks_t start = timer_clock();
00473         #endif
00474 
00475         while (fifo_isempty_locked(&af->rx_fifo))
00476         {
00477             cpu_relax();
00478             #if CONFIG_AFSK_RXTIMEOUT != -1
00479             if (timer_clock() - start > ms_to_ticks(CONFIG_AFSK_RXTIMEOUT))
00480                 return buf - (uint8_t *)_buf;
00481             #endif
00482         }
00483 
00484         *buf++ = fifo_pop_locked(&af->rx_fifo);
00485     }
00486 
00487     return buf - (uint8_t *)_buf;
00488 }
00489 
00490 static size_t afsk_write(KFile *fd, const void *_buf, size_t size)
00491 {
00492     Afsk *af = AFSK_CAST(fd);
00493     const uint8_t *buf = (const uint8_t *)_buf;
00494 
00495     while (size--)
00496     {
00497         while (fifo_isfull_locked(&af->tx_fifo))
00498             cpu_relax();
00499 
00500         fifo_push_locked(&af->tx_fifo, *buf++);
00501         afsk_txStart(af);
00502     }
00503 
00504     return buf - (const uint8_t *)_buf;
00505 }
00506 
00507 static int afsk_flush(KFile *fd)
00508 {
00509     Afsk *af = AFSK_CAST(fd);
00510     while (af->sending)
00511         cpu_relax();
00512     return 0;
00513 }
00514 
00515 static int afsk_error(KFile *fd)
00516 {
00517     Afsk *af = AFSK_CAST(fd);
00518     int err;
00519 
00520     ATOMIC(err = af->status);
00521     return err;
00522 }
00523 
00524 static void afsk_clearerr(KFile *fd)
00525 {
00526     Afsk *af = AFSK_CAST(fd);
00527     ATOMIC(af->status = 0);
00528 }
00529 
00530 
00537 void afsk_init(Afsk *af, int adc_ch, int dac_ch)
00538 {
00539     #if CONFIG_AFSK_RXTIMEOUT != -1
00540     MOD_CHECK(timer);
00541     #endif
00542     memset(af, 0, sizeof(*af));
00543     af->adc_ch = adc_ch;
00544     af->dac_ch = dac_ch;
00545 
00546     fifo_init(&af->delay_fifo, (uint8_t *)af->delay_buf, sizeof(af->delay_buf));
00547     fifo_init(&af->rx_fifo, af->rx_buf, sizeof(af->rx_buf));
00548 
00549     /* Fill sample FIFO with 0 */
00550     for (int i = 0; i < SAMPLEPERBIT / 2; i++)
00551         fifo_push(&af->delay_fifo, 0);
00552 
00553     fifo_init(&af->tx_fifo, af->tx_buf, sizeof(af->tx_buf));
00554 
00555     AFSK_ADC_INIT(adc_ch, af);
00556     AFSK_DAC_INIT(dac_ch, af);
00557     AFSK_STROBE_INIT();
00558     LOG_INFO("MARK_INC %d, SPACE_INC %d\n", MARK_INC, SPACE_INC);
00559 
00560     DB(af->fd._type = KFT_AFSK);
00561     af->fd.write = afsk_write;
00562     af->fd.read = afsk_read;
00563     af->fd.flush = afsk_flush;
00564     af->fd.error = afsk_error;
00565     af->fd.clearerr = afsk_clearerr;
00566     af->phase_inc = MARK_INC;
00567 }