diff --git a/Cargo.lock b/Cargo.lock
index 145837ef450e09fbf5e6c3a6576c2cb1305563df..537dd87a5b557b4ebf96ec2772c7fadac678fb25 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -223,10 +223,13 @@ dependencies = [
"arrayvec",
"async-stream 0.2.1",
"bitvec",
+ "cc",
"colored",
"futures-core",
"half",
"ham-cats",
+ "itertools 0.12.0",
+ "libc",
"prost 0.12.3",
"rand",
"serde",
@@ -698,6 +701,15 @@ dependencies = [
"either",
]
+[[package]]
+name = "itertools"
+version = "0.12.0"
+source = "registry+https://github.com/rust-lang/crates.io-index"
+checksum = "25db6b064527c5d482d0423354fcd07a89a2dfe07b67892e62411946db7f07b0"
+dependencies = [
+ "either",
+]
+
[[package]]
name = "itoa"
version = "1.0.10"
diff --git a/Cargo.toml b/Cargo.toml
index d1aac9aad0f594833abca011466683d4e6b6f940..6eea815f5c85c6158937e230039aae3afe50ece1 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -2,7 +2,7 @@
name = "cats-sdr-igate"
version = "0.1.0"
edition = "2021"
-license = "MIT"
+license = "MIT OR LGPL-2.1-only"
authors = ["Stephen D"]
description = "An RX-only SDR CATS I-gate"
@@ -35,6 +35,9 @@ colored = "2.0.4"
uuid = { version = "1.5.0", features = ["v4"] }
rand = "0.8.5"
bitvec = "1.0.1"
+libc = "0.2.152"
+itertools = "0.12.0"
[build-dependencies]
tonic-build = "0.9"
+cc = "1.0.83"
diff --git a/build.rs b/build.rs
index ca1dc7dd7e85984da446120ab3df2fe66645d9f7..ddb0e6d3b54039cfc385696c53c4032d42c300f4 100644
--- a/build.rs
+++ b/build.rs
@@ -1,4 +1,22 @@
fn main() -> Result<(), Box<dyn std::error::Error>> {
tonic_build::compile_protos("proto/felinet.proto")?;
+
+ println!("cargo:rerun-if-changed=src/codec2/fsk.c");
+ println!("cargo:rerun-if-changed=src/codec2/mpdecode_core.c");
+ println!("cargo:rerun-if-changed=src/codec2/kiss_fft.c");
+ println!("cargo:rerun-if-changed=src/codec2/kiss_fftr.c");
+ println!("cargo:rerun-if-changed=src/codec2/modem_probe.c");
+ println!("cargo:rerun-if-changed=src/codec2/modem_stats.c");
+
+ cc::Build::new()
+ .file("src/codec2/fsk.c")
+ .file("src/codec2/mpdecode_core.c")
+ .file("src/codec2/kiss_fft.c")
+ .file("src/codec2/kiss_fftr.c")
+ .file("src/codec2/modem_probe.c")
+ .file("src/codec2/modem_stats.c")
+ .extra_warnings(false)
+ .compile("fsk");
+
Ok(())
}
diff --git a/src/codec2/_kiss_fft_guts.h b/src/codec2/_kiss_fft_guts.h
new file mode 100644
index 0000000000000000000000000000000000000000..16305af1a55af330ac8f977a9169b66aa63d9fa3
--- /dev/null
+++ b/src/codec2/_kiss_fft_guts.h
@@ -0,0 +1,194 @@
+/*
+Copyright (c) 2003-2010, Mark Borgerding
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+ * Neither the author nor the names of any contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/* kiss_fft.h
+ defines kiss_fft_scalar as either short or a float type
+ and defines
+ typedef struct { kiss_fft_scalar r; kiss_fft_scalar i; }kiss_fft_cpx; */
+#include <limits.h>
+
+#include "kiss_fft.h"
+
+#define MAXFACTORS 32
+/* e.g. an fft of length 128 has 4 factors
+ as far as kissfft is concerned
+ 4*4*4*2
+ */
+
+struct kiss_fft_state {
+ int nfft;
+ int inverse;
+ int factors[2 * MAXFACTORS];
+ kiss_fft_cpx twiddles[1];
+};
+
+/*
+ Explanation of macros dealing with complex math:
+
+ C_MUL(m,a,b) : m = a*b
+ C_FIXDIV( c , div ) : if a fixed point impl., c /= div. noop otherwise
+ C_SUB( res, a,b) : res = a - b
+ C_SUBFROM( res , a) : res -= a
+ C_ADDTO( res , a) : res += a
+ * */
+#ifdef FIXED_POINT
+#if (FIXED_POINT == 32)
+#define FRACBITS 31
+#define SAMPPROD int64_t
+#define SAMP_MAX 2147483647
+#else
+#define FRACBITS 15
+#define SAMPPROD int32_t
+#define SAMP_MAX 32767
+#endif
+
+#define SAMP_MIN -SAMP_MAX
+
+#if defined(CHECK_OVERFLOW)
+#define CHECK_OVERFLOW_OP(a, op, b) \
+ if ((SAMPPROD)(a)op(SAMPPROD)(b) > SAMP_MAX || \
+ (SAMPPROD)(a)op(SAMPPROD)(b) < SAMP_MIN) { \
+ fprintf(stderr, \
+ "WARNING:overflow @ " __FILE__ "(%d): (%d " #op " %d) = %ld\n", \
+ __LINE__, (a), (b), (SAMPPROD)(a)op(SAMPPROD)(b)); \
+ }
+#endif
+
+#define smul(a, b) ((SAMPPROD)(a) * (b))
+#define sround(x) (kiss_fft_scalar)(((x) + (1 << (FRACBITS - 1))) >> FRACBITS)
+
+#define S_MUL(a, b) sround(smul(a, b))
+
+#define C_MUL(m, a, b) \
+ do { \
+ (m).r = sround(smul((a).r, (b).r) - smul((a).i, (b).i)); \
+ (m).i = sround(smul((a).r, (b).i) + smul((a).i, (b).r)); \
+ } while (0)
+
+#define DIVSCALAR(x, k) (x) = sround(smul(x, SAMP_MAX / k))
+
+#define C_FIXDIV(c, div) \
+ do { \
+ DIVSCALAR((c).r, div); \
+ DIVSCALAR((c).i, div); \
+ } while (0)
+
+#define C_MULBYSCALAR(c, s) \
+ do { \
+ (c).r = sround(smul((c).r, s)); \
+ (c).i = sround(smul((c).i, s)); \
+ } while (0)
+
+#else /* not FIXED_POINT*/
+
+#define S_MUL(a, b) ((a) * (b))
+#define C_MUL(m, a, b) \
+ do { \
+ (m).r = (a).r * (b).r - (a).i * (b).i; \
+ (m).i = (a).r * (b).i + (a).i * (b).r; \
+ } while (0)
+#define C_FIXDIV(c, div) /* NOOP */
+#define C_MULBYSCALAR(c, s) \
+ do { \
+ (c).r *= (s); \
+ (c).i *= (s); \
+ } while (0)
+#endif
+
+#ifndef CHECK_OVERFLOW_OP
+#define CHECK_OVERFLOW_OP(a, op, b) /* noop */
+#endif
+
+#define C_ADD(res, a, b) \
+ do { \
+ CHECK_OVERFLOW_OP((a).r, +, (b).r) \
+ CHECK_OVERFLOW_OP((a).i, +, (b).i) \
+ (res).r = (a).r + (b).r; \
+ (res).i = (a).i + (b).i; \
+ } while (0)
+#define C_SUB(res, a, b) \
+ do { \
+ CHECK_OVERFLOW_OP((a).r, -, (b).r) \
+ CHECK_OVERFLOW_OP((a).i, -, (b).i) \
+ (res).r = (a).r - (b).r; \
+ (res).i = (a).i - (b).i; \
+ } while (0)
+#define C_ADDTO(res, a) \
+ do { \
+ CHECK_OVERFLOW_OP((res).r, +, (a).r) \
+ CHECK_OVERFLOW_OP((res).i, +, (a).i) \
+ (res).r += (a).r; \
+ (res).i += (a).i; \
+ } while (0)
+
+#define C_SUBFROM(res, a) \
+ do { \
+ CHECK_OVERFLOW_OP((res).r, -, (a).r) \
+ CHECK_OVERFLOW_OP((res).i, -, (a).i) \
+ (res).r -= (a).r; \
+ (res).i -= (a).i; \
+ } while (0)
+
+#ifdef FIXED_POINT
+#define KISS_FFT_COS(phase) floorf(.5 + SAMP_MAX * cosf(phase))
+#define KISS_FFT_SIN(phase) floorf(.5 + SAMP_MAX * sinf(phase))
+#define HALF_OF(x) ((x) >> 1)
+#elif defined(USE_SIMD)
+#define KISS_FFT_COS(phase) _mm_set1_ps(cosf(phase))
+#define KISS_FFT_SIN(phase) _mm_set1_ps(sinf(phase))
+#define HALF_OF(x) ((x)*_mm_set1_ps(.5))
+#else
+#define KISS_FFT_COS(phase) (kiss_fft_scalar) cosf(phase)
+#define KISS_FFT_SIN(phase) (kiss_fft_scalar) sinf(phase)
+#define HALF_OF(x) ((x)*.5)
+#endif
+
+#define kf_cexp(x, phase) \
+ do { \
+ (x)->r = KISS_FFT_COS(phase); \
+ (x)->i = KISS_FFT_SIN(phase); \
+ } while (0)
+
+/* a debugging function */
+#define pcpx(c) \
+ fprintf(stderr, "%g + %gi\n", (double)((c)->r), (double)((c)->i))
+
+#ifdef KISS_FFT_USE_ALLOCA
+// define this to allow use of alloca instead of malloc for temporary buffers
+// Temporary buffers are used in two case:
+// 1. FFT sizes that have "bad" factors. i.e. not 2,3 and 5
+// 2. "in-place" FFTs. Notice the quotes, since kissfft does not really do an
+// in-place transform.
+#include <alloca.h>
+#define KISS_FFT_TMP_ALLOC(nbytes) alloca(nbytes)
+#define KISS_FFT_TMP_FREE(ptr)
+#else
+#define KISS_FFT_TMP_ALLOC(nbytes) KISS_FFT_MALLOC(nbytes)
+#define KISS_FFT_TMP_FREE(ptr) KISS_FFT_FREE(ptr)
+#endif
diff --git a/src/codec2/codec2_fdmdv.h b/src/codec2/codec2_fdmdv.h
new file mode 100644
index 0000000000000000000000000000000000000000..79e699dbfbc854c1f7639a5fbca8effb4a395f7b
--- /dev/null
+++ b/src/codec2/codec2_fdmdv.h
@@ -0,0 +1,138 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: codec2_fdmdv.h
+ AUTHOR......: David Rowe
+ DATE CREATED: April 14 2012
+
+ A 1400 bit/s (nominal) Frequency Division Multiplexed Digital Voice
+ (FDMDV) modem. Used for digital audio over HF SSB. See
+ README_fdmdv.txt for more information, and fdmdv_mod.c and
+ fdmdv_demod.c for example usage.
+
+ The name codec2_fdmdv.h is used to make it unique when "make
+ installed".
+
+ References:
+
+ [1] http://n1su.com/fdmdv/FDMDV_Docs_Rel_1_4b.pdf
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ 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/>.
+*/
+
+#ifndef __FDMDV__
+#define __FDMDV__
+
+#include "comp.h"
+#include "modem_stats.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* set up the calling convention for DLL function import/export for
+ WIN32 cross compiling */
+
+#ifdef __CODEC2_WIN32__
+#ifdef __CODEC2_BUILDING_DLL__
+#define CODEC2_WIN32SUPPORT __declspec(dllexport) __stdcall
+#else
+#define CODEC2_WIN32SUPPORT __declspec(dllimport) __stdcall
+#endif
+#else
+#define CODEC2_WIN32SUPPORT
+#endif
+
+#define FDMDV_NC \
+ 14 /* default number of data carriers */
+#define FDMDV_NC_MAX \
+ 20 /* maximum number of data carriers */
+#define FDMDV_BITS_PER_FRAME \
+ 28 /* 20ms frames, for nominal 1400 bit/s */
+#define FDMDV_NOM_SAMPLES_PER_FRAME \
+ 160 /* modulator output samples/frame and nominal demod samples/frame */
+ /* at 8000 Hz sample rate */
+#define FDMDV_MAX_SAMPLES_PER_FRAME \
+ 200 /* max demod samples/frame, use this to allocate storage */
+#define FDMDV_SCALE \
+ 825 /* suggested scaling for 16 bit shorts */
+#define FDMDV_FCENTRE \
+ 1500 /* Centre frequency, Nc/2 carriers below this, Nc/2 carriers above (Hz) \
+ */
+
+/* 8 to 18 kHz sample rate conversion */
+
+#define FDMDV_OS 2 /* oversampling rate */
+#define FDMDV_OS_TAPS_16K 48 /* number of OS filter taps at 16kHz */
+#define FDMDV_OS_TAPS_8K \
+ (FDMDV_OS_TAPS_16K / FDMDV_OS) /* number of OS filter taps at 8kHz */
+
+/* 8 to 48 kHz sample rate conversion */
+
+#define FDMDV_OS_48 6 /* oversampling rate */
+#define FDMDV_OS_TAPS_48K 48 /* number of OS filter taps at 48kHz */
+#define FDMDV_OS_TAPS_48_8K \
+ (FDMDV_OS_TAPS_48K / FDMDV_OS_48) /* number of OS filter taps at 8kHz */
+
+/* FDMDV states and stats structures */
+
+struct FDMDV;
+
+struct FDMDV *fdmdv_create(int Nc);
+void fdmdv_destroy(struct FDMDV *fdmdv_state);
+void fdmdv_use_old_qpsk_mapping(struct FDMDV *fdmdv_state);
+int fdmdv_bits_per_frame(struct FDMDV *fdmdv_state);
+float fdmdv_get_fsep(struct FDMDV *fdmdv_state);
+void fdmdv_set_fsep(struct FDMDV *fdmdv_state, float fsep);
+
+void fdmdv_mod(struct FDMDV *fdmdv_state, COMP tx_fdm[], int tx_bits[],
+ int *sync_bit);
+void fdmdv_demod(struct FDMDV *fdmdv_state, int rx_bits[],
+ int *reliable_sync_bit, COMP rx_fdm[], int *nin);
+
+void fdmdv_get_test_bits(struct FDMDV *fdmdv_state, int tx_bits[]);
+int fdmdv_error_pattern_size(struct FDMDV *fdmdv_state);
+void fdmdv_put_test_bits(struct FDMDV *f, int *sync, short error_pattern[],
+ int *bit_errors, int *ntest_bits, int rx_bits[]);
+
+void fdmdv_get_demod_stats(struct FDMDV *fdmdv_state,
+ struct MODEM_STATS *stats);
+
+void fdmdv_8_to_16(float out16k[], float in8k[], int n);
+void fdmdv_8_to_16_short(short out16k[], short in8k[], int n);
+void fdmdv_16_to_8(float out8k[], float in16k[], int n);
+void fdmdv_16_to_8_short(short out8k[], short in16k[], int n);
+void fdmdv_8_to_48(float out48k[], float in8k[], int n);
+void fdmdv_48_to_8(float out8k[], float in48k[], int n);
+void fdmdv_8_to_48_short(short out48k[], short in8k[], int n);
+void fdmdv_48_to_8_short(short out8k[], short in48k[], int n);
+
+void fdmdv_freq_shift(COMP rx_fdm_fcorr[], COMP rx_fdm[], float foff,
+ COMP *foff_phase_rect, int nin);
+
+/* debug/development function(s) */
+
+void fdmdv_dump_osc_mags(struct FDMDV *f);
+void fdmdv_simulate_channel(float *sig_pwr_av, COMP samples[], int nin,
+ float target_snr);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/codec2/comp.h b/src/codec2/comp.h
new file mode 100644
index 0000000000000000000000000000000000000000..ffc20c163d4f5a0523c50bfbc1b890c931239a44
--- /dev/null
+++ b/src/codec2/comp.h
@@ -0,0 +1,38 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: comp.h
+ AUTHOR......: David Rowe
+ DATE CREATED: 24/08/09
+
+ Complex number definition.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2009 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/>.
+*/
+
+#ifndef __COMP__
+#define __COMP__
+
+/* Complex number */
+
+typedef struct {
+ float real;
+ float imag;
+} COMP;
+
+#endif
diff --git a/src/codec2/comp_prim.h b/src/codec2/comp_prim.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0166d3d14f330ee46b1b7af8836d2f4dd57631d
--- /dev/null
+++ b/src/codec2/comp_prim.h
@@ -0,0 +1,138 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: comp_prim.h
+ AUTHOR......: David Rowe
+ DATE CREATED: Marh 2015
+
+ Complex number maths primitives.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2015 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/>.
+*/
+
+#ifndef __COMP_PRIM__
+#define __COMP_PRIM__
+
+#include <math.h>
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTIONS
+
+\*---------------------------------------------------------------------------*/
+
+inline static COMP cneg(COMP a) {
+ COMP res;
+
+ res.real = -a.real;
+ res.imag = -a.imag;
+
+ return res;
+}
+
+inline static COMP cconj(COMP a) {
+ COMP res;
+
+ res.real = a.real;
+ res.imag = -a.imag;
+
+ return res;
+}
+
+inline static COMP cmult(COMP a, COMP b) {
+ COMP res;
+
+ res.real = a.real * b.real - a.imag * b.imag;
+ res.imag = a.real * b.imag + a.imag * b.real;
+
+ return res;
+}
+
+inline static COMP fcmult(float a, COMP b) {
+ COMP res;
+
+ res.real = a * b.real;
+ res.imag = a * b.imag;
+
+ return res;
+}
+
+inline static COMP cadd(COMP a, COMP b) {
+ COMP res;
+
+ res.real = a.real + b.real;
+ res.imag = a.imag + b.imag;
+
+ return res;
+}
+
+inline static float cabsolute(COMP a) {
+ return sqrtf((a.real * a.real) + (a.imag * a.imag));
+}
+
+/*
+ * Euler's formula in a new convenient function
+ */
+inline static COMP comp_exp_j(float phi) {
+ COMP res;
+ res.real = cosf(phi);
+ res.imag = sinf(phi);
+ return res;
+}
+
+/*
+ * Quick and easy complex 0
+ */
+inline static COMP comp0() {
+ COMP res;
+ res.real = 0;
+ res.imag = 0;
+ return res;
+}
+
+/*
+ * Quick and easy complex subtract
+ */
+inline static COMP csub(COMP a, COMP b) {
+ COMP res;
+ res.real = a.real - b.real;
+ res.imag = a.imag - b.imag;
+ return res;
+}
+
+/*
+ * Compare the magnitude of a and b. if |a|>|b|, return true, otw false.
+ * This needs no square roots
+ */
+inline static int comp_mag_gt(COMP a, COMP b) {
+ return ((a.real * a.real) + (a.imag * a.imag)) >
+ ((b.real * b.real) + (b.imag * b.imag));
+}
+
+/*
+ * Normalize a complex number's magnitude to 1
+ */
+inline static COMP comp_normalize(COMP a) {
+ COMP b;
+ float av = cabsolute(a);
+ b.real = a.real / av;
+ b.imag = a.imag / av;
+ return b;
+}
+
+#endif
diff --git a/src/codec2/fsk.c b/src/codec2/fsk.c
new file mode 100644
index 0000000000000000000000000000000000000000..fe83cc6009025beec5deff3873c1fc1b6844795c
--- /dev/null
+++ b/src/codec2/fsk.c
@@ -0,0 +1,1060 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: fsk.c
+ AUTHOR......: Brady O'Brien & David Rowe
+ DATE CREATED: 7 January 2016
+
+ C Implementation of 2/4FSK modulator/demodulator, based on octave/fsk_lib.m
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2016-2020 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/>.
+*/
+
+/*---------------------------------------------------------------------------*\
+
+ DEFINES
+
+\*---------------------------------------------------------------------------*/
+
+/* Define this to enable EbNodB estimate */
+/* This needs square roots, may take more cpu time than it's worth */
+#define EST_EBNO
+
+/* This is a flag for the freq. estimator to use a precomputed/rt computed hann
+ window table On platforms with slow cosf, this will produce a substantial
+ speedup at the cost of a small amount of memory
+*/
+#define USE_HANN_TABLE
+
+/* This flag turns on run-time hann table generation. If USE_HANN_TABLE is
+ unset, this flag has no effect. If USE_HANN_TABLE is set and this flag is
+ set, the hann table will be allocated and generated when fsk_init or
+ fsk_init_hbr is called. If this flag is not set, a hann function table of
+ size fsk->Ndft MUST be provided. On small platforms, this can be used with a
+ precomputed table to save memory at the cost of flash space.
+*/
+#define GENERATE_HANN_TABLE_RUNTIME
+
+/* Turn off table generation if on cortex M4 to save memory */
+#ifdef CORTEX_M4
+#undef USE_HANN_TABLE
+#endif
+
+/*---------------------------------------------------------------------------*\
+
+ INCLUDES
+
+\*---------------------------------------------------------------------------*/
+
+#include "fsk.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "comp_prim.h"
+#include "kiss_fftr.h"
+#include "modem_probe.h"
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTIONS
+
+\*---------------------------------------------------------------------------*/
+
+static void stats_init(struct FSK *fsk);
+
+#ifdef USE_HANN_TABLE
+/*
+ This is used by fsk_create and fsk_create_hbr to generate a hann function
+ table
+*/
+static void fsk_generate_hann_table(struct FSK *fsk) {
+ int Ndft = fsk->Ndft;
+ size_t i;
+
+ for (i = 0; i < Ndft; i++) {
+ fsk->hann_table[i] =
+ 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float)(Ndft - 1));
+ }
+}
+#endif
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_create_core
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 7 January 2016
+
+ In this version of the demod the standard/hbr modes have been
+ largely combined at they shared so much common code. The
+ fsk_create/fsk_create_hbr function interface has been retained to
+ maximise compatibility with existing applications.
+
+\*---------------------------------------------------------------------------*/
+
+struct FSK *fsk_create_core(int Fs, int Rs, int M, int P, int Nsym, int f1_tx,
+ int tone_spacing) {
+ struct FSK *fsk;
+ int i;
+
+ /* Check configuration validity */
+ assert(Fs > 0);
+ assert(Rs > 0);
+ assert(P > 0);
+ assert(Nsym > 0);
+ /* Ts (Fs/Rs) must be an integer */
+ assert((Fs % Rs) == 0);
+ /* Ts/P (Fs/Rs/P) must be an integer */
+ assert(((Fs / Rs) % P) == 0);
+ /* If P is too low we don't have a good choice of timing offsets to choose
+ * from */
+ assert(P >= 4);
+ assert(M == 2 || M == 4);
+
+ fsk = (struct FSK *)calloc(1, sizeof(struct FSK));
+ assert(fsk != NULL);
+
+ // Need enough bins to within 10% of tone centre
+ float bin_width_Hz = 0.1 * Rs;
+ float Ndft = (float)Fs / bin_width_Hz;
+ Ndft = pow(2.0, ceil(log2(Ndft)));
+
+ /* Set constant config parameters */
+ fsk->Fs = Fs;
+ fsk->Rs = Rs;
+ fsk->Ts = Fs / Rs;
+ fsk->burst_mode = 0;
+ fsk->P = P;
+ fsk->Nsym = Nsym;
+ fsk->N = fsk->Ts * fsk->Nsym;
+ fsk->Ndft = Ndft;
+ fsk->tc = 0.1;
+ fsk->Nmem = fsk->N + (2 * fsk->Ts);
+ fsk->f1_tx = f1_tx;
+ fsk->tone_spacing = tone_spacing;
+ fsk->nin = fsk->N;
+ fsk->lock_nin = 0;
+ fsk->mode = M == 2 ? MODE_2FSK : MODE_4FSK;
+ fsk->Nbits = M == 2 ? fsk->Nsym : fsk->Nsym * 2;
+ fsk->est_min = 0;
+ fsk->est_max = Fs;
+ fsk->est_space = 0.75 * Rs;
+ fsk->freq_est_type = 0;
+
+ // printf("C.....: M: %d Fs: %d Rs: %d Ts: %d nsym: %d nbit: %d N: %d Ndft: %d
+ // fmin: %d fmax: %d\n",
+ // M, fsk->Fs, fsk->Rs, fsk->Ts, fsk->Nsym, fsk->Nbits, fsk->N,
+ // fsk->Ndft, fsk->est_min, fsk->est_max);
+ /* Set up rx state */
+ for (i = 0; i < M; i++) fsk->phi_c[i] = comp_exp_j(0);
+ fsk->f_dc = (COMP *)malloc(M * fsk->Nmem * sizeof(COMP));
+ assert(fsk->f_dc != NULL);
+ for (i = 0; i < M * fsk->Nmem; i++) fsk->f_dc[i] = comp0();
+
+ fsk->fft_cfg = kiss_fft_alloc(Ndft, 0, NULL, NULL);
+ assert(fsk->fft_cfg != NULL);
+ fsk->Sf = (float *)malloc(sizeof(float) * fsk->Ndft);
+ assert(fsk->Sf != NULL);
+ for (i = 0; i < Ndft; i++) fsk->Sf[i] = 0;
+
+#ifdef USE_HANN_TABLE
+#ifdef GENERATE_HANN_TABLE_RUNTIME
+ fsk->hann_table = (float *)malloc(sizeof(float) * fsk->Ndft);
+ assert(fsk->hann_table != NULL);
+ fsk_generate_hann_table(fsk);
+#else
+ fsk->hann_table = NULL;
+#endif
+#endif
+
+ fsk->norm_rx_timing = 0;
+
+ /* Set up tx state */
+ fsk->tx_phase_c = comp_exp_j(0);
+
+ /* Set up demod stats */
+ fsk->EbNodB = 0;
+
+ for (i = 0; i < M; i++) fsk->f_est[i] = 0;
+
+ fsk->ppm = 0;
+
+ fsk->stats = (struct MODEM_STATS *)malloc(sizeof(struct MODEM_STATS));
+ assert(fsk->stats != NULL);
+ stats_init(fsk);
+ fsk->normalise_eye = 1;
+
+ return fsk;
+}
+
+/*---------------------------------------------------------------------------* \
+
+ FUNCTION....: fsk_create
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 7 January 2016
+
+ Create and initialize an instance of the FSK modem. Returns a pointer
+ to the modem state/config struct. One modem config struct may be used
+ for both mod and demod.
+
+ If you are not intending to use the modulation functions, you can
+ set f1_tx to FSK_NONE.
+
+\*---------------------------------------------------------------------------*/
+
+struct FSK *fsk_create(int Fs, int Rs, int M, int tx_f1, int tx_fs) {
+ return fsk_create_core(Fs, Rs, M, FSK_DEFAULT_P, FSK_DEFAULT_NSYM, tx_f1,
+ tx_fs);
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_create_hbr
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ Alternate version of create allows user defined oversampling P and
+ averaging window Nsym. In the current version of the demod it's
+ simply an alias for the default core function.
+
+ P is the oversampling rate of the internal demod processing, which
+ happens at Rs*P Hz. We filter the tones at P different timing
+ offsets, and choose the best one. P should be >=8, so we have a
+ choice of at least 8 timing offsets. This may require some
+ adjustment of Fs and Rs, as Fs/Rs/P must be an integer.
+
+ Nsym is the number of symbols we average demod parameters like
+ symbol timing over.
+
+\*---------------------------------------------------------------------------*/
+
+struct FSK *fsk_create_hbr(int Fs, int Rs, int M, int P, int Nsym, int f1_tx,
+ int tone_spacing) {
+ return fsk_create_core(Fs, Rs, M, P, Nsym, f1_tx, tone_spacing);
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_destroy
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ Call this to free all memory and shut down the modem.
+
+\*---------------------------------------------------------------------------*/
+
+void fsk_destroy(struct FSK *fsk) {
+ free(fsk->Sf);
+ free(fsk->f_dc);
+ free(fsk->fft_cfg);
+ free(fsk->stats);
+ free(fsk->hann_table);
+ free(fsk);
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_mod
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ FSK modulator function, real valued output samples with amplitude 2.
+
+\*---------------------------------------------------------------------------*/
+
+void fsk_mod(struct FSK *fsk, float fsk_out[], uint8_t tx_bits[], int nbits) {
+ COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
+ int f1_tx = fsk->f1_tx; /* '0' frequency */
+ int tone_spacing = fsk->tone_spacing; /* space between frequencies */
+ int Ts = fsk->Ts; /* samples-per-symbol */
+ int Fs = fsk->Fs; /* sample freq */
+ int M = fsk->mode;
+ COMP dosc_f[M]; /* phase shift per sample */
+ COMP dph; /* phase shift of current bit */
+ size_t i, j, m, bit_i, sym;
+
+ /* trap these parameters being set to FSK_UNUSED, then calling mod */
+ assert(f1_tx > 0);
+ assert(tone_spacing > 0);
+
+ /* Init the per sample phase shift complex numbers */
+ for (m = 0; m < M; m++) {
+ dosc_f[m] = comp_exp_j(2 * M_PI *
+ ((float)(f1_tx + (tone_spacing * m)) / (float)(Fs)));
+ }
+
+ bit_i = 0;
+ int nsym = nbits / (M >> 1);
+ for (i = 0; i < nsym; i++) {
+ sym = 0;
+ /* Pack the symbol number from the bit stream */
+ for (m = M; m >>= 1;) {
+ uint8_t bit = tx_bits[bit_i];
+ bit = (bit == 1) ? 1 : 0;
+ sym = (sym << 1) | bit;
+ bit_i++;
+ }
+ /* Look up symbol phase shift */
+ dph = dosc_f[sym];
+ /* Spin the oscillator for a symbol period */
+ for (j = 0; j < Ts; j++) {
+ tx_phase_c = cmult(tx_phase_c, dph);
+ fsk_out[i * Ts + j] = 2 * tx_phase_c.real;
+ }
+ }
+
+ /* Normalize TX phase to prevent drift */
+ tx_phase_c = comp_normalize(tx_phase_c);
+
+ /* save TX phase */
+ fsk->tx_phase_c = tx_phase_c;
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_mod_c
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ FSK modulator function, complex valued output samples with magnitude 1.
+
+\*---------------------------------------------------------------------------*/
+
+void fsk_mod_c(struct FSK *fsk, COMP fsk_out[], uint8_t tx_bits[], int nbits) {
+ COMP tx_phase_c = fsk->tx_phase_c; /* Current complex TX phase */
+ int f1_tx = fsk->f1_tx; /* '0' frequency */
+ int tone_spacing = fsk->tone_spacing; /* space between frequencies */
+ int Ts = fsk->Ts; /* samples-per-symbol */
+ int Fs = fsk->Fs; /* sample freq */
+ int M = fsk->mode;
+ COMP dosc_f[M]; /* phase shift per sample */
+ COMP dph; /* phase shift of current bit */
+ size_t i, j, bit_i, sym;
+ int m;
+
+ /* trap these parameters being set to FSK_UNUSED, then calling mod */
+ assert(f1_tx > 0);
+ assert(tone_spacing > 0);
+
+ /* Init the per sample phase shift complex numbers */
+ for (m = 0; m < M; m++) {
+ dosc_f[m] = comp_exp_j(2 * M_PI *
+ ((float)(f1_tx + (tone_spacing * m)) / (float)(Fs)));
+ }
+
+ bit_i = 0;
+ int nsym = nbits / (M >> 1);
+ for (i = 0; i < nsym; i++) {
+ sym = 0;
+ /* Pack the symbol number from the bit stream */
+ for (m = M; m >>= 1;) {
+ uint8_t bit = tx_bits[bit_i];
+ bit = (bit == 1) ? 1 : 0;
+ sym = (sym << 1) | bit;
+ bit_i++;
+ }
+ /* Look up symbol phase shift */
+ dph = dosc_f[sym];
+ /* Spin the oscillator for a symbol period */
+ for (j = 0; j < Ts; j++) {
+ tx_phase_c = cmult(tx_phase_c, dph);
+ fsk_out[i * Ts + j] = tx_phase_c;
+ }
+ }
+
+ /* Normalize TX phase to prevent drift */
+ tx_phase_c = comp_normalize(tx_phase_c);
+
+ /* save TX phase */
+ fsk->tx_phase_c = tx_phase_c;
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_mod_ext_vco
+ AUTHOR......: David Rowe
+ DATE CREATED: February 2018
+
+ Modulator that assume an external VCO. The output is a voltage
+ that changes for each symbol.
+
+\*---------------------------------------------------------------------------*/
+
+void fsk_mod_ext_vco(struct FSK *fsk, float vco_out[], uint8_t tx_bits[],
+ int nbits) {
+ int f1_tx = fsk->f1_tx; /* '0' frequency */
+ int tone_spacing = fsk->tone_spacing; /* space between frequencies */
+ int Ts = fsk->Ts; /* samples-per-symbol */
+ int M = fsk->mode;
+ int i, j, m, sym, bit_i;
+
+ /* trap these parameters being set to FSK_UNUSED, then calling mod */
+ assert(f1_tx > 0);
+ assert(tone_spacing > 0);
+
+ bit_i = 0;
+ int nsym = nbits / (M >> 1);
+ for (i = 0; i < nsym; i++) {
+ /* generate the symbol number from the bit stream,
+ e.g. 0,1 for 2FSK, 0,1,2,3 for 4FSK */
+
+ sym = 0;
+
+ /* unpack the symbol number from the bit stream */
+
+ for (m = M; m >>= 1;) {
+ uint8_t bit = tx_bits[bit_i];
+ bit = (bit == 1) ? 1 : 0;
+ sym = (sym << 1) | bit;
+ bit_i++;
+ }
+
+ /*
+ Map 'sym' to VCO frequency
+ Note: drive is inverted, a higher tone drives VCO voltage lower
+ */
+
+ // fprintf(stderr, "i: %d sym: %d freq: %f\n", i, sym, f1_tx +
+ // tone_spacing*(float)sym);
+ for (j = 0; j < Ts; j++) {
+ vco_out[i * Ts + j] = f1_tx + tone_spacing * (float)sym;
+ }
+ }
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_nin
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ Call me before each call to fsk_demod() to determine how many new
+ samples you should pass in. the number of samples will vary due to
+ timing variations.
+
+\*---------------------------------------------------------------------------*/
+
+uint32_t fsk_nin(struct FSK *fsk) { return (uint32_t)fsk->nin; }
+
+/*
+ * Internal function to estimate the frequencies of the FSK tones.
+ * This is split off because it is fairly complicated, needs a bunch of memory,
+ * and probably takes more cycles than the rest of the demod. Parameters: fsk -
+ * FSK struct from demod containing FSK config fsk_in - block of samples in this
+ * demod cycles, must be nin long freqs - Array for the estimated frequencies M
+ * - number of frequency peaks to find
+ */
+void fsk_demod_freq_est(struct FSK *fsk, COMP fsk_in[], float *freqs, int M) {
+ int Ndft = fsk->Ndft;
+ int Fs = fsk->Fs;
+ int nin = fsk->nin;
+ size_t i, j;
+ float hann;
+ float max;
+ int imax;
+ kiss_fft_cfg fft_cfg = fsk->fft_cfg;
+ int freqi[M];
+ int st, en, f_zero;
+
+ kiss_fft_cpx *fftin = (kiss_fft_cpx *)malloc(sizeof(kiss_fft_cpx) * Ndft);
+ kiss_fft_cpx *fftout = (kiss_fft_cpx *)malloc(sizeof(kiss_fft_cpx) * Ndft);
+
+ st = (fsk->est_min * Ndft) / Fs + Ndft / 2;
+ if (st < 0) st = 0;
+ en = (fsk->est_max * Ndft) / Fs + Ndft / 2;
+ if (en > Ndft) en = Ndft;
+ // fprintf(stderr, "min: %d max: %d st: %d en: %d\n", fsk->est_min,
+ // fsk->est_max, st, en);
+
+ f_zero = (fsk->est_space * Ndft) / Fs;
+ // fprintf(stderr, "fsk->est_space: %d f_zero = %d\n", fsk->est_space,
+ // f_zero);
+
+ int numffts = floor((float)nin / (Ndft / 2)) - 1;
+ for (j = 0; j < numffts; j++) {
+ int a = j * Ndft / 2;
+ // fprintf(stderr, "numffts: %d j: %d a: %d\n", numffts, (int)j, a);
+ /* Copy FSK buffer into reals of FFT buffer and apply a hann window */
+ for (i = 0; i < Ndft; i++) {
+#ifdef USE_HANN_TABLE
+ hann = fsk->hann_table[i];
+#else
+ hann = 0.5 - 0.5 * cosf(2.0 * M_PI * (float)i / (float)(fft_samps - 1));
+#endif
+ fftin[i].r = hann * fsk_in[i + a].real;
+ fftin[i].i = hann * fsk_in[i + a].imag;
+ }
+
+ /* Do the FFT */
+ kiss_fft(fft_cfg, fftin, fftout);
+
+ /* FFT shift to put DC bin at Ndft/2 */
+ kiss_fft_cpx tmp;
+ for (i = 0; i < Ndft / 2; i++) {
+ tmp = fftout[i];
+ fftout[i] = fftout[i + Ndft / 2];
+ fftout[i + Ndft / 2] = tmp;
+ }
+
+ /* Find the magnitude^2 of each freq slot */
+ for (i = 0; i < Ndft; i++) {
+ fftout[i].r = (fftout[i].r * fftout[i].r) + (fftout[i].i * fftout[i].i);
+ }
+
+ /* Mix back in with the previous fft block */
+ /* Copy new fft est into imag of fftout for frequency divination below */
+ float tc = fsk->tc;
+ for (i = 0; i < Ndft; i++) {
+ fsk->Sf[i] = (fsk->Sf[i] * (1 - tc)) + (sqrtf(fftout[i].r) * tc);
+ fftout[i].i = fsk->Sf[i];
+ }
+ }
+
+ modem_probe_samp_f("t_Sf", fsk->Sf, Ndft);
+
+ max = 0;
+ /* Find the M frequency peaks here */
+ for (i = 0; i < M; i++) {
+ imax = 0;
+ max = 0;
+ for (j = st; j < en; j++) {
+ if (fftout[j].i > max) {
+ max = fftout[j].i;
+ imax = j;
+ }
+ }
+ /* Blank out FMax +/-Fspace/2 */
+ int f_min, f_max;
+ f_min = imax - f_zero;
+ f_min = f_min < 0 ? 0 : f_min;
+ f_max = imax + f_zero;
+ f_max = f_max > Ndft ? Ndft : f_max;
+ for (j = f_min; j < f_max; j++) fftout[j].i = 0;
+
+ /* Stick the freq index on the list */
+ freqi[i] = imax - Ndft / 2;
+ }
+
+ /* Gnome sort the freq list */
+ /* My favorite sort of sort*/
+ i = 1;
+ while (i < M) {
+ if (freqi[i] >= freqi[i - 1])
+ i++;
+ else {
+ j = freqi[i];
+ freqi[i] = freqi[i - 1];
+ freqi[i - 1] = j;
+ if (i > 1) i--;
+ }
+ }
+
+ /* Convert freqs from indices to frequencies */
+ for (i = 0; i < M; i++) {
+ freqs[i] = (float)(freqi[i]) * ((float)Fs / (float)Ndft);
+ }
+
+ /* Search for each tone method 2 - correlate with mask with non-zero entries
+ * at tone spacings ----- */
+
+ /* construct mask */
+ float mask[Ndft];
+ for (i = 0; i < Ndft; i++) mask[i] = 0.0;
+ for (i = 0; i < 3; i++) mask[i] = 1.0;
+ int bin = 0;
+ for (int m = 1; m <= M - 1; m++) {
+ bin = round((float)m * fsk->tone_spacing * Ndft / Fs) - 1;
+ for (i = bin; i <= bin + 2; i++) mask[i] = 1.0;
+ }
+ int len_mask = bin + 2 + 1;
+
+#ifdef MODEMPROBE_ENABLE
+ modem_probe_samp_f("t_mask", mask, len_mask);
+#endif
+
+ /* drag mask over Sf, looking for peak in correlation */
+ int b_max = st;
+ float corr_max = 0.0;
+ float *Sf = fsk->Sf;
+ for (int b = st; b < en - len_mask; b++) {
+ float corr = 0.0;
+ for (i = 0; i < len_mask; i++) corr += mask[i] * Sf[b + i];
+ if (corr > corr_max) {
+ corr_max = corr;
+ b_max = b;
+ }
+ }
+ float foff = (b_max - Ndft / 2) * Fs / Ndft;
+ // fprintf(stderr, "fsk->tone_spacing: %d\n",fsk->tone_spacing);
+ for (int m = 0; m < M; m++) fsk->f2_est[m] = foff + m * fsk->tone_spacing;
+
+#ifdef MODEMPROBE_ENABLE
+ modem_probe_samp_f("t_f2_est", fsk->f2_est, M);
+#endif
+
+ free(fftin);
+ free(fftout);
+}
+
+/* core demodulator function */
+void fsk_demod_core(struct FSK *fsk, uint8_t rx_bits[], float rx_filt[],
+ COMP fsk_in[]) {
+ int N = fsk->N;
+ int Ts = fsk->Ts;
+ int Rs = fsk->Rs;
+ int Fs = fsk->Fs;
+ int nsym = fsk->Nsym;
+ int nin = fsk->nin;
+ int P = fsk->P;
+ int Nmem = fsk->Nmem;
+ int M = fsk->mode;
+ size_t i, j, m;
+ float ft1;
+
+ COMP t[M]; /* complex number temps */
+ COMP t_c; /* another complex temp */
+ COMP *phi_c = fsk->phi_c;
+ COMP *f_dc = fsk->f_dc;
+ COMP phi_ft;
+ int nold = Nmem - nin;
+
+ COMP dphift;
+ float rx_timing, norm_rx_timing, old_norm_rx_timing, d_norm_rx_timing, appm;
+
+ float fc_avg, fc_tx;
+ float meanebno, stdebno, eye_max;
+ int neyesamp, neyeoffset;
+
+#ifdef MODEMPROBE_ENABLE
+#define NMP_NAME 26
+ char mp_name_tmp[NMP_NAME +
+ 1]; /* Temporary string for modem probe trace names */
+#endif
+
+ /* Estimate tone frequencies */
+ fsk_demod_freq_est(fsk, fsk_in, fsk->f_est, M);
+#ifdef MODEMPROBE_ENABLE
+ modem_probe_samp_f("t_f_est", fsk->f_est, M);
+#endif
+ float *f_est;
+ if (fsk->freq_est_type)
+ f_est = fsk->f2_est;
+ else
+ f_est = fsk->f_est;
+
+ /* update filter (integrator) memory by shifting in nin samples */
+ for (m = 0; m < M; m++) {
+ for (i = 0, j = Nmem - nold; i < nold; i++, j++)
+ f_dc[m * Nmem + i] = f_dc[m * Nmem + j];
+ }
+
+ /* freq shift down to around DC, ensuring continuous phase from last frame */
+ COMP dphi_m;
+ for (m = 0; m < M; m++) {
+ dphi_m = comp_exp_j(2 * M_PI * ((f_est[m]) / (float)(Fs)));
+ for (i = nold, j = 0; i < Nmem; i++, j++) {
+ phi_c[m] = cmult(phi_c[m], dphi_m);
+ f_dc[m * Nmem + i] = cmult(fsk_in[j], cconj(phi_c[m]));
+ }
+ phi_c[m] = comp_normalize(phi_c[m]);
+#ifdef MODEMPROBE_ENABLE
+ snprintf(mp_name_tmp, NMP_NAME, "t_f%zd_dc", m + 1);
+ modem_probe_samp_c(mp_name_tmp, &f_dc[m * Nmem], Nmem);
+#endif
+ }
+
+ /* integrate over symbol period at a variety of offsets */
+ COMP f_int[M][(nsym + 1) * P];
+ for (i = 0; i < (nsym + 1) * P; i++) {
+ int st = i * Ts / P;
+ int en = st + Ts - 1;
+ for (m = 0; m < M; m++) {
+ f_int[m][i] = comp0();
+ for (j = st; j <= en; j++)
+ f_int[m][i] = cadd(f_int[m][i], f_dc[m * Nmem + j]);
+ }
+ }
+
+#ifdef MODEMPROBE_ENABLE
+ for (m = 0; m < M; m++) {
+ snprintf(mp_name_tmp, NMP_NAME, "t_f%zd_int", m + 1);
+ modem_probe_samp_c(mp_name_tmp, &f_int[m][0], (nsym + 1) * P);
+ }
+#endif
+
+ /* Fine Timing Estimation */
+ /* Apply magic nonlinearity to f1_int and f2_int, shift down to 0,
+ * extract angle */
+
+ /* Figure out how much to spin the oscillator to extract magic spectral line
+ */
+ dphift = comp_exp_j(2 * M_PI * ((float)(Rs) / (float)(P * Rs)));
+ phi_ft.real = 1;
+ phi_ft.imag = 0;
+ t_c = comp0();
+ for (i = 0; i < (nsym + 1) * P; i++) {
+ /* Get abs^2 of fx_int[i], and add 'em */
+ ft1 = 0;
+ for (m = 0; m < M; m++) {
+ ft1 += (f_int[m][i].real * f_int[m][i].real) +
+ (f_int[m][i].imag * f_int[m][i].imag);
+ }
+
+ /* Down shift and accumulate magic line */
+ t_c = cadd(t_c, fcmult(ft1, phi_ft));
+
+ /* Spin the oscillator for the magic line shift */
+ phi_ft = cmult(phi_ft, dphift);
+ }
+
+ /* Check for NaNs in the fine timing estimate, return if found */
+ /* otherwise segfaults happen */
+ if (isnan(t_c.real) || isnan(t_c.imag)) {
+ return;
+ }
+
+ /* Get the magic angle */
+ norm_rx_timing = atan2f(t_c.imag, t_c.real) / (2 * M_PI);
+ rx_timing = norm_rx_timing * (float)P;
+
+ old_norm_rx_timing = fsk->norm_rx_timing;
+ fsk->norm_rx_timing = norm_rx_timing;
+
+ /* Estimate sample clock offset */
+ d_norm_rx_timing = norm_rx_timing - old_norm_rx_timing;
+
+ /* Filter out big jumps in due to nin change */
+ if (fabsf(d_norm_rx_timing) < .2) {
+ appm = 1e6 * d_norm_rx_timing / (float)nsym;
+ fsk->ppm = .9 * fsk->ppm + .1 * appm;
+ }
+
+ /* Figure out how many samples are needed the next modem cycle */
+ /* Unless we're in burst mode or nin locked */
+ if (!fsk->burst_mode && !fsk->lock_nin) {
+ if (norm_rx_timing > 0.25)
+ fsk->nin = N + Ts / 4;
+ else if (norm_rx_timing < -0.25)
+ fsk->nin = N - Ts / 4;
+ else
+ fsk->nin = N;
+ }
+
+ modem_probe_samp_f("t_norm_rx_timing", &(norm_rx_timing), 1);
+ modem_probe_samp_i("t_nin", &(fsk->nin), 1);
+
+ /* Re-sample the integrators with linear interpolation magic */
+ int low_sample = (int)floorf(rx_timing);
+ float fract = rx_timing - (float)low_sample;
+ int high_sample = (int)ceilf(rx_timing);
+
+ /* Vars for finding the max-of-4 for each bit */
+ float tmax[M];
+
+#ifdef EST_EBNO
+ meanebno = 0;
+ stdebno = 0;
+#endif
+
+ float rx_nse_pow = 1E-12;
+ float rx_sig_pow = 0.0;
+ for (i = 0; i < nsym; i++) {
+ /* resample at ideal sampling instant */
+ int st = (i + 1) * P;
+ for (m = 0; m < M; m++) {
+ t[m] = fcmult(1 - fract, f_int[m][st + low_sample]);
+ t[m] = cadd(t[m], fcmult(fract, f_int[m][st + high_sample]));
+ /* Figure mag^2 of each resampled fx_int */
+ tmax[m] = (t[m].real * t[m].real) + (t[m].imag * t[m].imag);
+ }
+
+ /* hard decision decoding of bits */
+ float max = tmax[0]; /* Maximum for figuring correct symbol */
+ float min = tmax[0];
+ int sym = 0; /* Index of maximum */
+ for (m = 0; m < M; m++) {
+ if (tmax[m] > max) {
+ max = tmax[m];
+ sym = m;
+ }
+ if (tmax[m] < min) {
+ min = tmax[m];
+ }
+ }
+
+ if (rx_bits != NULL) {
+ /* Get bits for 2FSK and 4FSK */
+ if (M == 2) {
+ rx_bits[i] = sym == 1; /* 2FSK. 1 bit per symbol */
+ } else if (M == 4) {
+ rx_bits[(i * 2) + 1] = (sym & 0x1); /* 4FSK. 2 bits per symbol */
+ rx_bits[(i * 2)] = (sym & 0x2) >> 1;
+ }
+ }
+
+ /* Optionally output filter magnitudes for soft decision/LLR
+ calculation. Update SNRest always as this is a useful
+ alternative to the earlier EbNo estimator below */
+ float sum = 0.0;
+ for (m = 0; m < M; m++) {
+ if (rx_filt != NULL) rx_filt[m * nsym + i] = sqrtf(tmax[m]);
+ sum += tmax[m];
+ }
+ rx_sig_pow += max;
+ rx_nse_pow += (sum - max) / (M - 1);
+
+/* Accumulate resampled int magnitude for EbNodB estimation */
+/* Standard deviation is calculated by algorithm devised by crafty soviets */
+#ifdef EST_EBNO
+ /* Accumulate the square of the sampled value */
+ ft1 = max;
+ stdebno += ft1;
+
+ /* Figure the abs value of the max tone */
+ meanebno += sqrtf(ft1);
+#endif
+ }
+
+ fsk->rx_sig_pow = rx_sig_pow = rx_sig_pow / nsym;
+ fsk->rx_nse_pow = rx_nse_pow = rx_nse_pow / nsym;
+ fsk->v_est = sqrt(rx_sig_pow - rx_nse_pow);
+ fsk->SNRest = rx_sig_pow / rx_nse_pow;
+
+#ifdef EST_EBNO
+ /* Calculate mean for EbNodB estimation */
+ meanebno = meanebno / (float)nsym;
+
+ /* Calculate the std. dev for EbNodB estimate */
+ stdebno = (stdebno / (float)nsym) - (meanebno * meanebno);
+ /* trap any negative numbers to avoid NANs flowing through */
+ if (stdebno > 0.0) {
+ stdebno = sqrt(stdebno);
+ } else {
+ stdebno = 0.0;
+ }
+
+ fsk->EbNodB = -6 + (20 * log10f((1e-6 + meanebno) / (1e-6 + stdebno)));
+#else
+ fsk->EbNodB = 1;
+#endif
+
+ /* Write some statistics to the stats struct */
+
+ /* Save clock offset in ppm */
+ fsk->stats->clock_offset = fsk->ppm;
+
+ /* Calculate and save SNR from EbNodB estimate */
+
+ fsk->stats->snr_est =
+ .5 * fsk->stats->snr_est +
+ .5 * fsk->EbNodB; //+ 10*log10f(((float)Rs)/((float)Rs*M));
+
+ /* Save rx timing */
+ fsk->stats->rx_timing = (float)rx_timing;
+
+ /* Estimate and save frequency offset */
+ fc_avg = fc_tx = 0.0;
+ for (int m = 0; m < M; m++) {
+ fc_avg += f_est[m] / M;
+ fc_tx += (fsk->f1_tx + m * fsk->tone_spacing) / M;
+ }
+ fsk->stats->foff = fc_avg - fc_tx;
+
+ /* Take a sample for the eye diagrams ---------------------------------- */
+
+ /* due to oversample rate P, we have too many samples for eye
+ trace. So lets output a decimated version. We use 2P
+ as we want two symbols worth of samples in trace */
+#ifndef __EMBEDDED__
+ int neyesamp_dec = ceil(((float)P * 2) / MODEM_STATS_EYE_IND_MAX);
+ neyesamp = (P * 2) / neyesamp_dec;
+ assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
+ fsk->stats->neyesamp = neyesamp;
+
+ neyeoffset = high_sample + 1;
+
+ int eye_traces = MODEM_STATS_ET_MAX / M;
+ int ind;
+
+ fsk->stats->neyetr = fsk->mode * eye_traces;
+ for (i = 0; i < eye_traces; i++) {
+ for (m = 0; m < M; m++) {
+ for (j = 0; j < neyesamp; j++) {
+ /*
+ 2*P*i...........: advance two symbols for next trace
+ neyeoffset......: centre trace on ideal timing offset, peak eye
+ opening j*neweyesamp_dec: For 2*P>MODEM_STATS_EYE_IND_MAX advance
+ through integrated samples newamp_dec at a time so we dont overflow
+ rx_eye[][]
+ */
+ ind = 2 * P * (i + 1) + neyeoffset + j * neyesamp_dec;
+ assert((i * M + m) < MODEM_STATS_ET_MAX);
+ assert(ind >= 0);
+ assert(ind < (nsym + 1) * P);
+ fsk->stats->rx_eye[i * M + m][j] = cabsolute(f_int[m][ind]);
+ }
+ }
+ }
+
+ if (fsk->normalise_eye) {
+ eye_max = 0;
+ /* Normalize eye to +/- 1 */
+ for (i = 0; i < M * eye_traces; i++)
+ for (j = 0; j < neyesamp; j++)
+ if (fabsf(fsk->stats->rx_eye[i][j]) > eye_max)
+ eye_max = fabsf(fsk->stats->rx_eye[i][j]);
+
+ for (i = 0; i < M * eye_traces; i++)
+ for (j = 0; j < neyesamp; j++)
+ fsk->stats->rx_eye[i][j] = fsk->stats->rx_eye[i][j] / eye_max;
+ }
+
+ fsk->stats->nr = 0;
+ fsk->stats->Nc = 0;
+
+ for (i = 0; i < M; i++) fsk->stats->f_est[i] = f_est[i];
+#endif // !__EMBEDDED__
+
+ /* Dump some internal samples */
+ modem_probe_samp_f("t_EbNodB", &(fsk->EbNodB), 1);
+ modem_probe_samp_f("t_ppm", &(fsk->ppm), 1);
+ modem_probe_samp_f("t_rx_timing", &(rx_timing), 1);
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: fsk_demod
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 11 February 2016
+
+ FSK demodulator functions:
+
+ fsk_demod...: complex samples in, bits out
+ fsk_demos_sd: complex samples in, soft decision symbols out
+
+\*---------------------------------------------------------------------------*/
+
+void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], COMP fsk_in[]) {
+ fsk_demod_core(fsk, rx_bits, NULL, fsk_in);
+}
+
+void fsk_demod_sd(struct FSK *fsk, float rx_filt[], COMP fsk_in[]) {
+ fsk_demod_core(fsk, NULL, rx_filt, fsk_in);
+}
+
+/* make sure stats have known values in case monitoring process reads stats
+ * before they are set */
+static void stats_init(struct FSK *fsk) {
+ /* Take a sample for the eye diagrams */
+ int i, j, m;
+ int P = fsk->P;
+ int M = fsk->mode;
+
+ /* due to oversample rate P, we have too many samples for eye
+ trace. So lets output a decimated version */
+
+ /* asserts below as we found some problems over-running eye matrix */
+
+ /* TODO: refactor eye tracing code here and in fsk_demod */
+#ifndef __EMBEDDED__
+ int neyesamp_dec = ceil(((float)P * 2) / MODEM_STATS_EYE_IND_MAX);
+ int neyesamp = (P * 2) / neyesamp_dec;
+ assert(neyesamp <= MODEM_STATS_EYE_IND_MAX);
+ fsk->stats->neyesamp = neyesamp;
+
+ int eye_traces = MODEM_STATS_ET_MAX / M;
+
+ fsk->stats->neyetr = fsk->mode * eye_traces;
+ for (i = 0; i < eye_traces; i++) {
+ for (m = 0; m < M; m++) {
+ for (j = 0; j < neyesamp; j++) {
+ assert((i * M + m) < MODEM_STATS_ET_MAX);
+ fsk->stats->rx_eye[i * M + m][j] = 0;
+ }
+ }
+ }
+#endif // !__EMBEDDED__
+
+ fsk->stats->rx_timing = fsk->stats->snr_est = 0;
+}
+
+/* Set the FSK modem into burst demod mode */
+
+void fsk_enable_burst_mode(struct FSK *fsk) {
+ fsk->nin = fsk->N;
+ fsk->burst_mode = 1;
+}
+
+void fsk_clear_estimators(struct FSK *fsk) {
+ int i;
+ /* Clear freq estimator state */
+ for (i = 0; i < (fsk->Ndft); i++) {
+ fsk->Sf[i] = 0;
+ }
+ /* Reset timing diff correction */
+ fsk->nin = fsk->N;
+}
+
+void fsk_get_demod_stats(struct FSK *fsk, struct MODEM_STATS *stats) {
+ /* copy from internal stats, note we can't overwrite stats completely
+ as it has other states rqd by caller, also we want a consistent
+ interface across modem types for the freedv_api.
+ */
+
+ stats->clock_offset = fsk->stats->clock_offset;
+ stats->snr_est = fsk->stats->snr_est; // TODO: make this SNR not Eb/No
+ stats->rx_timing = fsk->stats->rx_timing;
+ stats->foff = fsk->stats->foff;
+#ifndef __EMBEDDED__
+ stats->neyesamp = fsk->stats->neyesamp;
+ stats->neyetr = fsk->stats->neyetr;
+ memcpy(stats->rx_eye, fsk->stats->rx_eye, sizeof(stats->rx_eye));
+ memcpy(stats->f_est, fsk->stats->f_est, fsk->mode * sizeof(float));
+#endif // !__EMBEDDED__
+
+ /* these fields not used for FSK so set to something sensible */
+
+ stats->sync = 0;
+ stats->nr = fsk->stats->nr;
+ stats->Nc = fsk->stats->Nc;
+}
+
+/*
+ * Set the minimum and maximum frequencies at which the freq. estimator can find
+ * tones
+ */
+void fsk_set_freq_est_limits(struct FSK *fsk, int est_min, int est_max) {
+ assert(fsk != NULL);
+ assert(est_min >= -fsk->Fs / 2);
+ assert(est_max <= fsk->Fs / 2);
+ assert(est_max > est_min);
+ fsk->est_min = est_min;
+ fsk->est_max = est_max;
+}
+
+void fsk_stats_normalise_eye(struct FSK *fsk, int normalise_enable) {
+ assert(fsk != NULL);
+ fsk->normalise_eye = normalise_enable;
+}
+
+void fsk_set_freq_est_alg(struct FSK *fsk, int est_type) {
+ assert(fsk != NULL);
+ fsk->freq_est_type = est_type;
+}
diff --git a/src/codec2/fsk.h b/src/codec2/fsk.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ee687b39c66f2a4efc0cb83b219e6e9976c707f
--- /dev/null
+++ b/src/codec2/fsk.h
@@ -0,0 +1,228 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: fsk.h
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 6 January 2016
+
+ C Implementation of 2FSK/4FSK modulator/demodulator, based on
+octave/fsk_horus.m
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2016 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/>.
+*/
+
+#ifndef __C2FSK_H
+#define __C2FSK_H
+
+#include <stdint.h>
+
+#include "comp.h"
+#include "kiss_fftr.h"
+#include "modem_stats.h"
+
+#define MODE_2FSK 2
+#define MODE_4FSK 4
+
+#define MODE_M_MAX 4
+
+#define FSK_SCALE 16383
+
+/* default internal parameters */
+#define FSK_DEFAULT_P \
+ 8 /* Number of timing offsets we have to choose from, try to keep P >= 8 */
+#define FSK_DEFAULT_NSYM 50 /* See Nsym below */
+#define FSK_NONE -1 /* unused parameter */
+
+#undef P /* avoid clash with #define P in fdmdv_internal.h */
+
+struct FSK {
+ /* Static parameters set up by fsk_init */
+ int Ndft; /* freq offset est fft */
+ int Fs; /* sample freq */
+ int N; /* processing buffer size */
+ int Rs; /* symbol rate */
+ int Ts; /* samples per symbol */
+ int Nmem; /* size of extra mem for timing adj */
+ int P; /* oversample rate for timing est/adj */
+ int Nsym; /* Number of symbols processed by demodulator in each call, also the
+ timing estimator window */
+ int Nbits; /* Number of bits spat out in a processing frame */
+ int f1_tx; /* f1 for modulator */
+ int tone_spacing; /* Space between TX freqs for modulator (and option mask
+ freq estimator) */
+ int mode; /* 2FSK or 4FSK */
+ float tc; /* time constant for smoothing FFTs */
+ int est_min; /* Minimum frequency for freq. estimator */
+ int est_max; /* Maximum frequency for freq. estimator */
+ int est_space; /* Minimum frequency spacing for freq. estimator */
+ float *hann_table; /* Precomputed or runtime computed hann window table */
+
+ /* Parameters used by demod */
+ float *Sf; /* Average of magnitude spectrum */
+ COMP phi_c[MODE_M_MAX]; /* phase of each demod local oscillator */
+ COMP *f_dc; /* down converted samples */
+
+ kiss_fft_cfg fft_cfg; /* Config for KISS FFT, used in freq est */
+ float norm_rx_timing; /* Normalized RX timing */
+
+ /* Parameters used by mod */
+ COMP tx_phase_c; /* TX phase, but complex */
+
+ /* Statistics generated by demod */
+ float EbNodB; /* Estimated EbNo in dB */
+ float f_est[MODE_M_MAX]; /* Estimated frequencies (peak method) */
+ float f2_est[MODE_M_MAX]; /* Estimated frequencies (mask method) */
+ int freq_est_type; /* which estimator to use */
+ float ppm; /* Estimated PPM clock offset */
+ float SNRest; /* used for LLRs */
+ float v_est; /* used for LLRs */
+ float rx_sig_pow;
+ float rx_nse_pow;
+
+ /* Parameters used by mod/demod and driving code */
+ int nin; /* Number of samples to feed the next demod cycle */
+ int burst_mode; /* enables/disables 'burst' mode */
+ int lock_nin; /* locks nin during testing */
+
+ /* modem statistics struct */
+ struct MODEM_STATS *stats;
+ int normalise_eye; /* enables/disables normalisation of eye diagram */
+};
+
+/*
+ * Create a FSK modem
+ *
+ * int Fs - Sample frequency
+ * int Rs - Symbol rate
+ * int M - 2 for 2FSK, 4 for 4FSK
+ * int f1_tx - first tone frequency
+ * int tone_spacing - frequency spacing (for modulator and optional "mask" freq
+ * estimator)
+ */
+struct FSK *fsk_create(int Fs, int Rs, int M, int f1_tx, int tone_spacing);
+
+/*
+ * Create a FSK modem - advanced version
+ *
+ * int Fs - Sample frequency
+ * int Rs - Symbol rate
+ * int M - 2 for 2FSK, 4 for 4FSK
+ * int P - number of timing offsets to choose from (suggest >= 8)
+ * int Nsym - windows size for timing estimator
+ * int f1_tx - first tone frequency
+ * int tone_spacing - frequency spacing (for modulator and optional "mask" freq
+ * estimator)
+ */
+struct FSK *fsk_create_hbr(int Fs, int Rs, int M, int P, int Nsym, int f1_tx,
+ int tone_spacing);
+
+/*
+ * Set the minimum and maximum frequencies at which the freq. estimator can find
+ * tones
+ */
+void fsk_set_freq_est_limits(struct FSK *fsk, int fmin, int fmax);
+
+/*
+ * Clear the estimator states
+ */
+void fsk_clear_estimators(struct FSK *fsk);
+
+/*
+ * Fills MODEM_STATS struct with demod statistics
+ */
+void fsk_get_demod_stats(struct FSK *fsk, struct MODEM_STATS *stats);
+
+/*
+ * Destroy an FSK state struct and free its memory
+ *
+ * struct FSK *fsk - FSK config/state struct to be destroyed
+ */
+void fsk_destroy(struct FSK *fsk);
+
+/*
+ * Modulates Nsym bits into N samples
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * float fsk_out[] - Buffer for samples of modulated FSK,
+ * fsk->Ts*(nbits/(M>>1)) in length uint8_t tx_bits[] - Buffer containing Nbits
+ * unpacked bits int nbits - number of bits to transmit
+ */
+void fsk_mod(struct FSK *fsk, float fsk_out[], uint8_t tx_bits[], int nbits);
+
+/*
+ * Modulates Nsym bits into N samples
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * float fsk_out[] - Buffer for samples of "voltage" used to modulate an
+ * external VCO
+ * - fsk->Ts*(nbits/(M>>1)) in length
+ * uint8_t tx_bits[] - Buffer containing Nbits unpacked bits
+ * int nbits - number of bits to transmit
+ */
+void fsk_mod_ext_vco(struct FSK *fsk, float vco_out[], uint8_t tx_bits[],
+ int nbits);
+
+/*
+ * Modulates Nsym bits into N complex samples
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * comp fsk_out[] - Buffer for samples of modulated FSK,
+ * fsk->Ts*(nbits/(M>>1)) in length uint8_t tx_bits[] - Buffer containing Nbits
+ * unpacked bits int nbits - number of bits to transmit
+ */
+void fsk_mod_c(struct FSK *fsk, COMP fsk_out[], uint8_t tx_bits[], int nbits);
+
+/*
+ * Returns the number of samples needed for the next fsk_demod() cycle
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * returns - number of samples to be fed into fsk_demod next cycle
+ */
+uint32_t fsk_nin(struct FSK *fsk);
+
+/*
+ * Demodulate some number of FSK samples. The number of samples to be
+ * demodulated can be found by calling fsk_nin().
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * uint8_t rx_bits[] - Buffer for fsk->Nbits unpacked bits to be written
+ * float fsk_in[] - nin samples of modulated FSK
+ */
+void fsk_demod(struct FSK *fsk, uint8_t rx_bits[], COMP fsk_in[]);
+
+/*
+ * Soft decision demodulation
+ *
+ * struct FSK *fsk - FSK config/state struct, set up by fsk_create
+ * float rx_flit[] - M x Nsym array of filtermagnitude outputs
+ * float fsk_in[] - nin samples of modualted FSK
+ */
+void fsk_demod_sd(struct FSK *fsk, float rx_filt[], COMP fsk_in[]);
+
+/* enables/disables normalisation of eye diagram samples */
+
+void fsk_stats_normalise_eye(struct FSK *fsk, int normalise_enable);
+
+/* Set the FSK modem into burst demod mode */
+
+void fsk_enable_burst_mode(struct FSK *fsk);
+
+/* Set freq est algorithm 0: peak 1:mask */
+void fsk_set_freq_est_alg(struct FSK *fsk, int est_type);
+
+#endif
diff --git a/src/codec2/kiss_fft.c b/src/codec2/kiss_fft.c
new file mode 100644
index 0000000000000000000000000000000000000000..e7993cf436b2adc1d1b61c3fb44abbc6975a38cd
--- /dev/null
+++ b/src/codec2/kiss_fft.c
@@ -0,0 +1,426 @@
+/*
+Copyright (c) 2003-2010, Mark Borgerding
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+ * Neither the author nor the names of any contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "_kiss_fft_guts.h"
+/* The guts header contains all the multiplication and addition macros that are
+ defined for fixed or floating point complex numbers. It also declares the kf_
+ internal functions.
+ */
+
+static void kf_bfly2(kiss_fft_cpx *Fout, const size_t fstride,
+ const kiss_fft_cfg st, int m) {
+ kiss_fft_cpx *Fout2;
+ kiss_fft_cpx *tw1 = st->twiddles;
+ kiss_fft_cpx t;
+ Fout2 = Fout + m;
+ do {
+ C_FIXDIV(*Fout, 2);
+ C_FIXDIV(*Fout2, 2);
+
+ C_MUL(t, *Fout2, *tw1);
+ tw1 += fstride;
+ C_SUB(*Fout2, *Fout, t);
+ C_ADDTO(*Fout, t);
+ ++Fout2;
+ ++Fout;
+ } while (--m);
+}
+
+static void kf_bfly4(kiss_fft_cpx *Fout, const size_t fstride,
+ const kiss_fft_cfg st, const size_t m) {
+ kiss_fft_cpx *tw1, *tw2, *tw3;
+ kiss_fft_cpx scratch[6];
+ size_t k = m;
+ const size_t m2 = 2 * m;
+ const size_t m3 = 3 * m;
+
+ tw3 = tw2 = tw1 = st->twiddles;
+
+ do {
+ C_FIXDIV(*Fout, 4);
+ C_FIXDIV(Fout[m], 4);
+ C_FIXDIV(Fout[m2], 4);
+ C_FIXDIV(Fout[m3], 4);
+
+ C_MUL(scratch[0], Fout[m], *tw1);
+ C_MUL(scratch[1], Fout[m2], *tw2);
+ C_MUL(scratch[2], Fout[m3], *tw3);
+
+ C_SUB(scratch[5], *Fout, scratch[1]);
+ C_ADDTO(*Fout, scratch[1]);
+ C_ADD(scratch[3], scratch[0], scratch[2]);
+ C_SUB(scratch[4], scratch[0], scratch[2]);
+ C_SUB(Fout[m2], *Fout, scratch[3]);
+ tw1 += fstride;
+ tw2 += fstride * 2;
+ tw3 += fstride * 3;
+ C_ADDTO(*Fout, scratch[3]);
+
+ if (st->inverse) {
+ Fout[m].r = scratch[5].r - scratch[4].i;
+ Fout[m].i = scratch[5].i + scratch[4].r;
+ Fout[m3].r = scratch[5].r + scratch[4].i;
+ Fout[m3].i = scratch[5].i - scratch[4].r;
+ } else {
+ Fout[m].r = scratch[5].r + scratch[4].i;
+ Fout[m].i = scratch[5].i - scratch[4].r;
+ Fout[m3].r = scratch[5].r - scratch[4].i;
+ Fout[m3].i = scratch[5].i + scratch[4].r;
+ }
+ ++Fout;
+ } while (--k);
+}
+
+static void kf_bfly3(kiss_fft_cpx *Fout, const size_t fstride,
+ const kiss_fft_cfg st, size_t m) {
+ size_t k = m;
+ const size_t m2 = 2 * m;
+ kiss_fft_cpx *tw1, *tw2;
+ kiss_fft_cpx scratch[5];
+ kiss_fft_cpx epi3;
+ epi3 = st->twiddles[fstride * m];
+
+ tw1 = tw2 = st->twiddles;
+
+ do {
+ C_FIXDIV(*Fout, 3);
+ C_FIXDIV(Fout[m], 3);
+ C_FIXDIV(Fout[m2], 3);
+
+ C_MUL(scratch[1], Fout[m], *tw1);
+ C_MUL(scratch[2], Fout[m2], *tw2);
+
+ C_ADD(scratch[3], scratch[1], scratch[2]);
+ C_SUB(scratch[0], scratch[1], scratch[2]);
+ tw1 += fstride;
+ tw2 += fstride * 2;
+
+ Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
+ Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
+
+ C_MULBYSCALAR(scratch[0], epi3.i);
+
+ C_ADDTO(*Fout, scratch[3]);
+
+ Fout[m2].r = Fout[m].r + scratch[0].i;
+ Fout[m2].i = Fout[m].i - scratch[0].r;
+
+ Fout[m].r -= scratch[0].i;
+ Fout[m].i += scratch[0].r;
+
+ ++Fout;
+ } while (--k);
+}
+
+static void kf_bfly5(kiss_fft_cpx *Fout, const size_t fstride,
+ const kiss_fft_cfg st, int m) {
+ kiss_fft_cpx *Fout0, *Fout1, *Fout2, *Fout3, *Fout4;
+ int u;
+ kiss_fft_cpx scratch[13];
+ kiss_fft_cpx *twiddles = st->twiddles;
+ kiss_fft_cpx *tw;
+ kiss_fft_cpx ya, yb;
+ ya = twiddles[fstride * m];
+ yb = twiddles[fstride * 2 * m];
+
+ Fout0 = Fout;
+ Fout1 = Fout0 + m;
+ Fout2 = Fout0 + 2 * m;
+ Fout3 = Fout0 + 3 * m;
+ Fout4 = Fout0 + 4 * m;
+
+ tw = st->twiddles;
+ for (u = 0; u < m; ++u) {
+ C_FIXDIV(*Fout0, 5);
+ C_FIXDIV(*Fout1, 5);
+ C_FIXDIV(*Fout2, 5);
+ C_FIXDIV(*Fout3, 5);
+ C_FIXDIV(*Fout4, 5);
+ scratch[0] = *Fout0;
+
+ C_MUL(scratch[1], *Fout1, tw[u * fstride]);
+ C_MUL(scratch[2], *Fout2, tw[2 * u * fstride]);
+ C_MUL(scratch[3], *Fout3, tw[3 * u * fstride]);
+ C_MUL(scratch[4], *Fout4, tw[4 * u * fstride]);
+
+ C_ADD(scratch[7], scratch[1], scratch[4]);
+ C_SUB(scratch[10], scratch[1], scratch[4]);
+ C_ADD(scratch[8], scratch[2], scratch[3]);
+ C_SUB(scratch[9], scratch[2], scratch[3]);
+
+ Fout0->r += scratch[7].r + scratch[8].r;
+ Fout0->i += scratch[7].i + scratch[8].i;
+
+ scratch[5].r =
+ scratch[0].r + S_MUL(scratch[7].r, ya.r) + S_MUL(scratch[8].r, yb.r);
+ scratch[5].i =
+ scratch[0].i + S_MUL(scratch[7].i, ya.r) + S_MUL(scratch[8].i, yb.r);
+
+ scratch[6].r = S_MUL(scratch[10].i, ya.i) + S_MUL(scratch[9].i, yb.i);
+ scratch[6].i = -S_MUL(scratch[10].r, ya.i) - S_MUL(scratch[9].r, yb.i);
+
+ C_SUB(*Fout1, scratch[5], scratch[6]);
+ C_ADD(*Fout4, scratch[5], scratch[6]);
+
+ scratch[11].r =
+ scratch[0].r + S_MUL(scratch[7].r, yb.r) + S_MUL(scratch[8].r, ya.r);
+ scratch[11].i =
+ scratch[0].i + S_MUL(scratch[7].i, yb.r) + S_MUL(scratch[8].i, ya.r);
+ scratch[12].r = -S_MUL(scratch[10].i, yb.i) + S_MUL(scratch[9].i, ya.i);
+ scratch[12].i = S_MUL(scratch[10].r, yb.i) - S_MUL(scratch[9].r, ya.i);
+
+ C_ADD(*Fout2, scratch[11], scratch[12]);
+ C_SUB(*Fout3, scratch[11], scratch[12]);
+
+ ++Fout0;
+ ++Fout1;
+ ++Fout2;
+ ++Fout3;
+ ++Fout4;
+ }
+}
+
+/* perform the butterfly for one stage of a mixed radix FFT */
+static void kf_bfly_generic(kiss_fft_cpx *Fout, const size_t fstride,
+ const kiss_fft_cfg st, int m, int p) {
+ int u, k, q1, q;
+ kiss_fft_cpx *twiddles = st->twiddles;
+ kiss_fft_cpx t;
+ int Norig = st->nfft;
+
+ kiss_fft_cpx *scratch =
+ (kiss_fft_cpx *)KISS_FFT_TMP_ALLOC(sizeof(kiss_fft_cpx) * p);
+
+ for (u = 0; u < m; ++u) {
+ k = u;
+ for (q1 = 0; q1 < p; ++q1) {
+ scratch[q1] = Fout[k];
+ C_FIXDIV(scratch[q1], p);
+ k += m;
+ }
+
+ k = u;
+ for (q1 = 0; q1 < p; ++q1) {
+ int twidx = 0;
+ Fout[k] = scratch[0];
+ for (q = 1; q < p; ++q) {
+ twidx += fstride * k;
+ if (twidx >= Norig) twidx -= Norig;
+ C_MUL(t, scratch[q], twiddles[twidx]);
+ C_ADDTO(Fout[k], t);
+ }
+ k += m;
+ }
+ }
+ KISS_FFT_TMP_FREE(scratch);
+}
+
+static void kf_work(kiss_fft_cpx *Fout, const kiss_fft_cpx *f,
+ const size_t fstride, int in_stride, int *factors,
+ const kiss_fft_cfg st) {
+ kiss_fft_cpx *Fout_beg = Fout;
+ const int p = *factors++; /* the radix */
+ const int m = *factors++; /* stage's fft length/p */
+ const kiss_fft_cpx *Fout_end = Fout + p * m;
+
+#ifdef _OPENMP
+ // use openmp extensions at the
+ // top-level (not recursive)
+ if (fstride == 1 && p <= 5) {
+ int k;
+
+ // execute the p different work units in different threads
+#pragma omp parallel for
+ for (k = 0; k < p; ++k)
+ kf_work(Fout + k * m, f + fstride * in_stride * k, fstride * p, in_stride,
+ factors, st);
+ // all threads have joined by this point
+
+ switch (p) {
+ case 2:
+ kf_bfly2(Fout, fstride, st, m);
+ break;
+ case 3:
+ kf_bfly3(Fout, fstride, st, m);
+ break;
+ case 4:
+ kf_bfly4(Fout, fstride, st, m);
+ break;
+ case 5:
+ kf_bfly5(Fout, fstride, st, m);
+ break;
+ default:
+ kf_bfly_generic(Fout, fstride, st, m, p);
+ break;
+ }
+ return;
+ }
+#endif
+
+ if (m == 1) {
+ do {
+ *Fout = *f;
+ f += fstride * in_stride;
+ } while (++Fout != Fout_end);
+ } else {
+ do {
+ // recursive call:
+ // DFT of size m*p performed by doing
+ // p instances of smaller DFTs of size m,
+ // each one takes a decimated version of the input
+ kf_work(Fout, f, fstride * p, in_stride, factors, st);
+ f += fstride * in_stride;
+ } while ((Fout += m) != Fout_end);
+ }
+
+ Fout = Fout_beg;
+
+ // recombine the p smaller DFTs
+ switch (p) {
+ case 2:
+ kf_bfly2(Fout, fstride, st, m);
+ break;
+ case 3:
+ kf_bfly3(Fout, fstride, st, m);
+ break;
+ case 4:
+ kf_bfly4(Fout, fstride, st, m);
+ break;
+ case 5:
+ kf_bfly5(Fout, fstride, st, m);
+ break;
+ default:
+ kf_bfly_generic(Fout, fstride, st, m, p);
+ break;
+ }
+}
+
+/* facbuf is populated by p1,m1,p2,m2, ...
+ where
+ p[i] * m[i] = m[i-1]
+ m0 = n */
+static void kf_factor(int n, int *facbuf) {
+ int p = 4;
+ double floor_sqrt;
+ floor_sqrt = floorf(sqrtf((double)n));
+
+ /*factor out powers of 4, powers of 2, then any remaining primes */
+ do {
+ while (n % p) {
+ switch (p) {
+ case 4:
+ p = 2;
+ break;
+ case 2:
+ p = 3;
+ break;
+ default:
+ p += 2;
+ break;
+ }
+ if (p > floor_sqrt) p = n; /* no more factors, skip to end */
+ }
+ n /= p;
+ *facbuf++ = p;
+ *facbuf++ = n;
+ } while (n > 1);
+}
+
+/*
+ *
+ * User-callable function to allocate all necessary storage space for the fft.
+ *
+ * The return value is a contiguous block of memory, allocated with malloc. As
+ * such, It can be freed with free(), rather than a kiss_fft-specific function.
+ * */
+kiss_fft_cfg kiss_fft_alloc(int nfft, int inverse_fft, void *mem,
+ size_t *lenmem) {
+ kiss_fft_cfg st = NULL;
+ size_t memneeded = sizeof(struct kiss_fft_state) +
+ sizeof(kiss_fft_cpx) * (nfft - 1); /* twiddle factors*/
+
+ if (lenmem == NULL) {
+ st = (kiss_fft_cfg)KISS_FFT_MALLOC(memneeded);
+ } else {
+ if (mem != NULL && *lenmem >= memneeded) st = (kiss_fft_cfg)mem;
+ *lenmem = memneeded;
+ }
+ if (st) {
+ int i;
+ st->nfft = nfft;
+ st->inverse = inverse_fft;
+
+ for (i = 0; i < nfft; ++i) {
+ const double pi =
+ 3.141592653589793238462643383279502884197169399375105820974944;
+ double phase = -2 * pi * i / nfft;
+ if (st->inverse) phase *= -1;
+ kf_cexp(st->twiddles + i, phase);
+ }
+
+ kf_factor(nfft, st->factors);
+ }
+ return st;
+}
+
+void kiss_fft_stride(kiss_fft_cfg st, const kiss_fft_cpx *fin,
+ kiss_fft_cpx *fout, int in_stride) {
+ if (fin == fout) {
+ // NOTE: this is not really an in-place FFT algorithm.
+ // It just performs an out-of-place FFT into a temp buffer
+ kiss_fft_cpx *tmpbuf =
+ (kiss_fft_cpx *)KISS_FFT_TMP_ALLOC(sizeof(kiss_fft_cpx) * st->nfft);
+ kf_work(tmpbuf, fin, 1, in_stride, st->factors, st);
+ memcpy(fout, tmpbuf, sizeof(kiss_fft_cpx) * st->nfft);
+ KISS_FFT_TMP_FREE(tmpbuf);
+ } else {
+ kf_work(fout, fin, 1, in_stride, st->factors, st);
+ }
+}
+
+void kiss_fft(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout) {
+ kiss_fft_stride(cfg, fin, fout, 1);
+}
+
+void kiss_fft_cleanup(void) {
+ // nothing needed any more
+}
+
+int kiss_fft_next_fast_size(int n) {
+ while (1) {
+ int m = n;
+ while ((m % 2) == 0) m /= 2;
+ while ((m % 3) == 0) m /= 3;
+ while ((m % 5) == 0) m /= 5;
+ if (m <= 1)
+ break; /* n is completely factorable by twos, threes, and fives */
+ n++;
+ }
+ return n;
+}
diff --git a/src/codec2/kiss_fft.h b/src/codec2/kiss_fft.h
new file mode 100644
index 0000000000000000000000000000000000000000..ea1349ed39c4f7d063a754e4f7acbc0329acefcd
--- /dev/null
+++ b/src/codec2/kiss_fft.h
@@ -0,0 +1,126 @@
+#ifndef KISS_FFT_H
+#define KISS_FFT_H
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ ATTENTION!
+ If you would like a :
+ -- a utility that will handle the caching of fft objects
+ -- real-only (no imaginary time component ) FFT
+ -- a multi-dimensional FFT
+ -- a command-line utility to perform ffts
+ -- a command-line utility to perform fast-convolution filtering
+
+ Then see kfc.h kiss_fftr.h kiss_fftnd.h fftutil.c kiss_fastfir.c
+ in the tools/ directory.
+*/
+
+#ifdef USE_SIMD
+#include <xmmintrin.h>
+#define kiss_fft_scalar __m128
+#define KISS_FFT_MALLOC(nbytes) _mm_malloc(nbytes, 16)
+#define KISS_FFT_FREE _mm_free
+#else
+#define KISS_FFT_MALLOC malloc
+#define KISS_FFT_FREE free
+#endif
+
+#ifdef FIXED_POINT
+#include <sys/types.h>
+#if (FIXED_POINT == 32)
+#define kiss_fft_scalar int32_t
+#else
+#define kiss_fft_scalar int16_t
+#endif
+#else
+#ifndef kiss_fft_scalar
+/* default is float */
+#define kiss_fft_scalar float
+#endif
+#endif
+
+typedef struct {
+ kiss_fft_scalar r;
+ kiss_fft_scalar i;
+} kiss_fft_cpx;
+
+typedef struct kiss_fft_state *kiss_fft_cfg;
+
+/*
+ * kiss_fft_alloc
+ *
+ * Initialize a FFT (or IFFT) algorithm's cfg/state buffer.
+ *
+ * typical usage: kiss_fft_cfg mycfg=kiss_fft_alloc(1024,0,NULL,NULL);
+ *
+ * The return value from fft_alloc is a cfg buffer used internally
+ * by the fft routine or NULL.
+ *
+ * If lenmem is NULL, then kiss_fft_alloc will allocate a cfg buffer using
+ * malloc. The returned value should be free()d when done to avoid memory leaks.
+ *
+ * The state can be placed in a user supplied buffer 'mem':
+ * If lenmem is not NULL and mem is not NULL and *lenmem is large enough,
+ * then the function places the cfg in mem and the size used in *lenmem
+ * and returns mem.
+ *
+ * If lenmem is not NULL and ( mem is NULL or *lenmem is not large enough),
+ * then the function returns NULL and places the minimum cfg
+ * buffer size in *lenmem.
+ * */
+
+kiss_fft_cfg kiss_fft_alloc(int nfft, int inverse_fft, void *mem,
+ size_t *lenmem);
+
+/*
+ * kiss_fft(cfg,in_out_buf)
+ *
+ * Perform an FFT on a complex input buffer.
+ * for a forward FFT,
+ * fin should be f[0] , f[1] , ... ,f[nfft-1]
+ * fout will be F[0] , F[1] , ... ,F[nfft-1]
+ * Note that each element is complex and can be accessed like
+ f[k].r and f[k].i
+ * */
+void kiss_fft(kiss_fft_cfg cfg, const kiss_fft_cpx *fin, kiss_fft_cpx *fout);
+
+/*
+ A more generic version of the above function. It reads its input from every Nth
+ sample.
+ * */
+void kiss_fft_stride(kiss_fft_cfg cfg, const kiss_fft_cpx *fin,
+ kiss_fft_cpx *fout, int fin_stride);
+
+/* If kiss_fft_alloc allocated a buffer, it is one contiguous
+ buffer and can be simply free()d when no longer needed*/
+#define kiss_fft_free free
+
+/*
+ Cleans up some memory that gets managed internally. Not necessary to call, but
+ it might clean up your compiler output to call this before you exit.
+*/
+void kiss_fft_cleanup(void);
+
+/*
+ * Returns the smallest integer k, such that k>=n and k has only "fast" factors
+ * (2,3,5)
+ */
+int kiss_fft_next_fast_size(int n);
+
+/* for real ffts, we need an even size */
+#define kiss_fftr_next_fast_size_real(n) \
+ (kiss_fft_next_fast_size(((n) + 1) >> 1) << 1)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/codec2/kiss_fftr.c b/src/codec2/kiss_fftr.c
new file mode 100644
index 0000000000000000000000000000000000000000..e3aee37a8477425e2daad6f25b914b0b4737c515
--- /dev/null
+++ b/src/codec2/kiss_fftr.c
@@ -0,0 +1,168 @@
+/*
+Copyright (c) 2003-2004, Mark Borgerding
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+ * Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+ * Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+ * Neither the author nor the names of any contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "kiss_fftr.h"
+
+#include "_kiss_fft_guts.h"
+#include "assert.h"
+
+struct kiss_fftr_state {
+ kiss_fft_cfg substate;
+ kiss_fft_cpx *tmpbuf;
+ kiss_fft_cpx *super_twiddles;
+#ifdef USE_SIMD
+ void *pad;
+#endif
+};
+
+kiss_fftr_cfg kiss_fftr_alloc(int nfft, int inverse_fft, void *mem,
+ size_t *lenmem) {
+ int i;
+ kiss_fftr_cfg st = NULL;
+ size_t subsize, memneeded;
+
+ if (nfft & 1) {
+ fprintf(stderr, "Real FFT optimization must be even.\n");
+ return NULL;
+ }
+ nfft >>= 1;
+
+ kiss_fft_alloc(nfft, inverse_fft, NULL, &subsize);
+ memneeded = sizeof(struct kiss_fftr_state) + subsize +
+ sizeof(kiss_fft_cpx) * (nfft * 3 / 2);
+
+ if (lenmem == NULL) {
+ st = (kiss_fftr_cfg)KISS_FFT_MALLOC(memneeded);
+ } else {
+ if (*lenmem >= memneeded) st = (kiss_fftr_cfg)mem;
+ *lenmem = memneeded;
+ }
+ if (!st) return NULL;
+
+ st->substate = (kiss_fft_cfg)(st + 1); /*just beyond kiss_fftr_state struct */
+ st->tmpbuf = (kiss_fft_cpx *)(((char *)st->substate) + subsize);
+ st->super_twiddles = st->tmpbuf + nfft;
+ kiss_fft_alloc(nfft, inverse_fft, st->substate, &subsize);
+
+ for (i = 0; i < nfft / 2; ++i) {
+ float phase =
+ -3.14159265358979323846264338327 * ((float)(i + 1) / nfft + .5);
+ if (inverse_fft) phase *= -1;
+ kf_cexp(st->super_twiddles + i, phase);
+ }
+ return st;
+}
+
+void kiss_fftr(kiss_fftr_cfg st, const kiss_fft_scalar *timedata,
+ kiss_fft_cpx *freqdata) {
+ /* input buffer timedata is stored row-wise */
+ int k, ncfft;
+ kiss_fft_cpx fpnk, fpk, f1k, f2k, tw, tdc;
+
+ assert(st->substate->inverse == 0);
+
+ ncfft = st->substate->nfft;
+
+ /*perform the parallel fft of two real signals packed in real,imag*/
+ kiss_fft(st->substate, (const kiss_fft_cpx *)timedata, st->tmpbuf);
+ /* The real part of the DC element of the frequency spectrum in st->tmpbuf
+ * contains the sum of the even-numbered elements of the input time sequence
+ * The imag part is the sum of the odd-numbered elements
+ *
+ * The sum of tdc.r and tdc.i is the sum of the input time sequence.
+ * yielding DC of input time sequence
+ * The difference of tdc.r - tdc.i is the sum of the input (dot product)
+ * [1,-1,1,-1... yielding Nyquist bin of input time sequence
+ */
+
+ tdc.r = st->tmpbuf[0].r;
+ tdc.i = st->tmpbuf[0].i;
+ C_FIXDIV(tdc, 2);
+ CHECK_OVERFLOW_OP(tdc.r, +, tdc.i);
+ CHECK_OVERFLOW_OP(tdc.r, -, tdc.i);
+ freqdata[0].r = tdc.r + tdc.i;
+ freqdata[ncfft].r = tdc.r - tdc.i;
+#ifdef USE_SIMD
+ freqdata[ncfft].i = freqdata[0].i = _mm_set1_ps(0);
+#else
+ freqdata[ncfft].i = freqdata[0].i = 0;
+#endif
+
+ for (k = 1; k <= ncfft / 2; ++k) {
+ fpk = st->tmpbuf[k];
+ fpnk.r = st->tmpbuf[ncfft - k].r;
+ fpnk.i = -st->tmpbuf[ncfft - k].i;
+ C_FIXDIV(fpk, 2);
+ C_FIXDIV(fpnk, 2);
+
+ C_ADD(f1k, fpk, fpnk);
+ C_SUB(f2k, fpk, fpnk);
+ C_MUL(tw, f2k, st->super_twiddles[k - 1]);
+
+ freqdata[k].r = HALF_OF(f1k.r + tw.r);
+ freqdata[k].i = HALF_OF(f1k.i + tw.i);
+ freqdata[ncfft - k].r = HALF_OF(f1k.r - tw.r);
+ freqdata[ncfft - k].i = HALF_OF(tw.i - f1k.i);
+ }
+}
+
+void kiss_fftri(kiss_fftr_cfg st, const kiss_fft_cpx *freqdata,
+ kiss_fft_scalar *timedata) {
+ /* input buffer timedata is stored row-wise */
+ int k, ncfft;
+
+ assert(st->substate->inverse == 1);
+
+ ncfft = st->substate->nfft;
+
+ st->tmpbuf[0].r = freqdata[0].r + freqdata[ncfft].r;
+ st->tmpbuf[0].i = freqdata[0].r - freqdata[ncfft].r;
+ C_FIXDIV(st->tmpbuf[0], 2);
+
+ for (k = 1; k <= ncfft / 2; ++k) {
+ kiss_fft_cpx fk, fnkc, fek, fok, tmp;
+ fk = freqdata[k];
+ fnkc.r = freqdata[ncfft - k].r;
+ fnkc.i = -freqdata[ncfft - k].i;
+ C_FIXDIV(fk, 2);
+ C_FIXDIV(fnkc, 2);
+
+ C_ADD(fek, fk, fnkc);
+ C_SUB(tmp, fk, fnkc);
+ C_MUL(fok, tmp, st->super_twiddles[k - 1]);
+ C_ADD(st->tmpbuf[k], fek, fok);
+ C_SUB(st->tmpbuf[ncfft - k], fek, fok);
+#ifdef USE_SIMD
+ st->tmpbuf[ncfft - k].i *= _mm_set1_ps(-1.0);
+#else
+ st->tmpbuf[ncfft - k].i *= -1;
+#endif
+ }
+ kiss_fft(st->substate, st->tmpbuf, (kiss_fft_cpx *)timedata);
+}
diff --git a/src/codec2/kiss_fftr.h b/src/codec2/kiss_fftr.h
new file mode 100644
index 0000000000000000000000000000000000000000..4ab52db114b07d39b9edbe4e109beb4f9738f365
--- /dev/null
+++ b/src/codec2/kiss_fftr.h
@@ -0,0 +1,47 @@
+#ifndef KISS_FTR_H
+#define KISS_FTR_H
+
+#include "kiss_fft.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+
+ Real optimized version can save about 45% cpu time vs. complex fft of a real
+ seq.
+
+
+
+ */
+
+typedef struct kiss_fftr_state *kiss_fftr_cfg;
+
+kiss_fftr_cfg kiss_fftr_alloc(int nfft, int inverse_fft, void *mem,
+ size_t *lenmem);
+/*
+ nfft must be even
+
+ If you don't care to allocate space, use mem = lenmem = NULL
+*/
+
+void kiss_fftr(kiss_fftr_cfg cfg, const kiss_fft_scalar *timedata,
+ kiss_fft_cpx *freqdata);
+/*
+ input timedata has nfft scalar points
+ output freqdata has nfft/2+1 complex points
+*/
+
+void kiss_fftri(kiss_fftr_cfg cfg, const kiss_fft_cpx *freqdata,
+ kiss_fft_scalar *timedata);
+/*
+ input freqdata has nfft/2+1 complex points
+ output timedata has nfft scalar points
+*/
+
+#define kiss_fftr_free free
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/src/codec2/mod.rs b/src/codec2/mod.rs
new file mode 100644
index 0000000000000000000000000000000000000000..9276caf8e7a8cc1cb892bd3e63a9b321144e98c8
--- /dev/null
+++ b/src/codec2/mod.rs
@@ -0,0 +1,218 @@
+use std::{
+ collections::VecDeque,
+ ops::{AddAssign, Div, Sub},
+};
+
+const MODE_M_MAX: usize = 4;
+const M: i32 = 2;
+const N_SYM: i32 = 50;
+
+#[link(name = "fsk", kind = "static")]
+extern "C" {
+ fn fsk_create_hbr(
+ Fs: libc::c_int,
+ Rs: libc::c_int,
+ M: libc::c_int,
+ P: libc::c_int,
+ n_sym: libc::c_int,
+ f1_tx: libc::c_int,
+ tone_spacing: libc::c_int,
+ ) -> *mut InternalFsk;
+
+ fn fsk_destroy(fsk: *mut InternalFsk);
+
+ fn fsk_demod_sd(fsk: *mut InternalFsk, rx_filt: *mut libc::c_float, fsk_in: *const Complex);
+
+ fn fsk_rx_filt_to_llrs(
+ llr: *mut libc::c_float,
+ rx_filt: *const libc::c_float,
+ v_est: libc::c_float,
+ snr_est: libc::c_float,
+ m: libc::c_int,
+ n_syms: libc::c_int,
+ );
+
+ fn fsk_set_freq_est_limits(fsk: *mut InternalFsk, f_min: libc::c_int, f_max: libc::c_int);
+
+ fn fsk_set_freq_est_alg(fsk: *mut InternalFsk, est_type: libc::c_int);
+}
+
+#[repr(C)]
+struct InternalFsk {
+ /* Static parameters set up by fsk_init */
+ n_dft: libc::c_int, /* freq offset est fft */
+ fs: libc::c_int, /* sample freq */
+ n: libc::c_int, /* processing buffer size */
+ rs: libc::c_int, /* symbol rate */
+ ts: libc::c_int, /* samples per symbol */
+ n_mem: libc::c_int, /* size of extra mem for timing adj */
+ p: libc::c_int, /* oversample rate for timing est/adj */
+ n_sym: libc::c_int, /* Number of symbols processed by demodulator in each call, also the
+ timing estimator window */
+ n_bits: libc::c_int, /* Number of bits spat out in a processing frame */
+ f1_tx: libc::c_int, /* f1 for modulator */
+ tone_spacing: libc::c_int, /* Space between TX freqs for modulator (and option mask
+ freq estimator) */
+ mode: libc::c_int, /* 2FSK or 4FSK */
+ tc: libc::c_float, /* time constant for smoothing FFTs */
+ est_min: libc::c_int, /* Minimum frequency for freq. estimator */
+ est_max: libc::c_int, /* Maximum frequency for freq. estimator */
+ est_space: libc::c_int, /* Minimum frequency spacing for freq. estimator */
+ hann_table: *mut libc::c_float, /* Precomputed or runtime computed hann window table */
+
+ /* Parameters used by demod */
+ s_f: *mut libc::c_float, /* Average of magnitude spectrum */
+ phi_c: [Complex; MODE_M_MAX], /* phase of each demod local oscillator */
+ f_dc: *mut Complex, /* down converted samples */
+
+ fft_cfg: *mut libc::c_void, /* Config for KISS FFT, used in freq est */
+ norm_rx_timing: libc::c_float, /* Normalized RX timing */
+
+ /* Parameters used by mod */
+ tx_phase_c: Complex, /* TX phase, but complex */
+
+ /* Statistics generated by demod */
+ eb_no_db: libc::c_float, /* Estimated EbNo in dB */
+ f_est: [libc::c_float; MODE_M_MAX], /* Estimated frequencies (peak method) */
+ f2_est: [libc::c_float; MODE_M_MAX], /* Estimated frequencies (mask method) */
+ freq_est_type: libc::c_int, /* which estimator to use */
+ ppm: libc::c_float, /* Estimated PPM clock offset */
+ snr_est: libc::c_float, /* used for LLRs */
+ v_est: libc::c_float, /* used for LLRs */
+ rx_sig_pow: libc::c_float,
+ rx_nse_pow: libc::c_float,
+
+ /* Parameters used by mod/demod and driving code */
+ nin: libc::c_int, /* Number of samples to feed the next demod cycle */
+ burst_mode: libc::c_int, /* enables/disables 'burst' mode */
+ lock_nin: libc::c_int, /* locks nin during testing */
+
+ /* modem statistics struct */
+ stats: *mut libc::c_void,
+ normalise_eye: libc::c_int, /* enables/disables normalisation of eye diagram */
+}
+
+#[repr(C)]
+#[derive(Debug, Copy, Clone)]
+pub struct Complex {
+ pub real: libc::c_float,
+ pub imag: libc::c_float,
+}
+
+impl Complex {
+ pub const fn zero() -> Self {
+ Self {
+ real: 0.0,
+ imag: 0.0,
+ }
+ }
+}
+
+impl Sub for Complex {
+ type Output = Complex;
+
+ fn sub(self, rhs: Self) -> Complex {
+ Complex {
+ real: self.real - rhs.real,
+ imag: self.imag - rhs.imag,
+ }
+ }
+}
+
+impl AddAssign for Complex {
+ fn add_assign(&mut self, rhs: Self) {
+ self.real += rhs.real;
+ self.imag += rhs.imag;
+ }
+}
+
+impl Div<f32> for Complex {
+ type Output = Complex;
+
+ fn div(self, rhs: f32) -> Complex {
+ Complex {
+ real: self.real / rhs,
+ imag: self.imag / rhs,
+ }
+ }
+}
+
+pub struct Fsk<I: Iterator<Item = Complex>> {
+ internal: *mut InternalFsk,
+ input_cache: Vec<Complex>,
+ output_cache: VecDeque<f32>,
+ iq_iter: I,
+}
+
+impl<I: Iterator<Item = Complex>> Fsk<I> {
+ pub fn new(iq_iter: I) -> Self {
+ const P: i32 = 5;
+ const FS: i32 = 9600 * P;
+
+ let internal = unsafe { fsk_create_hbr(FS, 9600, M, P, N_SYM, -1, 9600) };
+
+ // Set upper/lower bound on the FSK peaks, from center
+ // (Hz)
+ let fsk_lower = -15_000;
+ let fsk_upper = 15_000;
+
+ unsafe {
+ fsk_set_freq_est_limits(internal, fsk_lower, fsk_upper);
+ fsk_set_freq_est_alg(internal, 1); // mask estimator
+ }
+
+ Self {
+ internal,
+ iq_iter,
+ input_cache: vec![],
+ output_cache: VecDeque::new(),
+ }
+ }
+}
+
+impl<I: Iterator<Item = Complex>> Iterator for Fsk<I> {
+ type Item = f32;
+
+ fn next(&mut self) -> Option<Self::Item> {
+ if let Some(x) = self.output_cache.pop_front() {
+ return Some(x);
+ }
+
+ let nin = unsafe { (*self.internal).nin }.try_into().unwrap();
+ while self.input_cache.len() < nin {
+ self.input_cache.push(self.iq_iter.next()?);
+ }
+
+ let n_bits = unsafe { (*self.internal).n_bits };
+ self.output_cache.resize(n_bits.try_into().unwrap(), 0.0);
+
+ let n_sym = unsafe { (*self.internal).n_sym };
+ let mut rx_filt = vec![0.0; usize::try_from(M * n_sym).unwrap()];
+ let rx_filt = rx_filt.as_mut_ptr();
+ let fsk_in = self.input_cache.as_ptr();
+ let llrs = self.output_cache.make_contiguous().as_mut_ptr();
+ unsafe {
+ fsk_demod_sd(self.internal, rx_filt, fsk_in);
+
+ fsk_rx_filt_to_llrs(
+ llrs,
+ rx_filt,
+ (*self.internal).v_est,
+ (*self.internal).snr_est,
+ (*self.internal).mode,
+ (*self.internal).n_sym,
+ );
+ }
+ self.input_cache.clear();
+
+ Some(self.output_cache.pop_front().unwrap())
+ }
+}
+
+impl<I: Iterator<Item = Complex>> Drop for Fsk<I> {
+ fn drop(&mut self) {
+ unsafe {
+ fsk_destroy(self.internal);
+ }
+ }
+}
diff --git a/src/codec2/modem_probe.c b/src/codec2/modem_probe.c
new file mode 100644
index 0000000000000000000000000000000000000000..fb7c75e212bc81e8239e3669e493d78cd26d7322
--- /dev/null
+++ b/src/codec2/modem_probe.c
@@ -0,0 +1,198 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: modem_probe.c
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 9 January 2016
+
+ Library to easily extract debug traces from modems during development and
+ verification
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2016 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/>.
+*/
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "comp.h"
+
+#define TRACE_I 1
+#define TRACE_F 2
+#define TRACE_C 3
+
+typedef struct probe_trace_info_s probe_trace_info;
+typedef struct datlink_s datlink;
+
+struct datlink_s {
+ void *data;
+ size_t len;
+ datlink *next;
+};
+
+struct probe_trace_info_s {
+ int type;
+ char name[255];
+ datlink *data;
+ datlink *last;
+ probe_trace_info *next;
+};
+
+static char *run = NULL;
+static char *mod = NULL;
+static probe_trace_info *first_trace = NULL;
+
+/* Init the probing library */
+void modem_probe_init_int(char *modname, char *runname) {
+ mod = malloc((strlen(modname) + 1) * sizeof(char));
+ run = malloc((strlen(runname) + 1) * sizeof(char));
+ strcpy(run, runname);
+ strcpy(mod, modname);
+}
+
+/*
+ * Gather the data stored in the linked list into a single blob,
+ * freeing links and buffers as it goes
+ */
+void *gather_data(datlink *d, size_t *len) {
+ size_t size = 0;
+ datlink *cur = d;
+ datlink *next;
+ while (cur != NULL) {
+ size += d->len;
+ cur = cur->next;
+ }
+ cur = d;
+ size_t i = 0;
+ void *newbuf = malloc(size);
+
+ while (cur != NULL) {
+ memcpy(newbuf + i, cur->data, cur->len);
+ i += cur->len;
+ free(cur->data);
+ next = cur->next;
+ free(cur);
+ cur = next;
+ }
+ *len = size;
+ return newbuf;
+}
+
+/* Look up or create a trace by name */
+probe_trace_info *modem_probe_get_trace(char *tracename) {
+ probe_trace_info *cur, *npti;
+
+ /* Make sure probe session is open */
+ if (run == NULL) return NULL;
+
+ cur = first_trace;
+ /* Walk through list, find trace with matching name */
+ while (cur != NULL) {
+ /* We got one! */
+ if (strcmp(cur->name, tracename) == 0) {
+ return cur;
+ }
+ cur = cur->next;
+ }
+ /* None found, open a new trace */
+
+ npti = (probe_trace_info *)malloc(sizeof(probe_trace_info));
+ npti->next = first_trace;
+ npti->data = NULL;
+ npti->last = NULL;
+ strcpy(npti->name, tracename);
+ first_trace = npti;
+
+ return npti;
+}
+
+void modem_probe_samp_i_int(char *tracename, int32_t samp[], size_t cnt) {
+ probe_trace_info *pti;
+ datlink *ndat;
+
+ pti = modem_probe_get_trace(tracename);
+ if (pti == NULL) return;
+
+ pti->type = TRACE_I;
+
+ ndat = (datlink *)malloc(sizeof(datlink));
+ ndat->data = malloc(sizeof(int32_t) * cnt);
+
+ ndat->len = cnt * sizeof(int32_t);
+ ndat->next = NULL;
+ memcpy(ndat->data, (void *)&(samp[0]), sizeof(int32_t) * cnt);
+
+ if (pti->last != NULL) {
+ pti->last->next = ndat;
+ pti->last = ndat;
+ } else {
+ pti->data = ndat;
+ pti->last = ndat;
+ }
+}
+
+void modem_probe_samp_f_int(char *tracename, float samp[], size_t cnt) {
+ probe_trace_info *pti;
+ datlink *ndat;
+
+ pti = modem_probe_get_trace(tracename);
+ if (pti == NULL) return;
+
+ pti->type = TRACE_F;
+
+ ndat = (datlink *)malloc(sizeof(datlink));
+ ndat->data = malloc(sizeof(float) * cnt);
+
+ ndat->len = cnt * sizeof(float);
+ ndat->next = NULL;
+ memcpy(ndat->data, (void *)&(samp[0]), sizeof(float) * cnt);
+
+ if (pti->last != NULL) {
+ pti->last->next = ndat;
+ pti->last = ndat;
+ } else {
+ pti->data = ndat;
+ pti->last = ndat;
+ }
+}
+
+void modem_probe_samp_c_int(char *tracename, COMP samp[], size_t cnt) {
+ probe_trace_info *pti;
+ datlink *ndat;
+
+ pti = modem_probe_get_trace(tracename);
+ if (pti == NULL) return;
+
+ pti->type = TRACE_C;
+
+ ndat = (datlink *)malloc(sizeof(datlink));
+ ndat->data = malloc(sizeof(COMP) * cnt);
+
+ ndat->len = cnt * sizeof(COMP);
+ ndat->next = NULL;
+ memcpy(ndat->data, (void *)&(samp[0]), sizeof(COMP) * cnt);
+
+ if (pti->last != NULL) {
+ pti->last->next = ndat;
+ pti->last = ndat;
+ } else {
+ pti->data = ndat;
+ pti->last = ndat;
+ }
+}
diff --git a/src/codec2/modem_probe.h b/src/codec2/modem_probe.h
new file mode 100644
index 0000000000000000000000000000000000000000..587ed032d0d3272487c56a891939395acfb8306b
--- /dev/null
+++ b/src/codec2/modem_probe.h
@@ -0,0 +1,129 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: modem_probe.h
+ AUTHOR......: Brady O'Brien
+ DATE CREATED: 9 January 2016
+
+ Library to easily extract debug traces from modems during development
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2016 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/>.
+*/
+
+#ifndef __MODEMPROBE_H
+#define __MODEMPROBE_H
+
+#include <complex.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "comp.h"
+
+#ifdef MODEMPROBE_ENABLE
+
+/* Internal functions */
+void modem_probe_init_int(char *modname, char *runname);
+void modem_probe_close_int();
+
+void modem_probe_samp_i_int(char *tracename, int samp[], size_t cnt);
+void modem_probe_samp_f_int(char *tracename, float samp[], size_t cnt);
+void modem_probe_samp_c_int(char *tracename, COMP samp[], size_t cnt);
+
+/*
+ * Init the probe library.
+ * char *modname - Name of the modem under test
+ * char *runname - Name/path of the file data is dumped to
+ */
+static inline void modem_probe_init(char *modname, char *runname) {
+ modem_probe_init_int(modname, runname);
+}
+
+/*
+ * Dump traces to a file and clean up
+ */
+static inline void modem_probe_close() { modem_probe_close_int(); }
+
+/*
+ * Save some number of int samples to a named trace
+ * char *tracename - name of trace being saved to
+ * int samp[] - int samples
+ * size_t cnt - how many samples to save
+ */
+static inline void modem_probe_samp_i(char *tracename, int samp[], size_t cnt) {
+ modem_probe_samp_i_int(tracename, samp, cnt);
+}
+
+/*
+ * Save some number of float samples to a named trace
+ * char *tracename - name of trace being saved to
+ * float samp[] - int samples
+ * size_t cnt - how many samples to save
+ */
+static inline void modem_probe_samp_f(char *tracename, float samp[],
+ size_t cnt) {
+ modem_probe_samp_f_int(tracename, samp, cnt);
+}
+
+/*
+ * Save some number of complex samples to a named trace
+ * char *tracename - name of trace being saved to
+ * COMP samp[] - int samples
+ * size_t cnt - how many samples to save
+ */
+static inline void modem_probe_samp_c(char *tracename, COMP samp[],
+ size_t cnt) {
+ modem_probe_samp_c_int(tracename, samp, cnt);
+}
+
+/*
+ * Save some number of complex samples to a named trace
+ * char *tracename - name of trace being saved to
+ * float complex samp[] - int samples
+ * size_t cnt - how many samples to save
+ */
+static inline void modem_probe_samp_cft(char *tracename, complex float samp[],
+ size_t cnt) {
+ modem_probe_samp_c_int(tracename, (COMP *)samp, cnt);
+}
+
+#else
+
+static inline void modem_probe_init(char *modname, char *runname) { return; }
+
+static inline void modem_probe_close() { return; }
+
+static inline void modem_probe_samp_i(char *name, int samp[], size_t sampcnt) {
+ return;
+}
+
+static inline void modem_probe_samp_f(char *name, float samp[], size_t cnt) {
+ return;
+}
+
+static inline void modem_probe_samp_c(char *name, COMP samp[], size_t cnt) {
+ return;
+}
+
+static inline void modem_probe_samp_cft(char *name, complex float samp[],
+ size_t cnt) {
+ return;
+}
+
+#endif
+
+#endif
diff --git a/src/codec2/modem_stats.c b/src/codec2/modem_stats.c
new file mode 100644
index 0000000000000000000000000000000000000000..6f83baf25ae3db02f08631f068913960360ed503
--- /dev/null
+++ b/src/codec2/modem_stats.c
@@ -0,0 +1,126 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: modem_stats.c
+ AUTHOR......: David Rowe
+ DATE CREATED: June 2015
+
+ Common functions for returning demod stats from fdmdv and cohpsk modems.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2015 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/>.
+*/
+
+#include "modem_stats.h"
+
+#include <assert.h>
+#include <math.h>
+
+#include "codec2_fdmdv.h"
+#include "kiss_fft.h"
+
+void modem_stats_open(struct MODEM_STATS *f) {
+ int i;
+
+ /* zero out all the stats */
+
+ memset(f, 0, sizeof(struct MODEM_STATS));
+
+ /* init the FFT */
+
+#ifndef __EMBEDDED__
+ for (i = 0; i < 2 * MODEM_STATS_NSPEC; i++) f->fft_buf[i] = 0.0;
+ f->fft_cfg = (void *)kiss_fft_alloc(2 * MODEM_STATS_NSPEC, 0, NULL, NULL);
+ assert(f->fft_cfg != NULL);
+#endif
+}
+
+void modem_stats_close(struct MODEM_STATS *f) {
+#ifndef __EMBEDDED__
+ KISS_FFT_FREE(f->fft_cfg);
+#endif
+}
+
+/*---------------------------------------------------------------------------*\
+
+ FUNCTION....: modem_stats_get_rx_spectrum()
+ AUTHOR......: David Rowe
+ DATE CREATED: 9 June 2012
+
+ Returns the MODEM_STATS_NSPEC point magnitude spectrum of the rx signal in
+ dB. The spectral samples are scaled so that 0dB is the peak, a good
+ range for plotting is 0 to -40dB.
+
+ Note only the real part of the complex input signal is used at
+ present. A complex variable is used for input for compatibility
+ with the other rx signal processing.
+
+ Successive calls can be used to build up a waterfall or spectrogram
+ plot, by mapping the received levels to colours.
+
+ The time-frequency resolution of the spectrum can be adjusted by varying
+ MODEM_STATS_NSPEC. Note that a 2* MODEM_STATS_NSPEC size FFT is reqd to get
+ MODEM_STATS_NSPEC output points. MODEM_STATS_NSPEC must be a power of 2.
+
+ See octave/tget_spec.m for a demo real time spectral display using
+ Octave. This demo averages the output over time to get a smoother
+ display:
+
+ av = 0.9*av + 0.1*mag_dB
+
+\*---------------------------------------------------------------------------*/
+
+#ifndef __EMBEDDED__
+void modem_stats_get_rx_spectrum(struct MODEM_STATS *f, float mag_spec_dB[],
+ COMP rx_fdm[], int nin) {
+ int i, j;
+ COMP fft_in[2 * MODEM_STATS_NSPEC];
+ COMP fft_out[2 * MODEM_STATS_NSPEC];
+ float full_scale_dB;
+
+ /* update buffer of input samples */
+
+ for (i = 0; i < 2 * MODEM_STATS_NSPEC - nin; i++)
+ f->fft_buf[i] = f->fft_buf[i + nin];
+ for (j = 0; j < nin; j++, i++) f->fft_buf[i] = rx_fdm[j].real;
+ assert(i == 2 * MODEM_STATS_NSPEC);
+
+ /* window and FFT */
+
+ for (i = 0; i < 2 * MODEM_STATS_NSPEC; i++) {
+ fft_in[i].real =
+ f->fft_buf[i] *
+ (0.5 - 0.5 * cosf((float)i * 2.0 * M_PI / (2 * MODEM_STATS_NSPEC)));
+ fft_in[i].imag = 0.0;
+ }
+
+ kiss_fft((kiss_fft_cfg)f->fft_cfg, (kiss_fft_cpx *)fft_in,
+ (kiss_fft_cpx *)fft_out);
+
+ /* FFT scales up a signal of level 1 FDMDV_NSPEC */
+
+ full_scale_dB = 20 * log10(MODEM_STATS_NSPEC * FDMDV_SCALE);
+
+ /* scale and convert to dB */
+
+ for (i = 0; i < MODEM_STATS_NSPEC; i++) {
+ mag_spec_dB[i] = 10.0 * log10f(fft_out[i].real * fft_out[i].real +
+ fft_out[i].imag * fft_out[i].imag + 1E-12);
+ mag_spec_dB[i] -= full_scale_dB;
+ }
+}
+#endif
diff --git a/src/codec2/modem_stats.h b/src/codec2/modem_stats.h
new file mode 100644
index 0000000000000000000000000000000000000000..c85fe528457110675db604c715895e356b589609
--- /dev/null
+++ b/src/codec2/modem_stats.h
@@ -0,0 +1,90 @@
+/*---------------------------------------------------------------------------*\
+
+ FILE........: modem_stats.h
+ AUTHOR......: David Rowe
+ DATE CREATED: June 2015
+
+ Common structure for returning demod stats from fdmdv and cohpsk modems.
+
+\*---------------------------------------------------------------------------*/
+
+/*
+ Copyright (C) 2015 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/>.
+*/
+
+#ifndef __MODEM_STATS__
+#define __MODEM_STATS__
+
+#include "comp.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MODEM_STATS_NC_MAX 50
+#define MODEM_STATS_NR_MAX 320
+#define MODEM_STATS_ET_MAX 8
+#define MODEM_STATS_EYE_IND_MAX 160
+#define MODEM_STATS_NSPEC 512
+#define MODEM_STATS_MAX_F_HZ 4000
+#define MODEM_STATS_MAX_F_EST 4
+
+struct MODEM_STATS {
+ int Nc;
+ float snr_est; /* estimated SNR of rx signal in dB (3 kHz noise BW) */
+#ifndef __EMBEDDED__
+ COMP rx_symbols[MODEM_STATS_NR_MAX][MODEM_STATS_NC_MAX + 1];
+ /* latest received symbols, for scatter plot */
+#endif
+ int nr; /* number of rows in rx_symbols */
+ int sync; /* demod sync state */
+ float foff; /* estimated freq offset in Hz */
+ float rx_timing; /* estimated optimum timing offset in samples */
+ float clock_offset; /* Estimated tx/rx sample clock offset in ppm */
+ float sync_metric; /* number between 0 and 1 indicating quality of sync */
+ int pre, post; /* preamble/postamble det counters for burst data */
+ int uw_fails; /* Failed to detect Unique word (burst data) */
+
+ /* FSK eye diagram traces */
+ /* Eye diagram plot -- first dim is trace number, second is the trace idx */
+#ifndef __EMBEDDED__
+ float rx_eye[MODEM_STATS_ET_MAX][MODEM_STATS_EYE_IND_MAX];
+ int neyetr; /* How many eye traces are plotted */
+ int neyesamp; /* How many samples in the eye diagram */
+
+ /* optional for FSK modems - est tone freqs */
+
+ float f_est[MODEM_STATS_MAX_F_EST];
+#endif
+
+ /* Buf for FFT/waterfall */
+
+#ifndef __EMBEDDED__
+ float fft_buf[2 * MODEM_STATS_NSPEC];
+ void *fft_cfg;
+#endif
+};
+
+void modem_stats_open(struct MODEM_STATS *f);
+void modem_stats_close(struct MODEM_STATS *f);
+void modem_stats_get_rx_spectrum(struct MODEM_STATS *f, float mag_spec_dB[],
+ COMP rx_fdm[], int nin);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/codec2/mpdecode_core.c b/src/codec2/mpdecode_core.c
new file mode 100644
index 0000000000000000000000000000000000000000..2795dc672616e6c3bb19ffcccc4969a25e4bb341
--- /dev/null
+++ b/src/codec2/mpdecode_core.c
@@ -0,0 +1,222 @@
+/*
+ FILE...: mpdecode_core.c
+ AUTHOR.: Matthew C. Valenti, Rohit Iyer Seshadri, David Rowe
+ CREATED: Sep 2016
+
+ C-callable core functions moved from MpDecode.c, so they can be used for
+ Octave and C programs.
+*/
+
+#include "mpdecode_core.h"
+
+#include <assert.h>
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#define QPSK_CONSTELLATION_SIZE 4
+#define QPSK_BITS_PER_SYMBOL 2
+
+/* QPSK constellation for symbol likelihood calculations */
+
+static COMP S_matrix[] = {
+ {1.0f, 0.0f}, {0.0f, 1.0f}, {0.0f, -1.0f}, {-1.0f, 0.0f}};
+
+// c_nodes will be an array of NumberParityBits of struct c_node
+// Each c_node contains an array of <degree> c_sub_node elements
+// This structure reduces the indexing calclations in SumProduct()
+
+struct c_sub_node { // Order is important here to keep total size small.
+ uint16_t index; // Values from H_rows (except last 2 entries)
+ uint16_t socket; // The socket number at the v_node
+ float message; // modified during operation!
+};
+
+struct c_node {
+ int degree; // A count of elements in the following arrays
+ struct c_sub_node *subs;
+};
+
+// v_nodes will be an array of CodeLength of struct v_node
+
+struct v_sub_node {
+ uint16_t index; // the index of a c_node it is connected to
+ // Filled with values from H_cols (except last 2 entries)
+ uint16_t socket; // socket number at the c_node
+ float message; // Loaded with input data
+ // modified during operation!
+ uint8_t sign; // 1 if input is negative
+ // modified during operation!
+};
+
+struct v_node {
+ int degree; // A count of ???
+ float initial_value;
+ struct v_sub_node *subs;
+};
+/* Values for linear approximation (DecoderType=5) */
+
+#define AJIAN -0.24904163195436
+#define TJIAN 2.50681740420944
+
+/* The linear-log-MAP algorithm */
+
+static float max_star0(float delta1, float delta2) {
+ register float diff;
+
+ diff = delta2 - delta1;
+
+ if (diff > TJIAN)
+ return (delta2);
+ else if (diff < -TJIAN)
+ return (delta1);
+ else if (diff > 0)
+ return (delta2 + AJIAN * (diff - TJIAN));
+ else
+ return (delta1 - AJIAN * (diff + TJIAN));
+}
+
+void Somap(float bit_likelihood[], /* number_bits, bps*number_symbols */
+ float symbol_likelihood[], /* M*number_symbols */
+ int M, /* constellation size */
+ int bps, /* bits per symbol */
+ int number_symbols) {
+ int n, i, j, k, mask;
+ float num[bps], den[bps];
+ float metric;
+
+ for (n = 0; n < number_symbols; n++) { /* loop over symbols */
+ for (k = 0; k < bps; k++) {
+ /* initialize */
+ num[k] = -1000000;
+ den[k] = -1000000;
+ }
+
+ for (i = 0; i < M; i++) {
+ metric =
+ symbol_likelihood[n * M + i]; /* channel metric for this symbol */
+
+ mask = 1 << (bps - 1);
+ for (j = 0; j < bps; j++) {
+ mask = mask >> 1;
+ }
+ mask = 1 << (bps - 1);
+
+ for (k = 0; k < bps; k++) { /* loop over bits */
+ if (mask & i) {
+ /* this bit is a one */
+ num[k] = max_star0(num[k], metric);
+ } else {
+ /* this bit is a zero */
+ den[k] = max_star0(den[k], metric);
+ }
+ mask = mask >> 1;
+ }
+ }
+ for (k = 0; k < bps; k++) {
+ bit_likelihood[bps * n + k] = num[k] - den[k];
+ }
+ }
+}
+
+void symbols_to_llrs(float llr[], COMP rx_qpsk_symbols[], float rx_amps[],
+ float EsNo, float mean_amp, int nsyms) {
+ int i;
+
+ float symbol_likelihood[nsyms * QPSK_CONSTELLATION_SIZE];
+ float bit_likelihood[nsyms * QPSK_BITS_PER_SYMBOL];
+
+ Demod2D(symbol_likelihood, rx_qpsk_symbols, S_matrix, EsNo, rx_amps, mean_amp,
+ nsyms);
+ Somap(bit_likelihood, symbol_likelihood, QPSK_CONSTELLATION_SIZE,
+ QPSK_BITS_PER_SYMBOL, nsyms);
+ for (i = 0; i < nsyms * QPSK_BITS_PER_SYMBOL; i++) {
+ llr[i] = -bit_likelihood[i];
+ }
+}
+
+/*
+ Description: Transforms M-dimensional FSK symbols into ML symbol
+ log-likelihoods
+
+ The calling syntax is:
+ [output] = FskDemod( input, EsNo, [csi_flag], [fade_coef] )
+
+ Where:
+ output = M by N matrix of symbol log-likelihoods
+
+ input = M by N matrix of (complex) matched filter outputs
+ EsNo = the symbol SNR (in linear, not dB, units)
+ csi_flag = 0 for coherent reception (default)
+ 1 for noncoherent reception w/ perfect amplitude estimates
+ 2 for noncoherent reception without amplitude estimates
+ fade_coef = 1 by N matrix of (complex) fading coefficients (defaults
+ to all-ones, i.e. AWGN)
+
+ Copyright (C) 2006, Matthew C. Valenti
+
+ Last updated on May 6, 2006
+
+ Function DemodFSK is part of the Iterative Solutions
+ Coded Modulation Library. The Iterative Solutions Coded Modulation
+ Library is free software; you can redistribute it and/or modify it
+ under the terms of the GNU Lesser General Public License as published
+ by the Free Software Foundation; either version 2.1 of the License,
+ or (at your option) any later version.
+
+ This library 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
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+
+*/
+
+/* the logI_0 function */
+static float logbesseli0(float x) {
+ if (x < 1)
+ return (0.226 * x * x + 0.0125 * x - 0.0012);
+ else if (x < 2)
+ return (0.1245 * x * x + 0.2177 * x - 0.108);
+ else if (x < 5)
+ return (0.0288 * x * x + 0.6314 * x - 0.5645);
+ else if (x < 20)
+ return (0.002 * x * x + 0.9048 * x - 1.2997);
+ else
+ return (0.9867 * x - 2.2053);
+}
+
+/* Function that does the demodulation (can be used in stand-alone C) */
+
+static void FskDemod(float out[], float yr[], float v_est, float SNR, int M,
+ int number_symbols) {
+ int i, j;
+ float y_envelope, scale_factor;
+
+ scale_factor = 2 * SNR;
+ for (i = 0; i < number_symbols; i++) {
+ for (j = 0; j < M; j++) {
+ y_envelope = sqrt(yr[j * number_symbols + i] *
+ yr[j * number_symbols + i] / (v_est * v_est));
+ out[i * M + j] = logbesseli0(scale_factor * y_envelope);
+ }
+ }
+}
+
+void fsk_rx_filt_to_llrs(float llr[], float rx_filt[], float v_est,
+ float SNRest, int M, int nsyms) {
+ int i;
+ int bps = log2(M);
+ float symbol_likelihood[M * nsyms];
+ float bit_likelihood[bps * nsyms];
+
+ FskDemod(symbol_likelihood, rx_filt, v_est, SNRest, M, nsyms);
+ Somap(bit_likelihood, symbol_likelihood, M, bps, nsyms);
+ for (i = 0; i < bps * nsyms; i++) {
+ llr[i] = -bit_likelihood[i];
+ }
+}
diff --git a/src/codec2/mpdecode_core.h b/src/codec2/mpdecode_core.h
new file mode 100644
index 0000000000000000000000000000000000000000..af4edc964e237d1985f1309db884a4a172385249
--- /dev/null
+++ b/src/codec2/mpdecode_core.h
@@ -0,0 +1,28 @@
+/*
+ FILE...: mpdecode_core.h
+ AUTHOR.: David Rowe
+ CREATED: Sep 2016
+
+ C-callable core functions for MpDecode, so they can be used for
+ Octave and C programs. Also some convenience functions to help use
+ the C-callable LDPC decoder in C programs.
+*/
+
+#ifndef __MPDECODE_CORE__
+#define __MPDECODE_CORE__
+
+#include <stdint.h>
+
+#include "comp.h"
+
+void sd_to_llr(float llr[], float sd[], int n);
+void Demod2D(float symbol_likelihood[], COMP r[], COMP S_matrix[], float EsNo,
+ float fading[], float mean_amp, int number_symbols);
+void Somap(float bit_likelihood[], float symbol_likelihood[], int M, int bps,
+ int number_symbols);
+void symbols_to_llrs(float llr[], COMP rx_qpsk_symbols[], float rx_amps[],
+ float EsNo, float mean_amp, int nsyms);
+void fsk_rx_filt_to_llrs(float llr[], float rx_filt[], float v_est,
+ float SNRest, int M, int nsyms);
+
+#endif
diff --git a/src/decoder/dc_bias.rs b/src/decoder/dc_bias.rs
new file mode 100644
index 0000000000000000000000000000000000000000..cd769ed794c504ad0f09c18c45a293d937f45d8e
--- /dev/null
+++ b/src/decoder/dc_bias.rs
@@ -0,0 +1,40 @@
+use crate::codec2::Complex;
+
+// 1 seconds @ 48KHz
+const AVERAGE_TIME: usize = 48_000;
+
+pub struct RemoveDcBias<I: Iterator<Item = Complex>> {
+ iter: I,
+ sum: Complex,
+ i: usize,
+ avg: Complex,
+}
+
+impl<I: Iterator<Item = Complex>> RemoveDcBias<I> {
+ pub fn new(iter: I) -> Self {
+ Self {
+ iter,
+ sum: Complex::zero(),
+ i: 0,
+ avg: Complex::zero(),
+ }
+ }
+}
+
+impl<I: Iterator<Item = Complex>> Iterator for RemoveDcBias<I> {
+ type Item = Complex;
+
+ fn next(&mut self) -> Option<Self::Item> {
+ let val = self.iter.next()?;
+
+ self.sum += val;
+ self.i += 1;
+ if self.i == AVERAGE_TIME {
+ self.avg = self.sum / (self.i as f32);
+ self.i = 0;
+ self.sum = Complex::zero();
+ }
+
+ Some(val - self.avg)
+ }
+}
diff --git a/src/decoder/demod.rs b/src/decoder/demod.rs
index 5eb526314ded25ae91335e2055f75bb0c7c3e726..f7a6b4eb26d0674c164ffa3b05798a4bf2192252 100644
--- a/src/decoder/demod.rs
+++ b/src/decoder/demod.rs
@@ -8,7 +8,7 @@ use crate::MAX_PACKET_LEN;
use super::packet::{PacketDecoder, Status};
const SYNC_WORD: u32 = 0xABCDEF12;
-const MAX_BIT_FLIPS: u32 = 3;
+const MAX_BIT_FLIPS: u32 = 5;
pub struct Demod<T: DecodeFrom, I: Iterator<Item = T>> {
rolling_sync: u32,
@@ -49,8 +49,8 @@ impl<T: DecodeFrom, I: Iterator<Item = T>> Demod<T, I> {
}
}
- for i in to_remove {
- self.decoders.swap_remove(i);
+ for i in to_remove.iter().rev() {
+ self.decoders.swap_remove(*i);
}
if self.sync_matches() {
@@ -62,6 +62,6 @@ impl<T: DecodeFrom, I: Iterator<Item = T>> Demod<T, I> {
fn sync_matches(&self) -> bool {
let delta = self.rolling_sync ^ SYNC_WORD;
- delta.count_ones() < MAX_BIT_FLIPS
+ delta.count_ones() <= MAX_BIT_FLIPS
}
}
diff --git a/src/decoder/mod.rs b/src/decoder/mod.rs
index 1d57ce9e05b315e653e1afd5927f0379160dce95..3b77cc0b868802802bf1131a601fdd808f96e74d 100644
--- a/src/decoder/mod.rs
+++ b/src/decoder/mod.rs
@@ -1,19 +1,21 @@
use std::io::{self, Read};
+use itertools::Itertools;
use tokio::sync::mpsc;
use uuid::Uuid;
use crate::{
+ codec2::{Complex, Fsk},
config::FelinetConfig,
felinet::PacketIn as SemiPacketIn,
util::{append_internet_to_packet_route, print_packet},
};
-use self::{demod::Demod, parse::LeF32Decoder};
+use self::{dc_bias::RemoveDcBias, demod::Demod};
+mod dc_bias;
mod demod;
mod packet;
-mod parse;
pub fn decode_forever(
felinet_send: mpsc::Sender<SemiPacketIn>,
@@ -21,7 +23,22 @@ pub fn decode_forever(
uuid: &Uuid,
) {
let stdin = io::stdin();
- let soft_bits = LeF32Decoder::new(stdin.bytes().map(|x| x.expect("Could not read from stdin")));
+ let iq = stdin
+ .bytes()
+ .map(|x| x.expect("Could not read from stdin"))
+ .tuples()
+ .map(|(a, b)| i16::from_le_bytes([a, b]))
+ .tuples()
+ .map(|(real, imag)| {
+ let real = real as f32;
+ let imag = imag as f32;
+ Complex { real, imag }
+ });
+
+ let iq_no_dc = RemoveDcBias::new(iq);
+
+ let soft_bits = Fsk::new(iq_no_dc);
+
let demod = Demod::new(soft_bits);
demod.demod(|mut p| {
diff --git a/src/decoder/parse.rs b/src/decoder/parse.rs
deleted file mode 100644
index 0a187863f7141e5a1807af000da9f00a4d621ca5..0000000000000000000000000000000000000000
--- a/src/decoder/parse.rs
+++ /dev/null
@@ -1,22 +0,0 @@
-pub struct LeF32Decoder<I: Iterator<Item = u8>> {
- iter: I,
-}
-
-impl<I: Iterator<Item = u8>> LeF32Decoder<I> {
- pub fn new(iter: I) -> Self {
- Self { iter }
- }
-}
-
-impl<I: Iterator<Item = u8>> Iterator for LeF32Decoder<I> {
- type Item = f32;
-
- fn next(&mut self) -> Option<f32> {
- let mut bytes = [0; 4];
- for b in &mut bytes {
- *b = self.iter.next()?;
- }
-
- Some(f32::from_le_bytes(bytes))
- }
-}
diff --git a/src/main.rs b/src/main.rs
index 7f2a568beadf9d00fb2d4a28f825315985d33bb6..8d8a75489fb3327f51211a9be72c87996343ba9a 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -8,6 +8,7 @@ use tokio::sync::mpsc;
use tonic::{transport::Endpoint, Request};
use uuid::Uuid;
+mod codec2;
mod config;
mod decoder;
mod gate;