BeRTOS
|
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 }