From ac7c48b4dee99d4c772f133d70d8d1b38262fcd2 Mon Sep 17 00:00:00 2001
From: Author Name <david@rowetel.com>
Date: Fri, 7 Jul 2023 12:20:59 +0930
Subject: shallow zip-file copy from codec2 e9d726bf20

---
 src/fdmdv.c | 2082 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 2082 insertions(+)
 create mode 100644 src/fdmdv.c

(limited to 'src/fdmdv.c')

diff --git a/src/fdmdv.c b/src/fdmdv.c
new file mode 100644
index 0000000..7aeb4b8
--- /dev/null
+++ b/src/fdmdv.c
@@ -0,0 +1,2082 @@
+/*---------------------------------------------------------------------------*\
+
+  FILE........: fdmdv.c
+  AUTHOR......: David Rowe
+  DATE CREATED: April 14 2012
+
+  Functions that implement the FDMDV modem.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+  Copyright (C) 2012 David Rowe
+
+  All rights reserved.
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License version 2.1, as
+  published by the Free Software Foundation.  This program is
+  distributed in the hope that it will be useful, but WITHOUT ANY
+  WARRANTY; without even the implied warranty of MERCHANTABILITY or
+  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
+  License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with this program; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+/*---------------------------------------------------------------------------*\
+
+                               INCLUDES
+
+\*---------------------------------------------------------------------------*/
+
+#include <assert.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include "fdmdv_internal.h"
+#include "codec2_fdmdv.h"
+#include "comp_prim.h"
+#include "rn.h"
+#include "rxdec_coeff.h"
+#include "test_bits.h"
+#include "pilot_coeff.h"
+#include "codec2_fft.h"
+#include "hanning.h"
+#include "os.h"
+#include "machdep.h"
+
+#include "debug_alloc.h"
+
+static int sync_uw[] = {1,-1,1,-1,1,-1};
+
+static const COMP  pi_on_4 = { .70710678118654752439, .70710678118654752439 }; // cosf(PI/4) , sinf(PI/4)
+
+
+/*--------------------------------------------------------------------------* \
+
+  FUNCTION....: fdmdv_create
+  AUTHOR......: David Rowe
+  DATE CREATED: 16/4/2012
+
+  Create and initialise an instance of the modem.  Returns a pointer
+  to the modem states or NULL on failure.  One set of states is
+  sufficient for a full duplex modem.
+
+\*---------------------------------------------------------------------------*/
+
+struct FDMDV * fdmdv_create(int Nc)
+{
+    struct FDMDV *f;
+    int           c, i, k;
+
+    assert(NC == FDMDV_NC_MAX);  /* check public and private #defines match */
+    assert(Nc <= NC);
+    assert(FDMDV_NOM_SAMPLES_PER_FRAME == M_FAC);
+    assert(FDMDV_MAX_SAMPLES_PER_FRAME == (M_FAC+M_FAC/P));
+
+    f = (struct FDMDV*)MALLOC(sizeof(struct FDMDV));
+    if (f == NULL)
+	return NULL;
+
+    f->Nc = Nc;
+
+    f->ntest_bits = Nc*NB*4;
+    f->current_test_bit = 0;
+    f->rx_test_bits_mem = (int*)MALLOC(sizeof(int)*f->ntest_bits);
+    assert(f->rx_test_bits_mem != NULL);
+    for(i=0; i<f->ntest_bits; i++)
+	f->rx_test_bits_mem[i] = 0;
+    assert((sizeof(test_bits)/sizeof(int)) >= f->ntest_bits);
+
+    f->old_qpsk_mapping = 0;
+
+    f->tx_pilot_bit = 0;
+
+    for(c=0; c<Nc+1; c++) {
+	f->prev_tx_symbols[c].real = 1.0;
+	f->prev_tx_symbols[c].imag = 0.0;
+	f->prev_rx_symbols[c].real = 1.0;
+	f->prev_rx_symbols[c].imag = 0.0;
+
+	for(k=0; k<NSYM; k++) {
+	    f->tx_filter_memory[c][k].real = 0.0;
+	    f->tx_filter_memory[c][k].imag = 0.0;
+	}
+
+	/* Spread initial FDM carrier phase out as far as possible.
+           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_rx[c].real = 1.0;
+ 	f->phase_rx[c].imag = 0.0;
+
+	for(k=0; k<NT*P; k++) {
+	    f->rx_filter_mem_timing[c][k].real = 0.0;
+	    f->rx_filter_mem_timing[c][k].imag = 0.0;
+	}
+    }
+    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_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_pol           = 2.0*PI*FDMDV_FCENTRE/FS;
+    f->fbb_phase_tx.real = 1.0;
+    f->fbb_phase_tx.imag = 0.0;
+    f->fbb_phase_rx.real = 1.0;
+    f->fbb_phase_rx.imag = 0.0;
+
+    /* Generate DBPSK pilot Look Up Table (LUT) */
+
+    generate_pilot_lut(f->pilot_lut, &f->freq[Nc]);
+
+    /* freq Offset estimation states */
+
+    f->fft_pilot_cfg = codec2_fft_alloc (MPILOTFFT, 0, NULL, NULL);
+    assert(f->fft_pilot_cfg != NULL);
+
+    for(i=0; i<NPILOTBASEBAND; i++) {
+	f->pilot_baseband1[i].real = f->pilot_baseband2[i].real = 0.0;
+	f->pilot_baseband1[i].imag = f->pilot_baseband2[i].imag = 0.0;
+    }
+    f->pilot_lut_index = 0;
+    f->prev_pilot_lut_index = 3*M_FAC;
+
+    for(i=0; i<NRXDECMEM; i++) {
+        f->rxdec_lpf_mem[i].real = 0.0;
+        f->rxdec_lpf_mem[i].imag = 0.0;
+    }
+
+    for(i=0; i<NPILOTLPF; i++) {
+	f->pilot_lpf1[i].real = f->pilot_lpf2[i].real = 0.0;
+	f->pilot_lpf1[i].imag = f->pilot_lpf2[i].imag = 0.0;
+    }
+
+    f->foff = 0.0;
+    f->foff_phase_rect.real = 1.0;
+    f->foff_phase_rect.imag = 0.0;
+
+    for(i=0; i<NRX_FDM_MEM; i++) {
+        f->rx_fdm_mem[i].real = 0.0;
+        f->rx_fdm_mem[i].imag = 0.0;
+    }
+
+    f->fest_state = 0;
+    f->sync = 0;
+    f->timer = 0;
+    for(i=0; i<NSYNC_MEM; i++)
+        f->sync_mem[i] = 0;
+
+    for(c=0; c<Nc+1; c++) {
+	f->sig_est[c] = 0.0;
+	f->noise_est[c] = 0.0;
+    }
+
+    f->sig_pwr_av = 0.0;
+    f->foff_filt = 0.0;
+
+    return f;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_destroy
+  AUTHOR......: David Rowe
+  DATE CREATED: 16/4/2012
+
+  Destroy an instance of the modem.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_destroy(struct FDMDV *fdmdv)
+{
+    assert(fdmdv != NULL);
+    codec2_fft_free(fdmdv->fft_pilot_cfg);
+    FREE(fdmdv->rx_test_bits_mem);
+    FREE(fdmdv);
+}
+
+
+void fdmdv_use_old_qpsk_mapping(struct FDMDV *fdmdv) {
+    fdmdv->old_qpsk_mapping = 1;
+}
+
+
+int fdmdv_bits_per_frame(struct FDMDV *fdmdv)
+{
+    return (fdmdv->Nc * NB);
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_get_test_bits()
+  AUTHOR......: David Rowe
+  DATE CREATED: 16/4/2012
+
+  Generate a frame of bits from a repeating sequence of random data.  OK so
+  it's not very random if it repeats but it makes syncing at the demod easier
+  for test purposes.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_get_test_bits(struct FDMDV *f, int tx_bits[])
+{
+    int i;
+    int bits_per_frame = fdmdv_bits_per_frame(f);
+
+    for(i=0; i<bits_per_frame; i++) {
+	tx_bits[i] = test_bits[f->current_test_bit];
+	f->current_test_bit++;
+	if (f->current_test_bit > (f->ntest_bits-1))
+	    f->current_test_bit = 0;
+    }
+}
+
+float fdmdv_get_fsep(struct FDMDV *f)
+{
+    return f->fsep;
+}
+
+void fdmdv_set_fsep(struct FDMDV *f, float fsep) {
+    int   c;
+    float carrier_freq;
+
+    f->fsep = fsep;
+
+    /* Set up frequency of each carrier */
+
+    for(c=0; c<f->Nc/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_pol[c]  = 2.0*PI*carrier_freq/FS;
+    }
+
+    for(c=f->Nc/2; c<f->Nc; 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_pol[c]  = 2.0*PI*carrier_freq/FS;
+    }
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: bits_to_dqpsk_symbols()
+  AUTHOR......: David Rowe
+  DATE CREATED: 16/4/2012
+
+  Maps bits to parallel DQPSK symbols. Generate Nc+1 QPSK symbols from
+  vector of (1,Nc*Nb) input tx_bits.  The Nc+1 symbol is the +1 -1 +1
+  .... BPSK sync carrier.
+
+\*---------------------------------------------------------------------------*/
+
+void bits_to_dqpsk_symbols(COMP tx_symbols[], int Nc, COMP prev_tx_symbols[], int tx_bits[], int *pilot_bit, int old_qpsk_mapping)
+{
+    int c, msb, lsb;
+    COMP j = {0.0,1.0};
+
+    /* Map tx_bits to to Nc DQPSK symbols.  Note legacy support for
+       old (suboptimal) V0.91 FreeDV mapping */
+
+    for(c=0; c<Nc; c++) {
+	msb = tx_bits[2*c];
+	lsb = tx_bits[2*c+1];
+	if ((msb == 0) && (lsb == 0))
+	    tx_symbols[c] = prev_tx_symbols[c];
+	if ((msb == 0) && (lsb == 1))
+            tx_symbols[c] = cmult(j, prev_tx_symbols[c]);
+	if ((msb == 1) && (lsb == 0)) {
+	    if (old_qpsk_mapping)
+                tx_symbols[c] = cneg(prev_tx_symbols[c]);
+            else
+                tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]);
+        }
+	if ((msb == 1) && (lsb == 1)) {
+	    if (old_qpsk_mapping)
+                tx_symbols[c] = cmult(cneg(j),prev_tx_symbols[c]);
+            else
+                tx_symbols[c] = cneg(prev_tx_symbols[c]);
+        }
+    }
+
+    /* +1 -1 +1 -1 BPSK sync carrier, once filtered becomes (roughly)
+       two spectral lines at +/- Rs/2 */
+
+    if (*pilot_bit)
+	tx_symbols[Nc] = cneg(prev_tx_symbols[Nc]);
+    else
+	tx_symbols[Nc] = prev_tx_symbols[Nc];
+
+    if (*pilot_bit)
+	*pilot_bit = 0;
+    else
+	*pilot_bit = 1;
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: tx_filter()
+  AUTHOR......: David Rowe
+  DATE CREATED: 17/4/2012
+
+  Given Nc*NB bits construct M_FAC samples (1 symbol) of Nc+1 filtered
+  symbols streams.
+
+\*---------------------------------------------------------------------------*/
+
+void tx_filter(COMP tx_baseband[NC+1][M_FAC], int Nc, COMP tx_symbols[], COMP tx_filter_memory[NC+1][NSYM])
+{
+    int     c;
+    int     i,j,k;
+    float   acc;
+    COMP    gain;
+
+    gain.real = sqrtf(2.0)/2.0;
+    gain.imag = 0.0;
+
+    for(c=0; c<Nc+1; c++)
+	tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain);
+
+    /*
+       tx filter each symbol, generate M_FAC filtered output samples for each symbol.
+       Efficient polyphase filter techniques used as tx_filter_memory is sparse
+    */
+
+    for(i=0; i<M_FAC; i++) {
+	for(c=0; c<Nc+1; c++) {
+
+	    /* filter real sample of symbol for carrier c */
+
+	    acc = 0.0;
+	    for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
+		acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k];
+	    tx_baseband[c][i].real = acc;
+
+	    /* filter imag sample of symbol for carrier c */
+
+	    acc = 0.0;
+	    for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
+		acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k];
+	    tx_baseband[c][i].imag = acc;
+
+	}
+    }
+
+    /* shift memory, inserting zeros at end */
+
+    for(i=0; i<NSYM-1; i++)
+	for(c=0; c<Nc+1; c++)
+	    tx_filter_memory[c][i] = tx_filter_memory[c][i+1];
+
+    for(c=0; c<Nc+1; c++) {
+	tx_filter_memory[c][NSYM-1].real = 0.0;
+	tx_filter_memory[c][NSYM-1].imag = 0.0;
+    }
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: tx_filter_and_upconvert()
+  AUTHOR......: David Rowe
+  DATE CREATED: 13 August 2014
+
+  Given Nc symbols construct M_FAC samples (1 symbol) of Nc+1 filtered
+  and upconverted symbols.
+
+\*---------------------------------------------------------------------------*/
+
+void tx_filter_and_upconvert(COMP tx_fdm[], int Nc, COMP tx_symbols[],
+                             COMP tx_filter_memory[NC+1][NSYM],
+                             COMP phase_tx[], COMP freq[],
+                             COMP *fbb_phase, COMP fbb_rect)
+{
+    int     c;
+    int     i,j,k;
+    float   acc;
+    COMP    gain;
+    COMP    tx_baseband;
+    COMP  two = {2.0, 0.0};
+    float mag;
+
+    gain.real = sqrtf(2.0)/2.0;
+    gain.imag = 0.0;
+
+    for(i=0; i<M_FAC; i++) {
+	tx_fdm[i].real = 0.0;
+	tx_fdm[i].imag = 0.0;
+    }
+
+    for(c=0; c<Nc+1; c++)
+	tx_filter_memory[c][NSYM-1] = cmult(tx_symbols[c], gain);
+
+    /*
+       tx filter each symbol, generate M_FAC filtered output samples for
+       each symbol, which we then freq shift and sum with other
+       carriers.  Efficient polyphase filter techniques used as
+       tx_filter_memory is sparse
+    */
+
+    for(c=0; c<Nc+1; c++) {
+        for(i=0; i<M_FAC; i++) {
+
+	    /* filter real sample of symbol for carrier c */
+
+	    acc = 0.0;
+	    for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
+		acc += M_FAC * tx_filter_memory[c][j].real * gt_alpha5_root[k];
+	    tx_baseband.real = acc;
+
+	    /* filter imag sample of symbol for carrier c */
+
+	    acc = 0.0;
+	    for(j=0,k=M_FAC-i-1; j<NSYM; j++,k+=M_FAC)
+		acc += M_FAC * tx_filter_memory[c][j].imag * gt_alpha5_root[k];
+	    tx_baseband.imag = acc;
+
+            /* freq shift and sum */
+
+	    phase_tx[c] = cmult(phase_tx[c], freq[c]);
+	    tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband, phase_tx[c]));
+	}
+    }
+
+    /* shift whole thing up to carrier freq */
+
+    for (i=0; i<M_FAC; i++) {
+	*fbb_phase = cmult(*fbb_phase, fbb_rect);
+	tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase);
+    }
+
+    /*
+      Scale such that total Carrier power C of real(tx_fdm) = Nc.  This
+      excludes the power of the pilot tone.
+      We return the complex (single sided) signal to make frequency
+      shifting for the purpose of testing easier
+    */
+
+    for (i=0; i<M_FAC; i++)
+	tx_fdm[i] = cmult(two, tx_fdm[i]);
+
+    /* normalise digital oscillators as the magnitude can drift over time */
+
+    for (c=0; c<Nc+1; c++) {
+        mag = cabsolute(phase_tx[c]);
+	phase_tx[c].real /= mag;
+	phase_tx[c].imag /= mag;
+    }
+
+    mag = cabsolute(*fbb_phase);
+    fbb_phase->real /= mag;
+    fbb_phase->imag /= mag;
+
+    /* shift memory, inserting zeros at end */
+
+    for(i=0; i<NSYM-1; i++)
+	for(c=0; c<Nc+1; c++)
+	    tx_filter_memory[c][i] = tx_filter_memory[c][i+1];
+
+    for(c=0; c<Nc+1; c++) {
+	tx_filter_memory[c][NSYM-1].real = 0.0;
+	tx_filter_memory[c][NSYM-1].imag = 0.0;
+    }
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdm_upconvert()
+  AUTHOR......: David Rowe
+  DATE CREATED: 17/4/2012
+
+  Construct FDM signal by frequency shifting each filtered symbol
+  stream.  Returns complex signal so we can apply frequency offsets
+  easily.
+
+\*---------------------------------------------------------------------------*/
+
+void fdm_upconvert(COMP tx_fdm[], int Nc, COMP tx_baseband[NC+1][M_FAC], COMP phase_tx[], COMP freq[],
+                   COMP *fbb_phase, COMP fbb_rect)
+{
+    int   i,c;
+    COMP  two = {2.0, 0.0};
+    float mag;
+
+    for(i=0; i<M_FAC; i++) {
+	tx_fdm[i].real = 0.0;
+	tx_fdm[i].imag = 0.0;
+    }
+
+    for (c=0; c<=Nc; c++)
+	for (i=0; i<M_FAC; i++) {
+	    phase_tx[c] = cmult(phase_tx[c], freq[c]);
+	    tx_fdm[i] = cadd(tx_fdm[i], cmult(tx_baseband[c][i], phase_tx[c]));
+	}
+
+    /* shift whole thing up to carrier freq */
+
+    for (i=0; i<M_FAC; i++) {
+	*fbb_phase = cmult(*fbb_phase, fbb_rect);
+	tx_fdm[i] = cmult(tx_fdm[i], *fbb_phase);
+    }
+
+    /*
+      Scale such that total Carrier power C of real(tx_fdm) = Nc.  This
+      excludes the power of the pilot tone.
+      We return the complex (single sided) signal to make frequency
+      shifting for the purpose of testing easier
+    */
+
+    for (i=0; i<M_FAC; i++)
+	tx_fdm[i] = cmult(two, tx_fdm[i]);
+
+    /* normalise digital oscilators as the magnitude can drift over time */
+
+    for (c=0; c<Nc+1; c++) {
+        mag = cabsolute(phase_tx[c]);
+	phase_tx[c].real /= mag;
+	phase_tx[c].imag /= mag;
+    }
+
+    mag = cabsolute(*fbb_phase);
+    fbb_phase->real /= mag;
+    fbb_phase->imag /= mag;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_mod()
+  AUTHOR......: David Rowe
+  DATE CREATED: 26/4/2012
+
+  FDMDV modulator, take a frame of FDMDV_BITS_PER_FRAME bits and
+  generates a frame of FDMDV_SAMPLES_PER_FRAME modulated symbols.
+  Sync bit is returned to aid alignment of your next frame.
+
+  The sync_bit value returned will be used for the _next_ frame.
+
+  The output signal is complex to support single sided frequency
+  shifting, for example when testing frequency offsets in channel
+  simulation.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_mod(struct FDMDV *fdmdv, COMP tx_fdm[], int tx_bits[], int *sync_bit)
+{
+    COMP          tx_symbols[NC+1];
+    PROFILE_VAR(mod_start, tx_filter_and_upconvert_start);
+
+    PROFILE_SAMPLE(mod_start);
+    bits_to_dqpsk_symbols(tx_symbols, fdmdv->Nc, fdmdv->prev_tx_symbols, tx_bits, &fdmdv->tx_pilot_bit, fdmdv->old_qpsk_mapping);
+    memcpy(fdmdv->prev_tx_symbols, tx_symbols, sizeof(COMP)*(fdmdv->Nc+1));
+    PROFILE_SAMPLE_AND_LOG(tx_filter_and_upconvert_start, mod_start, "    bits_to_dqpsk_symbols");
+    tx_filter_and_upconvert(tx_fdm, fdmdv->Nc, tx_symbols, fdmdv->tx_filter_memory,
+                            fdmdv->phase_tx, fdmdv->freq, &fdmdv->fbb_phase_tx, fdmdv->fbb_rect);
+    PROFILE_SAMPLE_AND_LOG2(tx_filter_and_upconvert_start, "    tx_filter_and_upconvert");
+
+    *sync_bit = fdmdv->tx_pilot_bit;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: generate_pilot_fdm()
+  AUTHOR......: David Rowe
+  DATE CREATED: 19/4/2012
+
+  Generate M_FAC samples of DBPSK pilot signal for Freq offset estimation.
+
+\*---------------------------------------------------------------------------*/
+
+void generate_pilot_fdm(COMP *pilot_fdm, int *bit, float *symbol,
+			float *filter_mem, COMP *phase, COMP *freq)
+{
+    int   i,j,k;
+    float tx_baseband[M_FAC];
+
+    /* +1 -1 +1 -1 DBPSK sync carrier, once filtered becomes (roughly)
+       two spectral lines at +/- RS/2 */
+
+    if (*bit)
+	*symbol = -*symbol;
+
+    if (*bit)
+	*bit = 0;
+    else
+	*bit = 1;
+
+    /* filter DPSK symbol to create M_FAC baseband samples */
+
+    filter_mem[NFILTER-1] = (sqrtf(2)/2) * *symbol;
+    for(i=0; i<M_FAC; i++) {
+	tx_baseband[i] = 0.0;
+	for(j=M_FAC-1,k=M_FAC-i-1; j<NFILTER; j+=M_FAC,k+=M_FAC)
+	    tx_baseband[i] += M_FAC * filter_mem[j] * gt_alpha5_root[k];
+    }
+
+    /* shift memory, inserting zeros at end */
+
+    for(i=0; i<NFILTER-M_FAC; i++)
+	filter_mem[i] = filter_mem[i+M_FAC];
+
+    for(i=NFILTER-M_FAC; i<NFILTER; i++)
+	filter_mem[i] = 0.0;
+
+    /* upconvert */
+
+    for(i=0; i<M_FAC; i++) {
+	*phase = cmult(*phase, *freq);
+	pilot_fdm[i].real = sqrtf(2)*2*tx_baseband[i] * phase->real;
+	pilot_fdm[i].imag = sqrtf(2)*2*tx_baseband[i] * phase->imag;
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: generate_pilot_lut()
+  AUTHOR......: David Rowe
+  DATE CREATED: 19/4/2012
+
+  Generate a 4M sample vector of DBPSK pilot signal.  As the pilot signal
+  is periodic in 4M samples we can then use this vector as a look up table
+  for pilot signal generation in the demod.
+
+\*---------------------------------------------------------------------------*/
+
+void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq)
+{
+    int   pilot_rx_bit = 0;
+    float pilot_symbol = sqrtf(2.0);
+    COMP  pilot_phase  = {1.0, 0.0};
+    float pilot_filter_mem[NFILTER];
+    COMP  pilot[M_FAC];
+    int   i,f;
+
+    for(i=0; i<NFILTER; i++)
+	pilot_filter_mem[i] = 0.0;
+
+    /* discard first 4 symbols as filter memory is filling, just keep
+       last four symbols */
+
+    for(f=0; f<8; f++) {
+	generate_pilot_fdm(pilot, &pilot_rx_bit, &pilot_symbol, pilot_filter_mem, &pilot_phase, pilot_freq);
+	if (f >= 4)
+	    memcpy(&pilot_lut[M_FAC*(f-4)], pilot, M_FAC*sizeof(COMP));
+    }
+
+    // create complex conjugate since we need this and only this later on
+    for (f=0;f<4*M_FAC;f++)
+    {
+        pilot_lut[f] = cconj(pilot_lut[f]);
+    }
+
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: lpf_peak_pick()
+  AUTHOR......: David Rowe
+  DATE CREATED: 20/4/2012
+
+  LPF and peak pick part of freq est, put in a function as we call it twice.
+
+\*---------------------------------------------------------------------------*/
+
+void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],
+		   COMP pilot_lpf[], codec2_fft_cfg fft_pilot_cfg, COMP S[], int nin,
+                   int do_fft)
+{
+    int   i,j,k;
+    int   mpilot;
+    float mag, imax;
+    int   ix;
+    float r;
+
+    /* LPF cutoff 200Hz, so we can handle max +/- 200 Hz freq offset */
+
+    for(i=0; i<NPILOTLPF-nin; i++)
+        pilot_lpf[i] = pilot_lpf[nin+i];
+    for(i=NPILOTLPF-nin, j=NPILOTBASEBAND-nin; i<NPILOTLPF; i++,j++) {
+        pilot_lpf[i].real = 0.0; pilot_lpf[i].imag = 0.0;
+
+        // STM32F4 hand optimized, this alone makes it go done from 1.6 to 1.17ms
+        // switching pilot_coeff to RAM (by removing const in pilot_coeff.h) would save
+        // another 0.11 ms at the expense of NPILOTCOEFF * 4 bytes == 120 bytes RAM
+
+        if (NPILOTCOEFF%5 == 0)
+        {
+            for(k=0; k<NPILOTCOEFF; k+=5)
+            {
+                COMP i0 = fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k]);
+                COMP i1 = fcmult(pilot_coeff[k+1], pilot_baseband[j-NPILOTCOEFF+1+k+1]);
+                COMP i2 = fcmult(pilot_coeff[k+2], pilot_baseband[j-NPILOTCOEFF+1+k+2]);
+                COMP i3 = fcmult(pilot_coeff[k+3], pilot_baseband[j-NPILOTCOEFF+1+k+3]);
+                COMP i4 = fcmult(pilot_coeff[k+4], pilot_baseband[j-NPILOTCOEFF+1+k+4]);
+
+                pilot_lpf[i] = cadd(cadd(cadd(cadd(cadd(pilot_lpf[i], i0),i1),i2),i3),i4);
+            }
+        }
+        else
+        {
+            for(k=0; k<NPILOTCOEFF; k++)
+            {
+                pilot_lpf[i] = cadd(pilot_lpf[i], fcmult(pilot_coeff[k], pilot_baseband[j-NPILOTCOEFF+1+k]));
+            }
+
+        }
+    }
+
+    /* We only need to do FFTs if we are out of sync.  Making them optional saves CPU in sync, which is when
+       we need to run the codec */
+
+    imax = 0.0;
+    *foff = 0.0;
+    for(i=0; i<MPILOTFFT; i++) {
+        S[i].real = 0.0;
+        S[i].imag = 0.0;
+    }
+
+    if (do_fft) {
+
+        /* decimate to improve DFT resolution, window and DFT */
+        mpilot = FS/(2*200);  /* calc decimation rate given new sample rate is twice LPF freq */
+        for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) {
+            S[j] = fcmult(hanning[i], pilot_lpf[i]);
+        }
+
+        codec2_fft_inplace(fft_pilot_cfg, S);
+
+        /* peak pick and convert to Hz */
+
+        imax = 0.0;
+        ix = 0;
+        for(i=0; i<MPILOTFFT; i++) {
+            mag = S[i].real*S[i].real + S[i].imag*S[i].imag;
+            if (mag > imax) {
+                imax = mag;
+                ix = i;
+            }
+        }
+        r = 2.0*200.0/MPILOTFFT;     /* maps FFT bin to frequency in Hz */
+
+        if (ix >= MPILOTFFT/2)
+            *foff = (ix - MPILOTFFT)*r;
+        else
+            *foff = (ix)*r;
+    }
+
+    *max = imax;
+
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: rx_est_freq_offset()
+  AUTHOR......: David Rowe
+  DATE CREATED: 19/4/2012
+
+  Estimate frequency offset of FDM signal using BPSK pilot.  Note that
+  this algorithm is quite sensitive to pilot tone level wrt other
+  carriers, so test variations to the pilot amplitude carefully.
+
+\*---------------------------------------------------------------------------*/
+
+float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft)
+{
+    int  i;
+#ifndef FDV_ARM_MATH
+    int j;
+#endif
+    COMP pilot[M_FAC+M_FAC/P];
+    COMP prev_pilot[M_FAC+M_FAC/P];
+    float foff, foff1, foff2;
+    float   max1, max2;
+
+    assert(nin <= M_FAC+M_FAC/P);
+
+    /* get pilot samples used for correlation/down conversion of rx signal */
+
+    for (i=0; i<nin; i++) {
+	pilot[i] = f->pilot_lut[f->pilot_lut_index];
+	f->pilot_lut_index++;
+	if (f->pilot_lut_index >= 4*M_FAC)
+	    f->pilot_lut_index = 0;
+
+	prev_pilot[i] = f->pilot_lut[f->prev_pilot_lut_index];
+	f->prev_pilot_lut_index++;
+	if (f->prev_pilot_lut_index >= 4*M_FAC)
+	    f->prev_pilot_lut_index = 0;
+    }
+
+    /*
+      Down convert latest M_FAC samples of pilot by multiplying by ideal
+      BPSK pilot signal we have generated locally.  The peak of the
+      resulting signal is sensitive to the time shift between the
+      received and local version of the pilot, so we do it twice at
+      different time shifts and choose the maximum.
+    */
+
+    for(i=0; i<NPILOTBASEBAND-nin; i++) {
+	f->pilot_baseband1[i] = f->pilot_baseband1[i+nin];
+	f->pilot_baseband2[i] = f->pilot_baseband2[i+nin];
+    }
+
+#ifndef FDV_ARM_MATH
+    for(i=0,j=NPILOTBASEBAND-nin; i<nin; i++,j++) {
+       	f->pilot_baseband1[j] = cmult(rx_fdm[i], pilot[i]);
+	f->pilot_baseband2[j] = cmult(rx_fdm[i], prev_pilot[i]);
+    }
+#else
+    // TODO: Maybe a handwritten mult taking advantage of rx_fdm[0] being
+    // used twice would be faster but this is for sure faster than
+    // the implementation above in any case.
+    arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&pilot[0].real,&f->pilot_baseband1[NPILOTBASEBAND-nin].real,nin);
+    arm_cmplx_mult_cmplx_f32(&rx_fdm[0].real,&prev_pilot[0].real,&f->pilot_baseband2[NPILOTBASEBAND-nin].real,nin);
+#endif
+
+    lpf_peak_pick(&foff1, &max1, f->pilot_baseband1, f->pilot_lpf1, f->fft_pilot_cfg, f->S1, nin, do_fft);
+    lpf_peak_pick(&foff2, &max2, f->pilot_baseband2, f->pilot_lpf2, f->fft_pilot_cfg, f->S2, nin, do_fft);
+
+    if (max1 > max2)
+	foff = foff1;
+    else
+	foff = foff2;
+
+    return foff;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_freq_shift()
+  AUTHOR......: David Rowe
+  DATE CREATED: 26/4/2012
+
+  Frequency shift modem signal.  The use of complex input and output allows
+  single sided frequency shifting (no images).
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff,
+                      COMP *foff_phase_rect, int nin)
+{
+    COMP  foff_rect;
+    float mag;
+    int   i;
+
+    foff_rect.real = cosf(2.0*PI*foff/FS);
+    foff_rect.imag = sinf(2.0*PI*foff/FS);
+    for(i=0; i<nin; i++) {
+	*foff_phase_rect = cmult(*foff_phase_rect, foff_rect);
+	rx_fdm_fcorr[i] = cmult(rx_fdm[i], *foff_phase_rect);
+    }
+
+    /* normalise digital oscillator as the magnitude can drift over time */
+
+    mag = cabsolute(*foff_phase_rect);
+    foff_phase_rect->real /= mag;
+    foff_phase_rect->imag /= mag;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdm_downconvert
+  AUTHOR......: David Rowe
+  DATE CREATED: 22/4/2012
+
+  Frequency shift each modem carrier down to Nc+1 baseband signals.
+
+\*---------------------------------------------------------------------------*/
+
+void fdm_downconvert(COMP rx_baseband[NC+1][M_FAC+M_FAC/P], int Nc, COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin)
+{
+    int   i,c;
+    float mag;
+
+    /* maximum number of input samples to demod */
+
+    assert(nin <= (M_FAC+M_FAC/P));
+
+    /* downconvert */
+
+    for (c=0; c<Nc+1; c++)
+	for (i=0; i<nin; i++) {
+	    phase_rx[c] = cmult(phase_rx[c], freq[c]);
+	    rx_baseband[c][i] = cmult(rx_fdm[i], cconj(phase_rx[c]));
+	}
+
+    /* normalise digital oscilators as the magnitude can drift over time */
+
+    for (c=0; c<Nc+1; c++) {
+        mag = cabsolute(phase_rx[c]);
+	phase_rx[c].real /= mag;
+	phase_rx[c].imag /= mag;
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: rx_filter()
+  AUTHOR......: David Rowe
+  DATE CREATED: 22/4/2012
+
+  Receive filter each baseband signal at oversample rate P.  Filtering at
+  rate P lowers CPU compared to rate M_FAC.
+
+  Depending on the number of input samples to the demod nin, we
+  produce P-1, P (usually), or P+1 filtered samples at rate P.  nin is
+  occasionally adjusted to compensate for timing slips due to
+  different tx and rx sample clocks.
+
+\*---------------------------------------------------------------------------*/
+
+void rx_filter(COMP rx_filt[][P+1], int Nc, COMP rx_baseband[][M_FAC+M_FAC/P], COMP rx_filter_memory[][NFILTER], int nin)
+{
+    int c, i,j,k,l;
+    int n=M_FAC/P;
+
+    /* rx filter each symbol, generate P filtered output samples for
+       each symbol.  Note we keep filter memory at rate M_FAC, it's just
+       the filter output at rate P */
+
+    for(i=0, j=0; i<nin; i+=n,j++) {
+
+	/* latest input sample */
+
+	for(c=0; c<Nc+1; c++)
+	    for(k=NFILTER-n,l=i; k<NFILTER; k++,l++)
+		rx_filter_memory[c][k] = rx_baseband[c][l];
+
+	/* convolution (filtering) */
+
+	for(c=0; c<Nc+1; c++) {
+	    rx_filt[c][j].real = 0.0; rx_filt[c][j].imag = 0.0;
+	    for(k=0; k<NFILTER; k++)
+		rx_filt[c][j] = cadd(rx_filt[c][j], fcmult(gt_alpha5_root[k], rx_filter_memory[c][k]));
+	}
+
+	/* make room for next input sample */
+
+	for(c=0; c<Nc+1; c++)
+	    for(k=0,l=n; k<NFILTER-n; k++,l++)
+		rx_filter_memory[c][k] = rx_filter_memory[c][l];
+    }
+
+    assert(j <= (P+1)); /* check for any over runs */
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: rxdec_filter()
+  AUTHOR......: David Rowe
+  DATE CREATED: 31 July 2014
+
+  +/- 1000Hz low pass filter, allows us to filter at rate Q to save CPU load.
+
+\*---------------------------------------------------------------------------*/
+
+void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], int nin) {
+    int i,j,k,st;
+
+    for(i=0; i<NRXDECMEM-nin; i++)
+        rxdec_lpf_mem[i] = rxdec_lpf_mem[i+nin];
+    for(i=0, j=NRXDECMEM-nin; i<nin; i++,j++)
+        rxdec_lpf_mem[j] = rx_fdm[i];
+
+    st = NRXDECMEM - nin - NRXDEC + 1;
+    for(i=0; i<nin; i++) {
+        rx_fdm_filter[i].real = 0.0;
+        for(k=0; k<NRXDEC; k++)
+            rx_fdm_filter[i].real += rxdec_lpf_mem[st+i+k].real * rxdec_coeff[k];
+        rx_fdm_filter[i].imag = 0.0;
+        for(k=0; k<NRXDEC; k++)
+            rx_fdm_filter[i].imag += rxdec_lpf_mem[st+i+k].imag * rxdec_coeff[k];
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fir_filter2()
+  AUTHOR......: Danilo Beuche
+  DATE CREATED: August 2016
+
+  This version submitted by Danilo for the STM32F4 platform.  The idea
+  is to avoid reading the same value from the STM32F4 "slow" flash
+  twice. 2-4ms of savings per frame were measured by Danilo and the mcHF
+  team.
+
+\*---------------------------------------------------------------------------*/
+
+static void fir_filter2(float acc[], float mem[], const float coeff[], const unsigned int dec_rate) {
+    acc[0] = 0.0;
+    acc[1] = 0.0;
+
+    float c1,c2,c3,c4,c5,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,a1,a2;
+    float* inpCmplx = &mem[0];
+    const float* coeffPtr = &coeff[0];
+
+    int m;
+
+    // this manual loop unrolling gives significant boost on STM32 machines
+    // reduction from avg 3.2ms to 2.4ms in tfdmv.c test
+    // 5 was the sweet spot, with 6 it took longer again
+    // and should not harm other, more powerful machines
+    // no significant difference in output, only rounding (which was to be expected)
+    // TODO: try to move coeffs to RAM and check if it makes a significant difference
+    if (NFILTER%(dec_rate*5) == 0) {
+        for(m=0; m<NFILTER; m+=dec_rate*5) {
+            c1 = *coeffPtr;
+
+            m1 = inpCmplx[0];
+            m2 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            c2 = *coeffPtr;
+            m3 = inpCmplx[0];
+            m4 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            c3 = *coeffPtr;
+            m5 = inpCmplx[0];
+            m6 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            c4 = *coeffPtr;
+            m7 = inpCmplx[0];
+            m8 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            c5 = *coeffPtr;
+            m9 = inpCmplx[0];
+            m10 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            a1 = c1 * m1 + c2 * m3 + c3 * m5 + c4 * m7 + c5 * m9;
+            a2 = c1 * m2 + c2 * m4 + c3 * m6 + c4 * m8 + c5 * m10;
+            acc[0] += a1;
+            acc[1] += a2;
+        }
+    }
+    else
+    {
+        for(m=0; m<NFILTER; m+=dec_rate) {
+            c1 = *coeffPtr;
+
+            m1 = inpCmplx[0];
+            m2 = inpCmplx[1];
+
+            inpCmplx+= dec_rate*2;
+            coeffPtr+= dec_rate;
+
+            a1 = c1 * m1;
+            a2 = c1 * m2;
+            acc[0] += a1;
+            acc[1] += a2;
+        }
+    }
+    acc[0] *= dec_rate;
+    acc[1] *= dec_rate;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: down_convert_and_rx_filter()
+  AUTHOR......: David Rowe
+  DATE CREATED: 30/6/2014
+
+  Combined down convert and rx filter, more memory efficient but less
+  intuitive design.
+
+  Depending on the number of input samples to the demod nin, we
+  produce P-1, P (usually), or P+1 filtered samples at rate P.  nin is
+  occasionally adjusted to compensate for timing slips due to
+  different tx and rx sample clocks.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+   TODO: [ ] windback phase calculated once at init time
+*/
+
+void down_convert_and_rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_fdm[],
+                                COMP rx_fdm_mem[], COMP phase_rx[], COMP freq[],
+                                float freq_pol[], int nin, int dec_rate)
+{
+    int i,k,c,st,Nval;
+    float windback_phase, mag;
+    COMP  windback_phase_rect;
+    COMP  rx_baseband[NRX_FDM_MEM];
+    COMP  f_rect;
+
+    //PROFILE_VAR(windback_start,  downconvert_start, filter_start);
+
+    /* update memory of rx_fdm */
+
+#if 0
+    for(i=0; i<NRX_FDM_MEM-nin; i++)
+        rx_fdm_mem[i] = rx_fdm_mem[i+nin];
+    for(i=NFILTER+M_FAC-nin, k=0; i<NFILTER+M_FAC; i++, k++)
+        rx_fdm_mem[i] = rx_fdm[k];
+#else
+    // this gives only 40uS gain on STM32 but now that we have, we keep it
+    memmove(&rx_fdm_mem[0],&rx_fdm_mem[nin],(NRX_FDM_MEM-nin)*sizeof(COMP));
+    memcpy(&rx_fdm_mem[NRX_FDM_MEM-nin],&rx_fdm[0],nin*sizeof(COMP));
+#endif
+    for(c=0; c<Nc+1; c++) {
+
+      /*
+
+        So we have rx_fdm_mem, a baseband array of samples at
+        rate Fs Hz, including the last nin samples at the end.  To
+        filter each symbol we require the baseband samples for all Nsym
+        symbols that we filter over.  So we need to downconvert the
+        entire rx_fdm_mem array.  To downconvert these we need the LO
+        phase referenced to the start of the rx_fdm_mem array.
+
+
+        <--------------- Nrx_filt_mem ------->
+        nin
+        |--------------------------|---------|
+        1                          |
+        phase_rx(c)
+
+        This means winding phase(c) back from this point
+        to ensure phase continuity.
+
+      */
+
+        //PROFILE_SAMPLE(windback_start);
+        windback_phase           = -freq_pol[c]*NFILTER;
+        windback_phase_rect.real = cosf(windback_phase);
+        windback_phase_rect.imag = sinf(windback_phase);
+        phase_rx[c]              = cmult(phase_rx[c],windback_phase_rect);
+        //PROFILE_SAMPLE_AND_LOG(downconvert_start, windback_start, "        windback");
+
+        /* down convert all samples in buffer */
+
+        st  = NRX_FDM_MEM-1;  /* end of buffer                  */
+        st -= nin-1;          /* first new sample               */
+        st -= NFILTER;        /* first sample used in filtering */
+
+        /* freq shift per dec_rate step is dec_rate times original shift */
+
+        f_rect = freq[c];
+        for(i=0; i<dec_rate-1; i++)
+            f_rect = cmult(f_rect,freq[c]);
+
+        for(i=st; i<NRX_FDM_MEM; i+=dec_rate) {
+            phase_rx[c]    = cmult(phase_rx[c], f_rect);
+            rx_baseband[i] = cmult(rx_fdm_mem[i],cconj(phase_rx[c]));
+        }
+        //PROFILE_SAMPLE_AND_LOG(filter_start, downconvert_start, "        downconvert");
+
+        /* now we can filter this carrier's P symbols */
+
+        Nval=M_FAC/P;
+        for(i=0, k=0; i<nin; i+=Nval, k++) {
+#ifdef ORIG
+            rx_filt[c][k].real = 0.0; rx_filt[c][k].imag = 0.0;
+
+            for(m=0; m<NFILTER; m++)
+                rx_filt[c][k] = cadd(rx_filt[c][k], fcmult(gt_alpha5_root[m], rx_baseband[st+i+m]));
+#else
+            // rx_filt[c][k].real = fir_filter(&rx_baseband[st+i].real, (float*)gt_alpha5_root, dec_rate);
+            // rx_filt[c][k].imag = fir_filter(&rx_baseband[st+i].imag, (float*)gt_alpha5_root, dec_rate);
+            fir_filter2(&rx_filt[c][k].real,&rx_baseband[st+i].real, gt_alpha5_root, dec_rate);
+#endif
+        }
+        //PROFILE_SAMPLE_AND_LOG2(filter_start, "        filter");
+
+        /* normalise digital oscilators as the magnitude can drift over time */
+
+        mag = cabsolute(phase_rx[c]);
+	phase_rx[c].real /= mag;
+	phase_rx[c].imag /= mag;
+
+       //printf("phase_rx[%d] = %f %f\n", c, phase_rx[c].real, phase_rx[c].imag);
+    }
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: rx_est_timing()
+  AUTHOR......: David Rowe
+  DATE CREATED: 23/4/2012
+
+  Estimate optimum timing offset, re-filter receive symbols at optimum
+  timing estimate.
+
+\*---------------------------------------------------------------------------*/
+
+float rx_est_timing(COMP rx_symbols[],
+                    int  Nc,
+		    COMP rx_filt[][P+1],
+		    COMP rx_filter_mem_timing[][NT*P],
+		    float env[],
+		    int nin,
+                    int m)
+{
+    int   c,i,j;
+    int   adjust;
+    COMP  x, phase, freq;
+    float rx_timing, fract, norm_rx_timing;
+    int   low_sample, high_sample;
+
+    /*
+      nin  adjust
+      --------------------------------
+      120  -1 (one less rate P sample)
+      160   0 (nominal)
+      200   1 (one more rate P sample)
+    */
+
+    adjust = P - nin*P/m;
+
+    /* update buffer of NT rate P filtered symbols */
+
+    for(c=0; c<Nc+1; c++)
+	for(i=0,j=P-adjust; i<(NT-1)*P+adjust; i++,j++)
+	    rx_filter_mem_timing[c][i] = rx_filter_mem_timing[c][j];
+    for(c=0; c<Nc+1; c++)
+	for(i=(NT-1)*P+adjust,j=0; i<NT*P; i++,j++)
+	    rx_filter_mem_timing[c][i] = rx_filt[c][j];
+
+    /* sum envelopes of all carriers */
+
+    for(i=0; i<NT*P; i++) {
+	env[i] = 0.0;
+	for(c=0; c<Nc+1; c++)
+	    env[i] += cabsolute(rx_filter_mem_timing[c][i]);
+    }
+
+    /* The envelope has a frequency component at the symbol rate.  The
+       phase of this frequency component indicates the timing.  So work
+       out single DFT at frequency 2*pi/P */
+
+    x.real = 0.0; x.imag = 0.0;
+    freq.real = cosf(2*PI/P);
+    freq.imag = sinf(2*PI/P);
+    phase.real = 1.0;
+    phase.imag = 0.0;
+
+    for(i=0; i<NT*P; i++) {
+	x = cadd(x, fcmult(env[i], phase));
+	phase = cmult(phase, freq);
+    }
+
+    /* Map phase to estimated optimum timing instant at rate P.  The
+       P/4 part was adjusted by experiment, I know not why.... */
+
+    norm_rx_timing = atan2f(x.imag, x.real)/(2*PI);
+    assert(fabsf(norm_rx_timing) < 1.0);
+    rx_timing      = norm_rx_timing*P + P/4;
+
+    if (rx_timing > P)
+	rx_timing -= P;
+    if (rx_timing < -P)
+	rx_timing += P;
+
+    /* rx_filter_mem_timing contains Nt*P samples (Nt symbols at rate
+       P), where Nt is odd.  Lets use linear interpolation to resample
+       in the centre of the timing estimation window .*/
+
+    rx_timing  += floorf(NT/2.0)*P;
+    low_sample = floorf(rx_timing);
+    fract = rx_timing - low_sample;
+    high_sample = ceilf(rx_timing);
+
+    //printf("rx_timing: %f low_sample: %d high_sample: %d fract: %f\n", rx_timing, low_sample, high_sample, fract);
+
+    for(c=0; c<Nc+1; c++) {
+        rx_symbols[c] = cadd(fcmult(1.0-fract, rx_filter_mem_timing[c][low_sample-1]), fcmult(fract, rx_filter_mem_timing[c][high_sample-1]));
+        //rx_symbols[c] = rx_filter_mem_timing[c][high_sample];
+    }
+
+    /* This value will be +/- half a symbol so will wrap around at +/-
+       M/2 or +/- 80 samples with M=160 */
+
+    return norm_rx_timing*m;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: qpsk_to_bits()
+  AUTHOR......: David Rowe
+  DATE CREATED: 24/4/2012
+
+  Convert DQPSK symbols back to an array of bits, extracts sync bit
+  from DBPSK pilot, and also uses pilot to estimate fine frequency
+  error.
+
+\*---------------------------------------------------------------------------*/
+
+float qpsk_to_bits(int rx_bits[], int *sync_bit, int Nc, COMP phase_difference[], COMP prev_rx_symbols[],
+                   COMP rx_symbols[], int old_qpsk_mapping)
+{
+    int   c;
+    COMP  d;
+    int   msb=0, lsb=0;
+    float ferr, norm;
+
+
+    /* Extra 45 degree clockwise lets us use real and imag axis as
+       decision boundaries. "norm" makes sure the phase subtraction
+       from the previous symbol doesn't affect the amplitude, which
+       leads to sensible scatter plots */
+
+    for(c=0; c<Nc; c++) {
+        norm = 1.0/(cabsolute(prev_rx_symbols[c])+1E-6);
+	phase_difference[c] = cmult(cmult(rx_symbols[c], fcmult(norm,cconj(prev_rx_symbols[c]))), pi_on_4);
+    }
+
+    /* map (Nc,1) DQPSK symbols back into an (1,Nc*Nb) array of bits */
+
+    for (c=0; c<Nc; c++) {
+      d = phase_difference[c];
+      if ((d.real >= 0) && (d.imag >= 0)) {
+          msb = 0; lsb = 0;
+      }
+      if ((d.real < 0) && (d.imag >= 0)) {
+          msb = 0; lsb = 1;
+      }
+      if ((d.real < 0) && (d.imag < 0)) {
+          if (old_qpsk_mapping) {
+              msb = 1; lsb = 0;
+          } else {
+              msb = 1; lsb = 1;
+          }
+      }
+      if ((d.real >= 0) && (d.imag < 0)) {
+          if (old_qpsk_mapping) {
+              msb = 1; lsb = 1;
+          } else {
+              msb = 1; lsb = 0;
+          }
+      }
+      rx_bits[2*c] = msb;
+      rx_bits[2*c+1] = lsb;
+    }
+
+    /* Extract DBPSK encoded Sync bit and fine freq offset estimate */
+
+    norm = 1.0/(cabsolute(prev_rx_symbols[Nc])+1E-6);
+    phase_difference[Nc] = cmult(rx_symbols[Nc], fcmult(norm, cconj(prev_rx_symbols[Nc])));
+    if (phase_difference[Nc].real < 0) {
+      *sync_bit = 1;
+      ferr = phase_difference[Nc].imag*norm;    /* make f_err magnitude insensitive */
+    }
+    else {
+      *sync_bit = 0;
+      ferr = -phase_difference[Nc].imag*norm;
+    }
+
+    /* pilot carrier gets an extra pi/4 rotation to make it consistent
+       with other carriers, as we need it for snr_update and scatter
+       diagram */
+
+    phase_difference[Nc] = cmult(phase_difference[Nc], pi_on_4);
+
+    return ferr;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: snr_update()
+  AUTHOR......: David Rowe
+  DATE CREATED: 17 May 2012
+
+  Given phase differences update estimates of signal and noise levels.
+
+\*---------------------------------------------------------------------------*/
+
+void snr_update(float sig_est[], float noise_est[], int Nc, COMP phase_difference[])
+{
+    float s[NC+1];
+    COMP  refl_symbols[NC+1];
+    float n[NC+1];
+    int   c;
+
+
+    /* mag of each symbol is distance from origin, this gives us a
+       vector of mags, one for each carrier. */
+
+    for(c=0; c<Nc+1; c++)
+	s[c] = cabsolute(phase_difference[c]);
+
+    /* signal mag estimate for each carrier is a smoothed version of
+       instantaneous magntitude, this gives us a vector of smoothed
+       mag estimates, one for each carrier. */
+
+    for(c=0; c<Nc+1; c++)
+	sig_est[c] = SNR_COEFF*sig_est[c] + (1.0 - SNR_COEFF)*s[c];
+
+    /* noise mag estimate is distance of current symbol from average
+       location of that symbol.  We reflect all symbols into the first
+       quadrant for convenience. */
+
+    for(c=0; c<Nc+1; c++) {
+	refl_symbols[c].real = fabsf(phase_difference[c].real);
+	refl_symbols[c].imag = fabsf(phase_difference[c].imag);
+	n[c] = cabsolute(cadd(fcmult(sig_est[c], pi_on_4), cneg(refl_symbols[c])));
+    }
+
+    /* noise mag estimate for each carrier is a smoothed version of
+       instantaneous noise mag, this gives us a vector of smoothed
+       noise power estimates, one for each carrier. */
+
+    for(c=0; c<Nc+1; c++)
+	noise_est[c] = SNR_COEFF*noise_est[c] + (1 - SNR_COEFF)*n[c];
+}
+
+// returns number of shorts in error_pattern[], one short per error
+
+int fdmdv_error_pattern_size(struct FDMDV *f) {
+    return f->ntest_bits;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_put_test_bits()
+  AUTHOR......: David Rowe
+  DATE CREATED: 24/4/2012
+
+  Accepts nbits from rx and attempts to sync with test_bits sequence.
+  If sync OK measures bit errors.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_put_test_bits(struct FDMDV *f, int *sync, short error_pattern[],
+			 int *bit_errors, int *ntest_bits, int rx_bits[])
+{
+    int   i,j;
+    float ber;
+    int   bits_per_frame = fdmdv_bits_per_frame(f);
+
+    /* Append to our memory */
+
+    for(i=0,j=bits_per_frame; i<f->ntest_bits-bits_per_frame; i++,j++)
+	f->rx_test_bits_mem[i] = f->rx_test_bits_mem[j];
+    for(i=f->ntest_bits-bits_per_frame,j=0; i<f->ntest_bits; i++,j++)
+	f->rx_test_bits_mem[i] = rx_bits[j];
+
+    /* see how many bit errors we get when checked against test sequence */
+
+    *bit_errors = 0;
+    for(i=0; i<f->ntest_bits; i++) {
+        error_pattern[i] = test_bits[i] ^ f->rx_test_bits_mem[i];
+	*bit_errors += error_pattern[i];
+	//printf("%d %d %d %d\n", i, test_bits[i], f->rx_test_bits_mem[i], test_bits[i] ^ f->rx_test_bits_mem[i]);
+    }
+
+    /* if less than a thresh we are aligned and in sync with test sequence */
+
+    ber = (float)*bit_errors/f->ntest_bits;
+
+    *sync = 0;
+    if (ber < 0.2)
+	*sync = 1;
+
+    *ntest_bits = f->ntest_bits;
+
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: freq_state(()
+  AUTHOR......: David Rowe
+  DATE CREATED: 24/4/2012
+
+  Freq offset state machine.  Moves between coarse and fine states
+  based on BPSK pilot sequence.  Freq offset estimator occasionally
+  makes mistakes when used continuously.  So we use it until we have
+  acquired the BPSK pilot, then switch to a more robust "fine"
+  tracking algorithm.  If we lose sync we switch back to coarse mode
+  for fast re-acquisition of large frequency offsets.
+
+  The sync state is also useful for higher layers to determine when
+  there is valid FDMDV data for decoding.  We want to reliably and
+  quickly get into sync, stay in sync even on fading channels, and
+  fall out of sync quickly if tx stops or it's a false sync.
+
+  In multipath fading channels the BPSK sync carrier may be pushed
+  down in the noise, despite other carriers being at full strength.
+  We want to avoid loss of sync in these cases.
+
+\*---------------------------------------------------------------------------*/
+
+int freq_state(int *reliable_sync_bit, int sync_bit, int *state, int *timer, int *sync_mem)
+{
+    int next_state, sync, unique_word, i, corr;
+
+    /* look for 6 symbols (120ms) 101010 of sync sequence */
+
+    unique_word = 0;
+    for(i=0; i<NSYNC_MEM-1; i++)
+        sync_mem[i] = sync_mem[i+1];
+    sync_mem[i] = 1 - 2*sync_bit;
+    corr = 0;
+    for(i=0; i<NSYNC_MEM; i++)
+        corr += sync_mem[i]*sync_uw[i];
+    if (abs(corr) == NSYNC_MEM)
+        unique_word = 1;
+    *reliable_sync_bit = (corr == NSYNC_MEM);
+
+    /* iterate state machine */
+
+    next_state = *state;
+    switch(*state) {
+    case 0:
+	if (unique_word) {
+	    next_state = 1;
+            *timer = 0;
+        }
+	break;
+    case 1:                   /* tentative sync state         */
+	if (unique_word) {
+            (*timer)++;
+            if (*timer == 25) /* sync has been good for 500ms */
+                next_state = 2;
+        }
+	else
+	    next_state = 0;  /* quickly fall out of sync     */
+	break;
+    case 2:                  /* good sync state */
+	if (unique_word == 0) {
+            *timer = 0;
+	    next_state = 3;
+        }
+	break;
+    case 3:                  /* tentative bad state, but could be a fade */
+	if (unique_word)
+	    next_state = 2;
+	else  {
+            (*timer)++;
+            if (*timer == 50) /* wait for 1000ms in case sync comes back  */
+                next_state = 0;
+        }
+	break;
+    }
+
+    //printf("state: %d next_state: %d uw: %d timer: %d\n", *state, next_state, unique_word, *timer);
+    *state = next_state;
+    if (*state)
+	sync = 1;
+    else
+	sync = 0;
+
+    return sync;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_demod()
+  AUTHOR......: David Rowe
+  DATE CREATED: 26/4/2012
+
+  FDMDV demodulator, take an array of FDMDV_SAMPLES_PER_FRAME
+  modulated samples, returns an array of FDMDV_BITS_PER_FRAME bits,
+  plus the sync bit.
+
+  The input signal is complex to support single sided frequency shifting
+  before the demod input (e.g. fdmdv2 click to tune feature).
+
+  The number of input samples nin will normally be M_FAC ==
+  FDMDV_SAMPLES_PER_FRAME.  However to adjust for differences in
+  transmit and receive sample clocks nin will occasionally be M_FAC-M_FAC/P,
+  or M_FAC+M_FAC/P.
+
+\*---------------------------------------------------------------------------*/
+
+
+void fdmdv_demod(struct FDMDV *fdmdv, int rx_bits[],
+		 int *reliable_sync_bit, COMP rx_fdm[], int *nin)
+{
+    float         foff_coarse, foff_fine;
+    COMP          rx_fdm_fcorr[M_FAC+M_FAC/P];
+    COMP          rx_fdm_filter[M_FAC+M_FAC/P];
+    COMP          rx_fdm_bb[M_FAC+M_FAC/P];
+    COMP          rx_filt[NC+1][P+1];
+    COMP          rx_symbols[NC+1];
+    float         env[NT*P];
+    int           sync_bit;
+    PROFILE_VAR(demod_start, fdmdv_freq_shift_start, down_convert_and_rx_filter_start);
+    PROFILE_VAR(rx_est_timing_start, qpsk_to_bits_start, snr_update_start, freq_state_start);
+
+    /* shift down to complex baseband */
+
+    fdmdv_freq_shift(rx_fdm_bb, rx_fdm, -FDMDV_FCENTRE, &fdmdv->fbb_phase_rx, *nin);
+
+    /* freq offset estimation and correction */
+
+    PROFILE_SAMPLE(demod_start);
+    foff_coarse = rx_est_freq_offset(fdmdv, rx_fdm_bb, *nin, !fdmdv->sync);
+    PROFILE_SAMPLE_AND_LOG(fdmdv_freq_shift_start, demod_start, "    rx_est_freq_offset");
+
+    if (fdmdv->sync == 0)
+	fdmdv->foff = foff_coarse;
+    fdmdv_freq_shift(rx_fdm_fcorr, rx_fdm_bb, -fdmdv->foff, &fdmdv->foff_phase_rect, *nin);
+    PROFILE_SAMPLE_AND_LOG(down_convert_and_rx_filter_start, fdmdv_freq_shift_start, "    fdmdv_freq_shift");
+
+    /* baseband processing */
+
+    rxdec_filter(rx_fdm_filter, rx_fdm_fcorr, fdmdv->rxdec_lpf_mem, *nin);
+    down_convert_and_rx_filter(rx_filt, fdmdv->Nc, rx_fdm_filter, fdmdv->rx_fdm_mem, fdmdv->phase_rx, fdmdv->freq,
+                               fdmdv->freq_pol, *nin, M_FAC/Q);
+    PROFILE_SAMPLE_AND_LOG(rx_est_timing_start, down_convert_and_rx_filter_start, "    down_convert_and_rx_filter");
+    fdmdv->rx_timing = rx_est_timing(rx_symbols, fdmdv->Nc, rx_filt, fdmdv->rx_filter_mem_timing, env, *nin, M_FAC);
+    PROFILE_SAMPLE_AND_LOG(qpsk_to_bits_start, rx_est_timing_start, "    rx_est_timing");
+
+    /* Adjust number of input samples to keep timing within bounds */
+
+    *nin = M_FAC;
+
+    if (fdmdv->rx_timing > M_FAC/P)
+	*nin += M_FAC/P;
+
+    if (fdmdv->rx_timing < -M_FAC/P)
+	*nin -= M_FAC/P;
+
+    foff_fine = qpsk_to_bits(rx_bits, &sync_bit, fdmdv->Nc, fdmdv->phase_difference, fdmdv->prev_rx_symbols, rx_symbols,
+                             fdmdv->old_qpsk_mapping);
+    memcpy(fdmdv->prev_rx_symbols, rx_symbols, sizeof(COMP)*(fdmdv->Nc+1));
+    PROFILE_SAMPLE_AND_LOG(snr_update_start, qpsk_to_bits_start, "    qpsk_to_bits");
+    snr_update(fdmdv->sig_est, fdmdv->noise_est, fdmdv->Nc, fdmdv->phase_difference);
+    PROFILE_SAMPLE_AND_LOG(freq_state_start, snr_update_start, "    snr_update");
+
+    /* freq offset estimation state machine */
+
+    fdmdv->sync = freq_state(reliable_sync_bit, sync_bit, &fdmdv->fest_state, &fdmdv->timer, fdmdv->sync_mem);
+    PROFILE_SAMPLE_AND_LOG2(freq_state_start, "    freq_state");
+    fdmdv->foff  -= TRACK_COEFF*foff_fine;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: calc_snr()
+  AUTHOR......: David Rowe
+  DATE CREATED: 17 May 2012
+
+  Calculate current SNR estimate (3000Hz noise BW)
+
+\*---------------------------------------------------------------------------*/
+
+float calc_snr(int Nc, float sig_est[], float noise_est[])
+{
+    float S, SdB;
+    float mean, N50, N50dB, N3000dB;
+    float snr_dB;
+    int   c;
+
+    S = 0.0;
+    for(c=0; c<Nc+1; c++) {
+        S += sig_est[c] * sig_est[c];
+    }
+    SdB = 10.0*log10f(S+1E-12);
+
+    /* Average noise mag across all carriers and square to get an
+       average noise power.  This is an estimate of the noise power in
+       Rs = 50Hz of BW (note for raised root cosine filters Rs is the
+       noise BW of the filter) */
+
+    mean = 0.0;
+    for(c=0; c<Nc+1; c++)
+	mean += noise_est[c];
+    mean /= (Nc+1);
+    N50 = mean * mean;
+    N50dB = 10.0*log10f(N50+1E-12);
+
+    /* Now multiply by (3000 Hz)/(50 Hz) to find the total noise power
+       in 3000 Hz */
+
+    N3000dB = N50dB + 10.0*log10f(3000.0/RS);
+
+    snr_dB = SdB - N3000dB;
+
+    return snr_dB;
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_get_demod_stats()
+  AUTHOR......: David Rowe
+  DATE CREATED: 1 May 2012
+
+  Fills stats structure with a bunch of demod information.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_get_demod_stats(struct FDMDV *fdmdv, struct MODEM_STATS *stats)
+{
+    assert(fdmdv->Nc <= MODEM_STATS_NC_MAX);
+
+    stats->Nc = fdmdv->Nc;
+    stats->snr_est = calc_snr(fdmdv->Nc, fdmdv->sig_est, fdmdv->noise_est);
+    stats->sync = fdmdv->sync;
+    stats->foff = fdmdv->foff;
+    stats->rx_timing = fdmdv->rx_timing;
+    stats->clock_offset = 0.0; /* TODO - implement clock offset estimation */
+
+#ifndef __EMBEDDED__
+    stats->nr = 1;
+    for(int c=0; c<fdmdv->Nc+1; c++) {
+	stats->rx_symbols[0][c] = fdmdv->phase_difference[c];
+    }
+#endif
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_8_to_16()
+  AUTHOR......: David Rowe
+  DATE CREATED: 9 May 2012
+
+  Changes the sample rate of a signal from 8 to 16 kHz.  Support function for
+  SM1000.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_8_to_16(float out16k[], float in8k[], int n8k)
+{
+    int i,k,l;
+    float acc;
+
+    /* this version unrolled for specific FDMDV_OS */
+
+    assert(FDMDV_OS == 2);
+
+    for(i=0; i<n8k; i++) {
+        acc = 0.0;
+        for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
+            acc += fdmdv_os_filter[k]*in8k[i-l];
+        out16k[i*FDMDV_OS] = FDMDV_OS*acc;
+
+        acc = 0.0;
+        for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
+            acc += fdmdv_os_filter[k]*in8k[i-l];
+        out16k[i*FDMDV_OS+1] = FDMDV_OS*acc;
+    }
+
+    /* update filter memory */
+
+    for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
+	in8k[i] = in8k[i + n8k];
+
+}
+
+void fdmdv_8_to_16_short(short out16k[], short in8k[], int n8k)
+{
+    int i,k,l;
+    float acc;
+
+    /* this version unrolled for specific FDMDV_OS */
+
+    assert(FDMDV_OS == 2);
+
+    for(i=0; i<n8k; i++) {
+        acc = 0.0;
+        for(k=0,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
+            acc += fdmdv_os_filter[k]*(float)in8k[i-l];
+        out16k[i*FDMDV_OS] = FDMDV_OS*acc;
+
+        acc = 0.0;
+        for(k=1,l=0; k<FDMDV_OS_TAPS_16K; k+=FDMDV_OS,l++)
+            acc += fdmdv_os_filter[k]*(float)in8k[i-l];
+        out16k[i*FDMDV_OS+1] = FDMDV_OS*acc;
+    }
+
+    /* update filter memory */
+
+    for(i=-(FDMDV_OS_TAPS_8K); i<0; i++)
+	in8k[i] = in8k[i + n8k];
+
+}
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_16_to_8()
+  AUTHOR......: David Rowe
+  DATE CREATED: 9 May 2012
+
+  Changes the sample rate of a signal from 16 to 8 kHz.
+
+  n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n
+  samples at the 16 kHz rate.  As above however a memory of
+  FDMDV_OS_TAPS samples is reqd for in16k[] (see t16_8.c unit test as example).
+
+  Low pass filter the 16 kHz signal at 4 kHz using the same filter as
+  the upsampler, then just output every FDMDV_OS-th filtered sample.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_16_to_8(float out8k[], float in16k[], int n)
+{
+    float acc;
+    int   i,j,k;
+
+    for(i=0, k=0; k<n; i+=FDMDV_OS, k++) {
+	acc = 0.0;
+	for(j=0; j<FDMDV_OS_TAPS_16K; j++)
+	    acc += fdmdv_os_filter[j]*in16k[i-j];
+        out8k[k] = acc;
+    }
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
+	in16k[i] = in16k[i + n*FDMDV_OS];
+}
+
+void fdmdv_16_to_8_short(short out8k[], short in16k[], int n)
+{
+    float acc;
+    int i,j,k;
+
+    for(i=0, k=0; k<n; i+=FDMDV_OS, k++) {
+	acc = 0.0;
+	for(j=0; j<FDMDV_OS_TAPS_16K; j++)
+	    acc += fdmdv_os_filter[j]*(float)in16k[i-j];
+        out8k[k] = acc;
+    }
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_16K; i<0; i++)
+	in16k[i] = in16k[i + n*FDMDV_OS];
+}
+
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: fdmdv_8_to_48()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 9 May 2012
+
+  Changes the sample rate of a signal from 8 to 48 kHz.
+
+  n is the number of samples at the 8 kHz rate, there are FDMDV_OS*n samples
+  at the 48 kHz rate.  A memory of FDMDV_OS_TAPS_48/FDMDV_OS samples is reqd for
+  in8k[] (see t48_8.c unit test as example).
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_8_to_48(float out48k[], float in8k[], int n)
+{
+    int i,j,k,l;
+
+    for(i=0; i<n; i++) {
+	for(j=0; j<FDMDV_OS_48; j++) {
+	    out48k[i*FDMDV_OS_48+j] = 0.0;
+	    for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++)
+		out48k[i*FDMDV_OS_48+j] += fdmdv_os_filter48[k+j]*in8k[i-l];
+	    out48k[i*FDMDV_OS_48+j] *= FDMDV_OS_48;
+	    
+	}
+    }	
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++)
+	in8k[i] = in8k[i + n];
+}
+
+void fdmdv_8_to_48_short(short out48k[], short in8k[], int n)
+{
+    int i,j,k,l;
+    float acc;
+    
+    for(i=0; i<n; i++) {
+	for(j=0; j<FDMDV_OS_48; j++) {
+	    acc = 0.0;
+	    for(k=0,l=0; k<FDMDV_OS_TAPS_48K; k+=FDMDV_OS_48,l++)
+		acc += fdmdv_os_filter48[k+j]*in8k[i-l];
+	    out48k[i*FDMDV_OS_48+j] = acc*FDMDV_OS_48;	    
+	}
+    }	
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_48_8K; i<0; i++)
+	in8k[i] = in8k[i + n];
+}
+
+/*---------------------------------------------------------------------------*\
+                                                       
+  FUNCTION....: fdmdv_48_to_8()	     
+  AUTHOR......: David Rowe			      
+  DATE CREATED: 9 May 2012
+
+  Changes the sample rate of a signal from 48 to 8 kHz.
+ 
+  n is the number of samples at the 8 kHz rate, there are FDMDV_OS_48*n
+  samples at the 48 kHz rate.  As above however a memory of
+  FDMDV_OS_TAPS_48 samples is reqd for in48k[] (see t48_8.c unit test as example).
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_48_to_8(float out8k[], float in48k[], int n)
+{
+    int i,j;
+
+    for(i=0; i<n; i++) {
+	out8k[i] = 0.0;
+	for(j=0; j<FDMDV_OS_TAPS_48K; j++)
+	    out8k[i] += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j];
+    }
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_48K; i<0; i++)
+	in48k[i] = in48k[i + n*FDMDV_OS_48];
+}
+
+void fdmdv_48_to_8_short(short out8k[], short in48k[], int n)
+{
+    int i,j;
+    float acc;
+    
+    for(i=0; i<n; i++) {
+	acc = 0.0;
+	for(j=0; j<FDMDV_OS_TAPS_48K; j++)
+	    acc += fdmdv_os_filter48[j]*in48k[i*FDMDV_OS_48-j];
+        out8k[i] = acc;
+    }
+
+    /* update filter memory */
+
+    for(i=-FDMDV_OS_TAPS_48K; i<0; i++)
+	in48k[i] = in48k[i + n*FDMDV_OS_48];
+}
+
+/*---------------------------------------------------------------------------*\
+
+  Function used during development to test if magnitude of digital
+  oscillators was drifting.  It was!
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_dump_osc_mags(struct FDMDV *f)
+{
+    int   i;
+
+    fprintf(stderr, "phase_tx[]:\n");
+    for(i=0; i<=f->Nc; i++)
+	fprintf(stderr,"  %1.3f", (double)cabsolute(f->phase_tx[i]));
+    fprintf(stderr,"\nfreq[]:\n");
+    for(i=0; i<=f->Nc; i++)
+	fprintf(stderr,"  %1.3f", (double)cabsolute(f->freq[i]));
+    fprintf(stderr,"\nfoff_phase_rect: %1.3f", (double)cabsolute(f->foff_phase_rect));
+    fprintf(stderr,"\nphase_rx[]:\n");
+    for(i=0; i<=f->Nc; i++)
+	fprintf(stderr,"  %1.3f", (double)cabsolute(f->phase_rx[i]));
+    fprintf(stderr, "\n\n");
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: randn()
+  AUTHOR......: David Rowe
+  DATE CREATED: 2 August 2014
+
+  Simple approximation to normal (gaussian) random number generator
+  with 0 mean and unit variance.
+
+\*---------------------------------------------------------------------------*/
+
+#define RANDN_IT 12  /* This magic number of iterations gives us a
+                        unit variance.  I think because var =
+                        (b-a)^2/12 for one uniform random variable, so
+                        for a sum of n random variables it's
+                        n(b-a)^2/12, or for b=1, a = 0, n=12, we get
+                        var = 12(1-0)^2/12 = 1 */
+
+static float randn() {
+    int   i;
+    float rn = 0.0;
+
+    for(i=0; i<RANDN_IT; i++)
+        rn += (float)rand()/RAND_MAX;
+
+    rn -= (float)RANDN_IT/2.0;
+    return rn;
+}
+
+
+/*---------------------------------------------------------------------------*\
+
+  FUNCTION....: fdmdv_simulate_channel()
+  AUTHOR......: David Rowe
+  DATE CREATED: 10 July 2014
+
+  Simple channel simulation function to aid in testing.  Target SNR
+  uses noise measured in a 3 kHz bandwidth.
+
+  Doesn't use fdmdv states so can be called from anywhere, e.g. non
+  fdmdv applications.
+
+  TODO: Measured SNR is coming out a few dB higher than target_snr, this
+  needs to be fixed.
+
+\*---------------------------------------------------------------------------*/
+
+void fdmdv_simulate_channel(float *sig_pwr_av, COMP samples[], int nin, float target_snr)
+{
+    float sig_pwr, target_snr_linear, noise_pwr, noise_pwr_1Hz, noise_pwr_4000Hz, noise_gain;
+    int   i;
+
+    /* prevent NAN when we divide by nin below */
+    if (nin == 0) return;
+
+    /* estimate signal power */
+
+    sig_pwr = 0.0;
+    for(i=0; i<nin; i++)
+        sig_pwr += samples[i].real*samples[i].real + samples[i].imag*samples[i].imag;
+
+    sig_pwr /= nin;
+
+    *sig_pwr_av = 0.9**sig_pwr_av + 0.1*sig_pwr;
+
+    /* det noise to meet target SNR */
+
+    target_snr_linear = POW10F(target_snr/10.0);
+    noise_pwr = *sig_pwr_av/target_snr_linear;       /* noise pwr in a 3000 Hz BW     */
+    noise_pwr_1Hz = noise_pwr/3000.0;                  /* noise pwr in a 1 Hz bandwidth */
+    noise_pwr_4000Hz = noise_pwr_1Hz*4000.0;           /* noise pwr in a 4000 Hz BW, which
+                                                          due to fs=8000 Hz in our simulation noise BW */
+
+    noise_gain = sqrtf(0.5*noise_pwr_4000Hz);          /* split noise pwr between real and imag sides  */
+
+    for(i=0; i<nin; i++) {
+        samples[i].real += noise_gain*randn();
+        samples[i].imag += noise_gain*randn();
+    }
+    /*
+    fprintf(stderr, "sig_pwr: %f f->sig_pwr_av: %e target_snr_linear: %f noise_pwr_4000Hz: %e noise_gain: %e\n",
+            sig_pwr, *sig_pwr_av, target_snr_linear, noise_pwr_4000Hz, noise_gain);
+    */
+}
-- 
cgit v1.2.3