From f9164e29b5ad3b0465dd9ada080a34baf930a513 Mon Sep 17 00:00:00 2001 From: Ed Gonzalez Date: Thu, 11 Jun 2015 14:13:33 -0500 Subject: [PATCH] Encapsulate DSTAR into state machine that is fed incoming bits. --- DSP_API/SmartSDR_Interface/sched_waveform.c | 109 ++-------- DSP_API/ThumbDV/dstar.c | 221 ++++++++++++++++++++ DSP_API/ThumbDV/dstar.h | 49 ++++- 3 files changed, 281 insertions(+), 98 deletions(-) diff --git a/DSP_API/SmartSDR_Interface/sched_waveform.c b/DSP_API/SmartSDR_Interface/sched_waveform.c index 6cc23d1..b0ad575 100644 --- a/DSP_API/SmartSDR_Interface/sched_waveform.c +++ b/DSP_API/SmartSDR_Interface/sched_waveform.c @@ -207,8 +207,7 @@ static int _dv_serial_fd = 0; static GMSK_DEMOD _gmsk_demod = NULL; static GMSK_MOD _gmsk_mod = NULL; -static BIT_PM _syn_pm = NULL; -static BIT_PM _data_sync_pm = NULL; +static DSTAR_MACHINE _dstar = NULL; static void* _sched_waveform_thread(void* param) { @@ -367,83 +366,19 @@ static void* _sched_waveform_thread(void* param) float_in_24k[i + MEM_24] = cbReadFloat(RX1_cb); state = gmsk_decode(_gmsk_demod, float_in_24k[i+MEM_24]); - if ( !found_syn_bits ) { - if ( state == DEMOD_TRUE ) { - found_syn_bits = bitPM_addBit(_syn_pm, TRUE); - //output("%d ", 1); - } else if ( state == DEMOD_FALSE ) { - found_syn_bits = bitPM_addBit(_syn_pm, FALSE); - //output("%d ", 0); - } else { - //output("UNKNOWN DEMOD STATE"); - //bits[bit] = 0x00; - } - - if ( found_syn_bits ) { - output("FOUND SYN BITS\n"); - bitPM_reset(_syn_pm); - } - } else if ( header_count < 660 ){ - /* We have already found syn-bits accumulate 660 bits */ - if ( state == DEMOD_TRUE ) { - dstar_header[header_count++] = TRUE; - } else if ( state == DEMOD_FALSE ) { - dstar_header[header_count++] = FALSE; - } else { - /* Do nothing */ - } - - if ( header_count == 660 ) { - output("Found 660 bits - descrambling\n"); - /* Found 660 bits of header */ - unsigned char bytes[660/8 +1]; - - gmsk_bitsToBytes(dstar_header, bytes, 660); - thumbDV_dump("RAW:", bytes, 660/8); - - uint32 scramble_count = 0; - BOOL descrambled[660] = {0}; - dstar_scramble(dstar_header, descrambled, header_count, &scramble_count); - gmsk_bitsToBytes(descrambled, bytes, 660); - thumbDV_dump("DESCRAMBLE:", bytes, 660/8); - - - BOOL out[660] = {0}; - dstar_deinterleave(descrambled, out, 660); - gmsk_bitsToBytes(out, bytes, 660); - thumbDV_dump("DEINTERLEAVE:", bytes, 660/8); - dstar_fec fec; - memset(&fec, 0, sizeof(dstar_fec)); - unsigned int outLen = 660; - BOOL decoded[330] = {0}; - dstar_FECdecode(&fec, out, decoded, 660, &outLen); - output("outLen = %d\n" ,outLen); - gmsk_bitsToBytes(decoded, bytes, outLen); - thumbDV_dump("FEC: ", bytes, outLen/8); - - } - } else { - if ( state == DEMOD_TRUE ) { - found_data_sync_bits = bitPM_addBit(_data_sync_pm, TRUE); - //output("%d ", 1); - } else if ( state == DEMOD_FALSE ) { - found_data_sync_bits = bitPM_addBit(_data_sync_pm, FALSE); - //output("%d ", 0); - } else { - //output("UNKNOWN DEMOD STATE"); - //bits[bit] = 0x00; - } - - if ( found_data_sync_bits ) { - output("FOUND DATA SYNC BITS\n"); - bitPM_reset(_syn_pm); - header_count = 0; - found_syn_bits = FALSE; - } - } + unsigned char ambe_out[9] = {0}; + BOOL ambe_packet_out = FALSE; + if ( state == DEMOD_TRUE ) { + ambe_packet_out = dstar_stateMachine(_dstar, TRUE, ambe_out, 9); + } else if ( state == DEMOD_FALSE ) { + ambe_packet_out = dstar_stateMachine(_dstar, FALSE, ambe_out, 9); + } else { + /* Nothing to do since we have not "locked" a bit out yet */ + } } + fdmdv_24_to_8(float_out_8k, &float_in_24k[MEM_24], DV_PACKET_SAMPLES); for(i=0 ; i< DV_PACKET_SAMPLES ; i++) @@ -680,7 +615,6 @@ static void* _sched_waveform_thread(void* param) gmsk_destroyDemodulator(_gmsk_demod); gmsk_destroyModulator(_gmsk_mod); - bitPM_destroy(_syn_pm); return NULL; } @@ -690,27 +624,10 @@ void sched_waveform_Init(void) dstar_FECTest(); //exit(0); + _dstar = dstar_createMachine(); + _gmsk_demod = gmsk_createDemodulator(); _gmsk_mod = gmsk_createModulator(); - BOOL syn_bits[64 + 15] = {0}; - uint32 i = 0; - uint32 j = 0; - for ( i = 0 ; i < 64 -1 ; i += 2 ) { - syn_bits[i] = TRUE; - syn_bits[i+1] = FALSE; - } - - BOOL frame_bits[] = {TRUE, TRUE, TRUE, FALSE, TRUE, TRUE, FALSE, FALSE, - TRUE, FALSE, TRUE, FALSE, FALSE, FALSE, FALSE}; - - for ( i = 64, j = 0 ; i < 64 + 15 ; i++,j++ ) { - syn_bits[i] = frame_bits[j]; - } - - _syn_pm = bitPM_create( syn_bits, 64+15); - _data_sync_pm = bitPM_create( DATA_SYNC_BITS, 24); - - pthread_rwlock_init(&_list_lock, NULL); diff --git a/DSP_API/ThumbDV/dstar.c b/DSP_API/ThumbDV/dstar.c index fc01d71..160c419 100644 --- a/DSP_API/ThumbDV/dstar.c +++ b/DSP_API/ThumbDV/dstar.c @@ -407,6 +407,227 @@ void dstar_FECencode(const BOOL * in, BOOL * out, unsigned int inLen, unsigned i } } } + +DSTAR_MACHINE dstar_createMachine(void) +{ + DSTAR_MACHINE machine = safe_malloc(sizeof(dstar_machine)); + memset(machine, 0, sizeof(dstar_machine)); + + machine->state = BIT_FRAME_SYNC_WAIT; + + BOOL syn_bits[64 + 15] = {0}; + uint32 i = 0; + uint32 j = 0; + for ( i = 0 ; i < 64 -1 ; i += 2 ) { + syn_bits[i] = TRUE; + syn_bits[i+1] = FALSE; + } + + BOOL frame_bits[] = {TRUE, TRUE, TRUE, FALSE, TRUE, TRUE, FALSE, FALSE, + TRUE, FALSE, TRUE, FALSE, FALSE, FALSE, FALSE}; + + for ( i = 64, j = 0 ; i < 64 + 15 ; i++,j++ ) { + syn_bits[i] = frame_bits[j]; + } + + machine->syn_pm = bitPM_create( syn_bits, 64+15); + machine->data_sync_pm = bitPM_create( DATA_SYNC_BITS, 24); + machine->end_pm = bitPM_create(END_PATTERN_BITS, END_PATTERN_LENGTH_BITS); + + return machine; +} + +void dstar_destroyMachine(DSTAR_MACHINE machine) +{ + bitPM_destroy(machine->syn_pm); + bitPM_destroy(machine->data_sync_pm); + bitPM_destroy(machine->end_pm); + + safe_free(machine); +} + +BOOL dstar_stateMachine(DSTAR_MACHINE machine, BOOL in_bit, unsigned char * ambe_out, uint32 ambe_buf_len) +{ + BOOL have_audio_packet = FALSE; + BOOL found_syn_bits = FALSE; + BOOL found_end_bits = FALSE; + static BOOL header[FEC_SECTION_LENGTH_BITS]; + static BOOL voice_bits[72]; + static BOOL data_bits[24]; + + unsigned char bytes[FEC_SECTION_LENGTH_BITS/8 + 1]; + + switch(machine->state) + { + case BIT_FRAME_SYNC_WAIT: + found_syn_bits = bitPM_addBit(machine->syn_pm, in_bit); + + if ( found_syn_bits ) { + output("FOUND SYN BITS\n"); + bitPM_reset(machine->syn_pm); + machine->state = HEADER_PROCESSING; + machine->bit_count = 0; + } + break; + case HEADER_PROCESSING: + header[machine->bit_count++] = in_bit; + + if ( machine->bit_count == FEC_SECTION_LENGTH_BITS ) { + output("Found 660 bits - descrambling\n"); + /* Found 660 bits of header */ + + gmsk_bitsToBytes(header, bytes, FEC_SECTION_LENGTH_BITS); + thumbDV_dump("RAW:", bytes, FEC_SECTION_LENGTH_BITS/8); + + + uint32 scramble_count = 0; + BOOL descrambled[FEC_SECTION_LENGTH_BITS] = {0}; + dstar_scramble(header, descrambled, FEC_SECTION_LENGTH_BITS, &scramble_count); + gmsk_bitsToBytes(descrambled, bytes, FEC_SECTION_LENGTH_BITS); + thumbDV_dump("DESCRAMBLE:", bytes, FEC_SECTION_LENGTH_BITS/8); + + + BOOL out[FEC_SECTION_LENGTH_BITS] = {0}; + dstar_deinterleave(descrambled, out, FEC_SECTION_LENGTH_BITS); + gmsk_bitsToBytes(out, bytes, FEC_SECTION_LENGTH_BITS); + thumbDV_dump("DEINTERLEAVE:", bytes, FEC_SECTION_LENGTH_BITS/8); + dstar_fec fec; + memset(&fec, 0, sizeof(dstar_fec)); + unsigned int outLen = FEC_SECTION_LENGTH_BITS; + BOOL decoded[FEC_SECTION_LENGTH_BITS / 2] = {0}; + dstar_FECdecode(&fec, out, decoded, FEC_SECTION_LENGTH_BITS, &outLen); + output("outLen = %d\n" ,outLen); + gmsk_bitsToBytes(decoded, bytes, outLen); + thumbDV_dump("FEC: ", bytes, outLen/8); + + + /* STATE CHANGE */ + machine->state = VOICE_FRAME; + machine->bit_count = 0; + machine->frame_count = 0; + } + + break; + case VOICE_FRAME: + voice_bits[machine->bit_count++] = in_bit; + + if ( machine->bit_count == VOICE_FRAME_LENGTH_BITS ) { + gmsk_bitsToBytes(voice_bits, bytes, VOICE_FRAME_LENGTH_BITS); + thumbDV_dump("Voice Frame:", bytes, VOICE_FRAME_LENGTH_BITS / 8); + memcpy(ambe_out, bytes, VOICE_FRAME_LENGTH_BITS / 8); + have_audio_packet = TRUE; + + /* STATE CHANGE */ + if ( machine->frame_count % 21 == 0 ) { + /* Expecting a SYNC FRAME */ + machine->state = DATA_SYNC_FRAME; + } else { + machine->state = DATA_FRAME; + } + machine->bit_count = 0; + } + + break; + case DATA_FRAME: + data_bits[machine->bit_count++] = in_bit; + + found_end_bits = bitPM_addBit(machine->end_pm, in_bit); + + if ( found_end_bits ) { + machine->state = END_PATTERN_FOUND; + machine->bit_count = 0; + } else if ( machine->bit_count == DATA_FRAME_LENGTH_BITS ) { + gmsk_bitsToBytes(data_bits, bytes, DATA_FRAME_LENGTH_BITS); + thumbDV_dump("Data Frame:", bytes, DATA_FRAME_LENGTH_BITS/8); + + machine->frame_count++; + + /* STATE CHANGE */ + machine->state = VOICE_FRAME; + machine->bit_count = 0; + } + break; + case DATA_SYNC_FRAME: { + BOOL found_sync = FALSE; + found_sync = bitPM_addBit(machine->data_sync_pm, in_bit); + machine->bit_count++; + + found_end_bits = bitPM_addBit(machine->end_pm, in_bit); + + if ( found_sync ) { + output("Found Sync\n"); + + machine->frame_count++; + + bitPM_reset(machine->data_sync_pm); + /* STATE CHANGE */ + machine->state = VOICE_FRAME; + machine->bit_count = 0; + } else if ( found_end_bits ) { + machine->state = END_PATTERN_FOUND; + machine->bit_count = 0; + } else if ( machine->bit_count > 25 * 2 ) { + /* Function as a timeout if we don't find the sync bits */ + output("Could not find SYNC\n"); + + bitPM_reset(machine->data_sync_pm); + + /* STATE CHANGE */ + machine->state = BIT_FRAME_SYNC_WAIT; + machine->bit_count = 0; + } + + break; + } + case END_PATTERN_FOUND: + + output("Found end patter bits\n"); + bitPM_reset(machine->end_pm); + bitPM_reset(machine->syn_pm); + + /* STATE CHANGE */ + machine->state = BIT_FRAME_SYNC_WAIT; + machine->bit_count = 0; + + break; + default: + output(ANSI_YELLOW "Unhandled state - dstar_stateMachine. State = 0x%08X" ANSI_WHITE, machine->state); + break; + } + + return have_audio_packet; +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + void dstar_FECTest(void) { diff --git a/DSP_API/ThumbDV/dstar.h b/DSP_API/ThumbDV/dstar.h index 8f0eddd..8f4fdc0 100644 --- a/DSP_API/ThumbDV/dstar.h +++ b/DSP_API/ThumbDV/dstar.h @@ -30,6 +30,45 @@ #ifndef THUMBDV_DSTAR_H_ #define THUMBDV_DSTAR_H_ + +enum DSTAR_STATE +{ + BIT_FRAME_SYNC_WAIT = 0x1, + HEADER_PROCESSING, + VOICE_FRAME, + DATA_FRAME, + DATA_SYNC_FRAME, + END_PATTERN_FOUND +}; + +typedef struct _dstar_header +{ + unsigned char flag1; + unsigned char flag2; + unsigned char flag3; + unsigned char destination_rptr[8]; + unsigned char departure_rptr[8]; + unsigned char companion_call[8]; + unsigned char own_call1[8]; + unsigned char own_call2[4]; + uint16 p_fcs; +} dstar_header, * DSTAR_HEADER; + +typedef struct _dstar_machine +{ + enum DSTAR_STATE state; + dstar_header incoming_header; + dstar_header outgoing_header; + + uint32 bit_count; + uint32 frame_count; + + BIT_PM syn_pm; + BIT_PM data_sync_pm; + BIT_PM end_pm; + +} dstar_machine, * DSTAR_MACHINE; + typedef struct _dstar_fec { BOOL mem0[330]; @@ -38,9 +77,15 @@ typedef struct _dstar_fec BOOL mem3[330]; int metric[4]; } dstar_fec, * DSTAR_FEC; + + +DSTAR_MACHINE dstar_createMachine(void); +void dstar_destroyMachine(DSTAR_MACHINE machine); +BOOL dstar_stateMachine(DSTAR_MACHINE machine, BOOL in_bit, unsigned char * ambe_out, uint32 ambe_buf_len); + + + void dstar_FECTest(void); - - void dstar_scramble(BOOL * in, BOOL * out, uint32 length, uint32 * scramble_count); void dstar_interleave(const BOOL * in, BOOL * out, unsigned int length); void dstar_deinterleave(const BOOL * in, BOOL * out, unsigned int length);