From 378b7f7d5060fd58b9bf1ff518d84bc44c8dd086 Mon Sep 17 00:00:00 2001 From: drowe67 Date: Thu, 8 Sep 2016 22:17:11 +0000 Subject: [PATCH] ARM specific optimisations to fdmdv modem, thanks Danilo and mcHF team who tested carefully using a platform specific tfmdv framework git-svn-id: https://svn.code.sf.net/p/freetel/code@2850 01035d8c-6547-0410-b346-abe4f91aad63 --- codec2-dev/src/fdmdv.c | 72 ++++++++++++--------- codec2-dev/src/fdmdv_internal.h | 2 + codec2-dev/src/freedv_api.h | 71 +++++++++++---------- codec2-dev/src/octave.c | 110 ++++++++++++++++++++++++-------- 4 files changed, 165 insertions(+), 90 deletions(-) diff --git a/codec2-dev/src/fdmdv.c b/codec2-dev/src/fdmdv.c index e23bf878..3b3d9fad 100644 --- a/codec2-dev/src/fdmdv.c +++ b/codec2-dev/src/fdmdv.c @@ -37,6 +37,13 @@ #include #include +#ifdef ARM_MATH_CM4 + #include "stm32f4xx.h" + #include "core_cm4.h" + #include "arm_math.h" +#endif + + #include "fdmdv_internal.h" #include "codec2_fdmdv.h" #include "comp_prim.h" @@ -50,11 +57,21 @@ #include "machdep.h" static int sync_uw[] = {1,-1,1,-1,1,-1}; - #ifdef __EMBEDDED__ #define printf gdb_stdio_printf #endif +#ifndef ARM_MATH_CM4 + #define SINF(a) sinf(a) + #define COSF(a) cosf(a) +#else + #define SINF(a) arm_sin_f32(a) + #define COSF(a) arm_cos_f32(a) +#endif + +static const COMP pi_on_4 = { .70710678118654752439, .70710678118654752439 }; // COSF(PI/4) , SINF(PI/4) + + /*--------------------------------------------------------------------------* \ FUNCTION....: fdmdv_create @@ -110,8 +127,8 @@ struct FDMDV * fdmdv_create(int Nc) This helped PAPR for a few dB. We don't need to adjust rx phase as DQPSK takes care of that. */ - f->phase_tx[c].real = cosf(2.0*PI*c/(Nc+1)); - f->phase_tx[c].imag = sinf(2.0*PI*c/(Nc+1)); + f->phase_tx[c].real = COSF(2.0*PI*c/(Nc+1)); + f->phase_tx[c].imag = SINF(2.0*PI*c/(Nc+1)); f->phase_rx[c].real = 1.0; f->phase_rx[c].imag = 0.0; @@ -124,12 +141,12 @@ struct FDMDV * fdmdv_create(int Nc) f->prev_tx_symbols[Nc].real = 2.0; fdmdv_set_fsep(f, FSEP); - f->freq[Nc].real = cosf(2.0*PI*0.0/FS); - f->freq[Nc].imag = sinf(2.0*PI*0.0/FS); + f->freq[Nc].real = COSF(2.0*PI*0.0/FS); + f->freq[Nc].imag = SINF(2.0*PI*0.0/FS); f->freq_pol[Nc] = 2.0*PI*0.0/FS; - f->fbb_rect.real = cosf(2.0*PI*FDMDV_FCENTRE/FS); - f->fbb_rect.imag = sinf(2.0*PI*FDMDV_FCENTRE/FS); + f->fbb_rect.real = COSF(2.0*PI*FDMDV_FCENTRE/FS); + f->fbb_rect.imag = SINF(2.0*PI*FDMDV_FCENTRE/FS); f->fbb_pol = 2.0*PI*FDMDV_FCENTRE/FS; f->fbb_phase_tx.real = 1.0; f->fbb_phase_tx.imag = 0.0; @@ -257,15 +274,15 @@ void fdmdv_set_fsep(struct FDMDV *f, float fsep) { for(c=0; cNc/2; c++) { carrier_freq = (-f->Nc/2 + c)*f->fsep; - f->freq[c].real = cosf(2.0*PI*carrier_freq/FS); - f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS); + f->freq[c].real = COSF(2.0*PI*carrier_freq/FS); + f->freq[c].imag = SINF(2.0*PI*carrier_freq/FS); f->freq_pol[c] = 2.0*PI*carrier_freq/FS; } for(c=f->Nc/2; cNc; c++) { carrier_freq = (-f->Nc/2 + c + 1)*f->fsep; - f->freq[c].real = cosf(2.0*PI*carrier_freq/FS); - f->freq[c].imag = sinf(2.0*PI*carrier_freq/FS); + f->freq[c].real = COSF(2.0*PI*carrier_freq/FS); + f->freq[c].imag = SINF(2.0*PI*carrier_freq/FS); f->freq_pol[c] = 2.0*PI*carrier_freq/FS; } } @@ -678,8 +695,8 @@ void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq) } // create complex conjugate since we need this and only this later on - - for (f=0;f<4*M;f++) { + for (f=0;f<4*M;f++) + { pilot_lut[f] = cconj(pilot_lut[f]); } @@ -777,7 +794,10 @@ void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[], float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft) { - int i,j; + int i; +#ifndef ARM_MATH_CM4 + int j; +#endif COMP pilot[M+M/P]; COMP prev_pilot[M+M/P]; float foff, foff1, foff2; @@ -854,8 +874,8 @@ void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff, float mag; int i; - foff_rect.real = cosf(2.0*PI*foff/FS); - foff_rect.imag = sinf(2.0*PI*foff/FS); + foff_rect.real = COSF(2.0*PI*foff/FS); + foff_rect.imag = SINF(2.0*PI*foff/FS); for(i=0; i #include "comp.h" #define FREEDV_MODE_1600 0 @@ -56,51 +57,51 @@ typedef void (*freedv_calback_error_pattern) /* Protocol bits are packed MSB-first */ /* Called when a frame containing protocol data is decoded */ typedef void (*freedv_callback_protorx)(void *, char *); -/* Called when a frame containing protocol data is to be sent */ -typedef void (*freedv_callback_prototx)(void *, char *); - -/* Data packet callbacks */ -/* Called when a packet has been received */ -typedef void (*freedv_callback_datarx)(void *, unsigned char *packet, size_t size); -/* Called when a new packet can be send */ -typedef void (*freedv_callback_datatx)(void *, unsigned char *packet, size_t *size); - - -// FreeDV API functions: -// open, close -struct freedv *freedv_open(int mode); +/* Called when a frame containing protocol data is to be sent */ +typedef void (*freedv_callback_prototx)(void *, char *); + +/* Data packet callbacks */ +/* Called when a packet has been received */ +typedef void (*freedv_callback_datarx)(void *, unsigned char *packet, size_t size); +/* Called when a new packet can be send */ +typedef void (*freedv_callback_datatx)(void *, unsigned char *packet, size_t *size); + + +// FreeDV API functions: +// open, close +struct freedv *freedv_open(int mode); void freedv_close (struct freedv *freedv); // Transmit -void freedv_tx (struct freedv *freedv, short mod_out[], short speech_in[]); -void freedv_comptx (struct freedv *freedv, COMP mod_out[], short speech_in[]); -void freedv_codectx (struct freedv *f, short mod_out[], unsigned char *packed_codec_bits); -void freedv_datatx (struct freedv *f, short mod_out[]); -int freedv_data_ntxframes (struct freedv *freedv); -// Receive -int freedv_nin (struct freedv *freedv); -int freedv_rx (struct freedv *freedv, short speech_out[], short demod_in[]); +void freedv_tx (struct freedv *freedv, short mod_out[], short speech_in[]); +void freedv_comptx (struct freedv *freedv, COMP mod_out[], short speech_in[]); +void freedv_codectx (struct freedv *f, short mod_out[], unsigned char *packed_codec_bits); +void freedv_datatx (struct freedv *f, short mod_out[]); +int freedv_data_ntxframes (struct freedv *freedv); +// Receive +int freedv_nin (struct freedv *freedv); +int freedv_rx (struct freedv *freedv, short speech_out[], short demod_in[]); int freedv_floatrx (struct freedv *freedv, short speech_out[], float demod_in[]); int freedv_comprx (struct freedv *freedv, short speech_out[], COMP demod_in[]); int freedv_codecrx (struct freedv *freedv, unsigned char *packed_codec_bits, short demod_in[]); -// Set parameters -void freedv_set_callback_txt (struct freedv *freedv, freedv_callback_rx rx, freedv_callback_tx tx, void *callback_state); -void freedv_set_callback_protocol (struct freedv *freedv, freedv_callback_protorx rx, freedv_callback_prototx tx, void *callback_state); -void freedv_set_callback_data (struct freedv *freedv, freedv_callback_datarx datarx, freedv_callback_datatx datatx, void *callback_state); -void freedv_set_test_frames (struct freedv *freedv, int test_frames); -void freedv_set_smooth_symbols (struct freedv *freedv, int smooth_symbols); -void freedv_set_squelch_en (struct freedv *freedv, int squelch_en); +// Set parameters +void freedv_set_callback_txt (struct freedv *freedv, freedv_callback_rx rx, freedv_callback_tx tx, void *callback_state); +void freedv_set_callback_protocol (struct freedv *freedv, freedv_callback_protorx rx, freedv_callback_prototx tx, void *callback_state); +void freedv_set_callback_data (struct freedv *freedv, freedv_callback_datarx datarx, freedv_callback_datatx datatx, void *callback_state); +void freedv_set_test_frames (struct freedv *freedv, int test_frames); +void freedv_set_smooth_symbols (struct freedv *freedv, int smooth_symbols); +void freedv_set_squelch_en (struct freedv *freedv, int squelch_en); void freedv_set_snr_squelch_thresh (struct freedv *freedv, float snr_squelch_thresh); void freedv_set_clip (struct freedv *freedv, int val); void freedv_set_total_bit_errors (struct freedv *freedv, int val); -void freedv_set_total_bits (struct freedv *freedv, int val); -void freedv_set_callback_error_pattern (struct freedv *freedv, freedv_calback_error_pattern cb, void *state); -void freedv_set_varicode_code_num (struct freedv *freedv, int val); +void freedv_set_total_bits (struct freedv *freedv, int val); +void freedv_set_callback_error_pattern (struct freedv *freedv, freedv_calback_error_pattern cb, void *state); +void freedv_set_varicode_code_num (struct freedv *freedv, int val); void freedv_set_data_header (struct freedv *freedv, unsigned char *header); -int freedv_set_alt_modem_samp_rate (struct freedv *freedv, int samp_rate); - -// Get parameters +int freedv_set_alt_modem_samp_rate (struct freedv *freedv, int samp_rate); + +// Get parameters struct MODEM_STATS; -int freedv_get_version(void); +int freedv_get_version(void); int freedv_get_mode (struct freedv *freedv); void freedv_get_modem_stats (struct freedv *freedv, int *sync, float *snr_est); void freedv_get_modem_extended_stats(struct freedv *freedv, struct MODEM_STATS *stats); diff --git a/codec2-dev/src/octave.c b/codec2-dev/src/octave.c index e73ae1d1..b2259896 100644 --- a/codec2-dev/src/octave.c +++ b/codec2-dev/src/octave.c @@ -25,61 +25,119 @@ You should have received a copy of the GNU Lesser General Public License along with this program; if not, see . -*/ + */ #include +#include + #include "octave.h" +#ifdef ARM_MATH_CM4 +#include "Trace.h" +#endif + +#define OCTAVE_BUFSIZE 2048 + + +void flush_buffer(FILE* f, char* buffer,size_t* buf_idx_ptr) +{ +#ifdef ARM_MATH_CM4 + trace_write(buffer,*buf_idx_ptr); +#else + fwrite(buffer,*buf_idx_ptr,1,f); +#endif + *buf_idx_ptr = 0; +} + +void handle_buffer(FILE* f, char* buffer,const size_t max_buf, size_t* buf_idx_ptr, size_t l) +{ + *buf_idx_ptr += l; + if (*buf_idx_ptr > max_buf - 64) + { + flush_buffer(f, buffer,buf_idx_ptr); + } +} + +signed int printf_buffer(FILE* f, char* buffer,const size_t max_buf, size_t* buf_idx_ptr, const char *pFormat, ...) +{ + va_list ap; + signed int rc; + + va_start(ap, pFormat); + rc = vsnprintf(&buffer[*buf_idx_ptr], max_buf - *buf_idx_ptr, pFormat, ap); + va_end(ap); + if (rc>0) + { + handle_buffer(f, buffer,max_buf,buf_idx_ptr,rc); + } + return rc; +} + + +signed int printf_header(FILE* f, char* buffer,const size_t max_buf, size_t* buf_idx_ptr, const char *name, const char *dtype, int rows, int cols, int isFloat) +{ +#ifdef ARM_MATH_CM4 + printf_buffer(f, buffer, OCTAVE_BUFSIZE, buf_idx_ptr, "# hex: %s\n", isFloat?"true":"false"); +#endif + printf_buffer(f, buffer, OCTAVE_BUFSIZE, buf_idx_ptr, "# name: %s\n", name); + printf_buffer(f, buffer, OCTAVE_BUFSIZE, buf_idx_ptr, "# type: %s\n",dtype); + printf_buffer(f, buffer, OCTAVE_BUFSIZE, buf_idx_ptr, "# rows: %d\n", rows); + printf_buffer(f, buffer, OCTAVE_BUFSIZE, buf_idx_ptr, "# columns: %d\n", cols); +} void octave_save_int(FILE *f, char name[], int data[], int rows, int cols) { int r,c; + char buffer[OCTAVE_BUFSIZE]; + size_t buf_idx = 0; - fprintf(f, "# name: %s\n", name); - fprintf(f, "# type: matrix\n"); - fprintf(f, "# rows: %d\n", rows); - fprintf(f, "# columns: %d\n", cols); + printf_header(f, buffer, OCTAVE_BUFSIZE, &buf_idx, name, "matrix", rows, cols, 0); for(r=0; r