From 300fd37880f67f66da85f69d4145a237356fde63 Mon Sep 17 00:00:00 2001 From: f4exb Date: Mon, 22 Mar 2021 02:06:26 +0100 Subject: [PATCH] DATV demod: leandvb: more memory management fixes and code formatting --- .../demoddatv/ldpctool/tables_handler.cpp | 53 +- plugins/channelrx/demoddatv/leansdr/bch.h | 99 ++- plugins/channelrx/demoddatv/leansdr/dsp.h | 207 +++-- plugins/channelrx/demoddatv/leansdr/dvbs2.h | 764 ++++++++++++++---- .../channelrx/demoddatv/leansdr/filtergen.cpp | 7 +- .../channelrx/demoddatv/leansdr/filtergen.h | 46 +- .../channelrx/demoddatv/leansdr/framework.cpp | 6 +- .../channelrx/demoddatv/leansdr/framework.h | 163 ++-- plugins/channelrx/demoddatv/leansdr/generic.h | 326 ++++++-- plugins/channelrx/demoddatv/leansdr/hdlc.h | 140 ++-- plugins/channelrx/demoddatv/leansdr/iess.h | 20 +- plugins/channelrx/demoddatv/leansdr/ldpc.h | 227 ++++-- plugins/channelrx/demoddatv/leansdr/math.cpp | 5 +- plugins/channelrx/demoddatv/leansdr/math.h | 66 +- plugins/channelrx/demoddatv/leansdr/rs.h | 120 ++- plugins/channelrx/demoddatv/leansdr/sdr.cpp | 18 +- plugins/channelrx/demoddatv/leansdr/sdr.h | 212 +++-- .../channelrx/demoddatv/leansdr/softword.h | 42 +- plugins/channelrx/demoddatv/leansdr/viterbi.h | 110 ++- 19 files changed, 1980 insertions(+), 651 deletions(-) diff --git a/plugins/channelrx/demoddatv/ldpctool/tables_handler.cpp b/plugins/channelrx/demoddatv/ldpctool/tables_handler.cpp index 1887bd801..deea54c11 100644 --- a/plugins/channelrx/demoddatv/ldpctool/tables_handler.cpp +++ b/plugins/channelrx/demoddatv/ldpctool/tables_handler.cpp @@ -30,9 +30,12 @@ const char *LDPCInterface::mc_tabnames[2][32] = { // [shortframes][modcod] LDPCInterface *create_ldpc(char *standard, char prefix, int number) { - if (!strcmp(standard, "S2")) { - if (prefix == 'B') { - switch (number) { + if (!strcmp(standard, "S2")) + { + if (prefix == 'B') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -57,8 +60,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) return new LDPC(); } } - if (prefix == 'C') { - switch (number) { + + if (prefix == 'C') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -82,9 +88,13 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) } } } - if (!strcmp(standard, "S2X")) { - if (prefix == 'B') { - switch (number) { + + if (!strcmp(standard, "S2X")) + { + if (prefix == 'B') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -135,8 +145,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) return new LDPC(); } } - if (prefix == 'C') { - switch (number) { + + if (prefix == 'C') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -160,9 +173,13 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) } } } - if (!strcmp(standard, "T2")) { - if (prefix == 'A') { - switch (number) { + + if (!strcmp(standard, "T2")) + { + if (prefix == 'A') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -177,8 +194,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) return new LDPC(); } } - if (prefix == 'B') { - switch (number) { + + if (prefix == 'B') + { + switch (number) + { case 1: return new LDPC(); case 2: @@ -200,7 +220,8 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number) } } } - return 0; + + return nullptr; } constexpr int DVB_S2_TABLE_B1::DEG[]; diff --git a/plugins/channelrx/demoddatv/leansdr/bch.h b/plugins/channelrx/demoddatv/leansdr/bch.h index 5462a7681..d937ed02b 100644 --- a/plugins/channelrx/demoddatv/leansdr/bch.h +++ b/plugins/channelrx/demoddatv/leansdr/bch.h @@ -26,6 +26,7 @@ namespace leansdr struct bch_interface { + virtual ~bch_interface() {} virtual void encode(const uint8_t *msg, size_t msgbytes, uint8_t *out) = 0; virtual int decode(uint8_t *cw, size_t cwbytes) = 0; }; // bch_interface @@ -41,42 +42,67 @@ struct bch_interface template struct bch_engine : bch_interface { - bch_engine(const bitvect *polys, int _npolys) - : npolys(_npolys) + bch_engine( + const bitvect *polys, + int _npolys + ) : + npolys(_npolys) { // Build the generator polynomial (product of polys[]). g = 1; - for (int i = 0; i < npolys; ++i) + + for (int i = 0; i < npolys; ++i) { g = g * polys[i]; + } + // Convert the polynomials to truncated representation // (with X^DP omitted) for use with divmod(). truncpolys = new bitvect[npolys]; - for (int i = 0; i < npolys; ++i) + + for (int i = 0; i < npolys; ++i) { truncpolys[i].copy(polys[i]); + } + // Check which polynomial contains each root. // Note: The DVB-S2 polynomials are numbered so that // syndpoly[2*i]==i, but we don't use that property. syndpolys = new int[2 * npolys]; + for (int i = 0; i < 2 * npolys; ++i) { int j; + for (j = 0; j < npolys; ++j) - if (!eval_poly(truncpolys[j], true, 1 + i)) + { + if (!eval_poly(truncpolys[j], true, 1 + i)) { break; - if (j == npolys) + } + } + + if (j == npolys) { fail("Bad polynomials/root"); + } + syndpolys[i] = j; } } + virtual ~bch_engine() + { + delete[] truncpolys; + delete[] syndpolys; + } + // Generate BCH parity bits. void encode(const uint8_t *msg, size_t msgbytes, uint8_t *out) { bitvect parity = shiftdivmod(msg, msgbytes, g); + // Output as bytes, coefficient of highest degree first - for (int i = N / 8; i--; ++out) + for (int i = N / 8; i--; ++out) { *out = parity.v[i / sizeof(T)] >> ((i & (sizeof(T) - 1)) * 8); + } } // Decode BCH. @@ -89,12 +115,15 @@ struct bch_engine : bch_interface // Divide by individual polynomials. // TBD Maybe do in parallel, scanning cw only once. bitvect *rem = new bitvect[npolys]; // npolys is not static hence 'bitvect rem[npolys]' does not compile in all compilers + for (int j = 0; j < npolys; ++j) { rem[j] = divmod(cw, cwbytes, truncpolys[j]); } + // Compute syndromes. TGF *S = new TGF[2 * npolys]; // npolys is not static hence 'TGF S[2 * npolys]' does not compile in all compilers + for (int i = 0; i < 2 * npolys; ++i) { // Compute R(alpha^(1+i)), exploiting the fact that @@ -102,9 +131,12 @@ struct bch_engine : bch_interface // for some j that we already determined. // TBD Compute even exponents using conjugates. S[i] = eval_poly(rem[syndpolys[i]], false, 1 + i); - if (S[i]) + + if (S[i]) { corrupted = true; + } } + if (!corrupted) { delete[] S; @@ -138,32 +170,45 @@ struct bch_engine : bch_interface // }; int L = 0, m = 1; TGF b = 1; + for (int n = 0; n < NN; ++n) { TGF d = S[n]; - for (int i = 1; i <= L; ++i) + + for (int i = 1; i <= L; ++i) { d = GF.add(d, GF.mul(C[i], S[n - i])); + } + if (d == 0) + { ++m; + } else { TGF d_div_b = GF.mul(d, GF.inv(b)); + if (2 * L <= n) { TGF *tmp = new TGF[NN]; // replaced crap code std::copy(C, C+NN, tmp); //memcpy(tmp, C, sizeof(tmp)); - for (int i = 0; i < NN - m; ++i) + + for (int i = 0; i < NN - m; ++i) { C[m + i] = GF.sub(C[m + i], GF.mul(d_div_b, B[i])); + } + L = n + 1 - L; std::copy(tmp, tmp+NN, B); //memcpy(B, tmp, sizeof(B)); b = d; m = 1; + delete[] tmp; } else { - for (int i = 0; i < NN - m; ++i) + for (int i = 0; i < NN - m; ++i) { C[m + i] = GF.sub(C[m + i], GF.mul(d_div_b, B[i])); + } + ++m; } } @@ -184,16 +229,19 @@ struct bch_engine : bch_interface // Find zeroes of C by exhaustive search. // TODO Chien method int roots_found = 0; + for (int i = 0; i < (1 << DP) - 1; ++i) { // Candidate root ALPHA^i TGF v = eval_poly(C, L, i); + if (!v) { // ALPHA^i is a root of C, i.e. the inverse of an X_l. int loc = (i ? (1 << DP) - 1 - i : 0); // exponent of inverse // Reverse because cw[0..cwbytes-1] is stored MSB first int rloc = cwbytes * 8 - 1 - loc; + if (rloc < 0) { // This may happen if the code is used truncated. @@ -203,10 +251,13 @@ struct bch_engine : bch_interface delete[] rem; return -1; } + cw[rloc / 8] ^= 128 >> (rloc & 7); ++roots_found; - if (roots_found == L) + + if (roots_found == L) { break; + } } } @@ -215,8 +266,10 @@ struct bch_engine : bch_interface delete[] S; delete[] rem; - if (roots_found != L) + if (roots_found != L) { return -1; + } + return L; } @@ -227,16 +280,24 @@ struct bch_engine : bch_interface { TGF acc = 0; int re = 0; + for (int i = 0; i < DP; ++i) { - if (poly[i]) + if (poly[i]) { acc = GF.add(acc, GF.exp(re)); + } + re += rootexp; - if (re >= (1 << DP) - 1) + + if (re >= (1 << DP) - 1) { re -= (1 << DP) - 1; // mod 2^DP-1 incrementally + } } - if (is_trunc) + + if (is_trunc) { acc = GF.add(acc, GF.exp(re)); + } + return acc; } @@ -246,13 +307,17 @@ struct bch_engine : bch_interface { TGF acc = 0; int re = 0; + for (int i = 0; i <= deg; ++i) { acc = GF.add(acc, GF.mul(poly[i], GF.exp(re))); re += rootexp; - if (re >= (1 << DP) - 1) + + if (re >= (1 << DP) - 1) { re -= (1 << DP) - 1; // mod 2^DP-1 incrementally + } } + return acc; } diff --git a/plugins/channelrx/demoddatv/leansdr/dsp.h b/plugins/channelrx/demoddatv/leansdr/dsp.h index 3061936e6..e6f924d6c 100644 --- a/plugins/channelrx/demoddatv/leansdr/dsp.h +++ b/plugins/channelrx/demoddatv/leansdr/dsp.h @@ -33,22 +33,29 @@ namespace leansdr template struct cconverter : runnable { - cconverter(scheduler *sch, pipebuf> &_in, - pipebuf> &_out) - : runnable(sch, "cconverter"), - in(_in), out(_out) + cconverter( + scheduler *sch, + pipebuf> &_in, + pipebuf> &_out + ) : + runnable(sch, "cconverter"), + in(_in), + out(_out) { } + void run() { unsigned long count = min(in.readable(), out.writable()); complex *pin = in.rd(), *pend = pin + count; complex *pout = out.wr(); + for (; pin < pend; ++pin, ++pout) { pout->re = Zout + (pin->re - (Tin)Zin) * Gn / Gd; pout->im = Zout + (pin->im - (Tin)Zin) * Gn / Gd; } + in.read(count); out.written(count); } @@ -82,21 +89,29 @@ struct cfft_engine release(); n = _n; invsqrtn = 1.0 / sqrt(n); + // Compute log2(n) logn = 0; - for (int t = n; t > 1; t >>= 1) + for (int t = n; t > 1; t >>= 1) { ++logn; + } + // Bit reversal bitrev = new int[n]; + for (int i = 0; i < n; ++i) { bitrev[i] = 0; - for (int b = 0; b < logn; ++b) + + for (int b = 0; b < logn; ++b) { bitrev[i] = (bitrev[i] << 1) | ((i >> b) & 1); + } } + // Float constants omega = new complex[n]; omega_rev = new complex[n]; + for (int i = 0; i < n; ++i) { float a = 2.0 * M_PI * i / n; @@ -111,6 +126,7 @@ struct cfft_engine for (int i = 0; i < n; ++i) { int r = bitrev[i]; + if (r < i) { complex tmp = data[i]; @@ -118,15 +134,18 @@ struct cfft_engine data[r] = tmp; } } + complex *om = reverse ? omega_rev : omega; // Danielson-Lanczos for (int i = 0; i < logn; ++i) { int hbs = 1 << i; int dom = 1 << (logn - 1 - i); + for (int j = 0; j < dom; ++j) { int p = j * hbs * 2, q = p + hbs; + for (int k = 0; k < hbs; ++k) { complex &w = om[k * dom]; @@ -140,9 +159,11 @@ struct cfft_engine } } } + if (reverse) { float invn = 1.0 / n; + for (int i = 0; i < n; ++i) { data[i].re *= invn; @@ -176,22 +197,37 @@ struct cfft_engine template struct adder : runnable { - adder(scheduler *sch, - pipebuf &_in1, pipebuf &_in2, pipebuf &_out) - : runnable(sch, "adder"), - in1(_in1), in2(_in2), out(_out) + adder( + scheduler *sch, + pipebuf &_in1, + pipebuf &_in2, + pipebuf &_out + ) : + runnable(sch, "adder"), + in1(_in1), + in2(_in2), + out(_out) { } + void run() { int n = out.writable(); - if (in1.readable() < n) + + if (in1.readable() < n) { n = in1.readable(); - if (in2.readable() < n) + } + + if (in2.readable() < n) { n = in2.readable(); + } + T *pin1 = in1.rd(), *pin2 = in2.rd(), *pout = out.wr(), *pend = pout + n; - while (pout < pend) + + while (pout < pend) { *pout++ = *pin1++ + *pin2++; + } + in1.read(n); in2.read(n); out.written(n); @@ -206,20 +242,30 @@ template struct scaler : runnable { Tscale scale; - scaler(scheduler *sch, Tscale _scale, - pipebuf &_in, pipebuf &_out) - : runnable(sch, "scaler"), - scale(_scale), - in(_in), out(_out) + + scaler( + scheduler *sch, + Tscale _scale, + pipebuf &_in, + pipebuf &_out + ) : + runnable(sch, "scaler"), + scale(_scale), + in(_in), + out(_out) { } + void run() { unsigned long count = min(in.readable(), out.writable()); Tin *pin = in.rd(), *pend = pin + count; Tout *pout = out.wr(); - for (; pin < pend; ++pin, ++pout) + + for (; pin < pend; ++pin, ++pout) { *pout = *pin * scale; + } + in.read(count); out.written(count); } @@ -234,29 +280,40 @@ struct scaler : runnable template struct wgn_c : runnable { - wgn_c(scheduler *sch, pipebuf> &_out) - : runnable(sch, "awgn"), stddev(1.0), out(_out) + wgn_c( + scheduler *sch, + pipebuf> + &_out + ) : + runnable(sch, "awgn"), + stddev(1.0), + out(_out) { } + void run() { int n = out.writable(); complex *pout = out.wr(), *pend = pout + n; + while (pout < pend) { // TAOCP float x, y, r2; + do { x = 2 * drand48() - 1; y = 2 * drand48() - 1; r2 = x * x + y * y; } while (r2 == 0 || r2 >= 1); + float k = sqrtf(-logf(r2) / r2) * stddev; pout->re = k * x; pout->im = k * y; ++pout; } + out.written(n); } float stddev; @@ -268,26 +325,41 @@ struct wgn_c : runnable template struct naive_lowpass : runnable { - naive_lowpass(scheduler *sch, pipebuf &_in, pipebuf &_out, int _w) - : runnable(sch, "lowpass"), in(_in), out(_out), w(_w) + naive_lowpass( + scheduler *sch, + pipebuf &_in, + pipebuf &_out, + int _w + ) : + runnable(sch, "lowpass"), + in(_in), + out(_out), + w(_w) { } void run() { - if (in.readable() < w) + if (in.readable() < w) { return; + } + unsigned long count = min(in.readable() - w, out.writable()); T *pin = in.rd(), *pend = pin + count; T *pout = out.wr(); float k = 1.0 / w; + for (; pin < pend; ++pin, ++pout) { T x = 0.0; - for (int i = 0; i < w; ++i) + + for (int i = 0; i < w; ++i) { x = x + pin[i]; + } + *pout = x * k; } + in.read(count); out.written(count); } @@ -301,19 +373,32 @@ struct naive_lowpass : runnable template struct fir_filter : runnable { - fir_filter(scheduler *sch, int _ncoeffs, Tc *_coeffs, - pipebuf &_in, pipebuf &_out, - unsigned int _decim = 1) - : runnable(sch, "fir_filter"), - ncoeffs(_ncoeffs), coeffs(_coeffs), - in(_in), out(_out), - decim(_decim), - freq_tap(NULL), tap_multiplier(1), freq_tol(0.1) + fir_filter( + scheduler *sch, + int _ncoeffs, + Tc *_coeffs, + pipebuf &_in, + pipebuf &_out, + unsigned int _decim = 1 + ) : + runnable(sch, "fir_filter"), + ncoeffs(_ncoeffs), + coeffs(_coeffs), + in(_in), + out(_out), + decim(_decim), + freq_tap(nullptr), + tap_multiplier(1), + freq_tol(0.1) { shifted_coeffs = new T[ncoeffs]; set_freq(0); } + ~fir_filter() { + delete[] shifted_coeffs; + } + void run() { if (in.readable() < ncoeffs) @@ -348,15 +433,20 @@ struct fir_filter : runnable out.written(count); } + public: + float *freq_tap; + float tap_multiplier; + float freq_tol; + private: int ncoeffs; Tc *coeffs; pipereader in; pipewriter out; int decim; - T *shifted_coeffs; float current_freq; + void set_freq(float f) { for (int i = 0; i < ncoeffs; ++i) @@ -369,11 +459,6 @@ struct fir_filter : runnable } current_freq = f; } - - public: - float *freq_tap; - float tap_multiplier; - float freq_tol; }; // fir_filter // FIR FILTER WITH INTERPOLATION AND DECIMATION @@ -381,14 +466,25 @@ struct fir_filter : runnable template struct fir_resampler : runnable { - fir_resampler(scheduler *sch, int _ncoeffs, Tc *_coeffs, - pipebuf &_in, pipebuf &_out, - int _interp = 1, int _decim = 1) - : runnable(sch, "fir_resampler"), - ncoeffs(_ncoeffs), coeffs(_coeffs), - interp(_interp), decim(_decim), - in(_in), out(_out, interp), - freq_tap(NULL), tap_multiplier(1), freq_tol(0.1) + fir_resampler( + scheduler *sch, + int _ncoeffs, + Tc *_coeffs, + pipebuf &_in, + pipebuf &_out, + int _interp = 1, + int _decim = 1 + ) : + runnable(sch, "fir_resampler"), + ncoeffs(_ncoeffs), + coeffs(_coeffs), + interp(_interp), + decim(_decim), + in(_in), + out(_out, interp), + freq_tap(nullptr), + tap_multiplier(1), + freq_tol(0.1) { if (decim != 1) fail("fir_resampler: decim not implemented"); // TBD @@ -396,6 +492,10 @@ struct fir_resampler : runnable set_freq(0); } + ~fir_resampler() { + delete[] shifted_coeffs; + } + void run() { if (in.readable() < ncoeffs) @@ -436,21 +536,20 @@ struct fir_resampler : runnable out.written(count * interp); } - private: - unsigned int ncoeffs; - Tc *coeffs; - int interp, decim; - pipereader in; - pipewriter out; - public: float *freq_tap; float tap_multiplier; float freq_tol; private: + unsigned int ncoeffs; + Tc *coeffs; + int interp, decim; + pipereader in; + pipewriter out; T *shifted_coeffs; float current_freq; + void set_freq(float f) { for (int i = 0; i < ncoeffs; ++i) diff --git a/plugins/channelrx/demoddatv/leansdr/dvbs2.h b/plugins/channelrx/demoddatv/leansdr/dvbs2.h index 1a1a3cf0f..36c0f6cf8 100644 --- a/plugins/channelrx/demoddatv/leansdr/dvbs2.h +++ b/plugins/channelrx/demoddatv/leansdr/dvbs2.h @@ -66,6 +66,7 @@ struct s2_sof static const uint32_t MASK = 0x3ffffff; static const int LENGTH = 26; complex symbols[LENGTH]; + s2_sof() { for (int s = 0; s < LENGTH; ++s) @@ -89,6 +90,7 @@ struct s2_plscodes static const int LENGTH = 64; uint64_t codewords[COUNT]; complex symbols[COUNT][LENGTH]; + s2_plscodes() { uint32_t G[6] = {0x55555555, @@ -97,26 +99,36 @@ struct s2_plscodes 0x00ff00ff, 0x0000ffff, 0xffffffff}; + for (int index = 0; index < COUNT; ++index) { uint32_t y = 0; + for (int row = 0; row < 6; ++row) - if ((index >> (6 - row)) & 1) + { + if ((index >> (6 - row)) & 1) { y ^= G[row]; + } + } + uint64_t code = 0; + for (int bit = 31; bit >= 0; --bit) { int yi = (y >> bit) & 1; - if (index & 1) + + if (index & 1) { code = (code << 2) | (yi << 1) | (yi ^ 1); - else + } else { code = (code << 2) | (yi << 1) | yi; + } } // Scrambling code ^= SCRAMBLING; // Store precomputed codeword. codewords[index] = code; // Also store as symbols. + for (int i = 0; i < LENGTH; ++i) { int yi = (code >> (LENGTH - 1 - i)) & 1; @@ -136,12 +148,16 @@ struct s2_plscodes struct s2_scrambling { uint8_t Rn[131072]; // 0..3 (* 2pi/4) + s2_scrambling(int codenum = 0) { uint32_t stx = 0x00001, sty = 0x3ffff; + // x starts at codenum, wraps at index 2^18-1 by design - for (int i = 0; i < codenum; ++i) + for (int i = 0; i < codenum; ++i) { stx = lfsr_x(stx); + } + // First half of sequence is LSB of scrambling angle for (int i = 0; i < 131072; ++i) { @@ -150,6 +166,7 @@ struct s2_scrambling stx = lfsr_x(stx); sty = lfsr_y(sty); } + // Second half is MSB for (int i = 0; i < 131072; ++i) { @@ -159,11 +176,13 @@ struct s2_scrambling sty = lfsr_y(sty); } } + uint32_t lfsr_x(uint32_t X) { int bit = ((X >> 7) ^ X) & 1; return ((bit << 18) | X) >> 1; } + uint32_t lfsr_y(uint32_t Y) { int bit = ((Y >> 10) ^ (Y >> 7) ^ (Y >> 5) ^ Y) & 1; @@ -180,27 +199,32 @@ struct s2_bbscrambling s2_bbscrambling() { uint16_t st = 0x00a9; // 000 0000 1010 1001 (Fig 5 reversed) + for (unsigned int i = 0; i < sizeof(pattern); ++i) { uint8_t out = 0; + for (int n = 8; n--;) { int bit = ((st >> 13) ^ (st >> 14)) & 1; // Taps out = (out << 1) | bit; // MSB first st = (st << 1) | bit; // Feedback } + pattern[i] = out; } } + void transform(const uint8_t *in, int bbsize, uint8_t *out) { - for (int i = 0; i < bbsize; ++i) + for (int i = 0; i < bbsize; ++i) { out[i] = in[i] ^ pattern[i]; + } } private: uint8_t pattern[58192]; // Values 0..3 -}; // s2_bbscrambling +}; // s2_bbscrambling // S2 PHYSICAL LAYER SIGNALLING @@ -209,7 +233,10 @@ struct s2_pls int modcod; // 0..31 bool sf; bool pilots; - int framebits() const { return sf ? 16200 : 64800; } + + int framebits() const { + return sf ? 16200 : 64800; + } }; template @@ -286,11 +313,16 @@ const struct modcod_info // Assert that a MODCOD number is valid const modcod_info *check_modcod(int m) { - if (m < 0 || m > 31) + if (m < 0 || m > 31) { fail("Invalid MODCOD number"); + } + const modcod_info *r = &modcod_infos[m]; - if (!r->nslots_nf) + + if (!r->nslots_nf) { fail("Unsupported MODCOD"); + } + return r; } @@ -306,7 +338,9 @@ struct s2_frame_transmitter : runnable ) : runnable(sch, "S2 frame transmitter"), in(_in), - out(_out, modcod_info::MAX_SYMBOLS_PER_FRAME) + out(_out, modcod_info::MAX_SYMBOLS_PER_FRAME), + cstln(nullptr), + csymbols(nullptr) { float amp = cstln_amp / sqrtf(2); qsymbols[0].re = +amp; @@ -318,34 +352,62 @@ struct s2_frame_transmitter : runnable qsymbols[3].re = -amp; qsymbols[3].im = -amp; } + + ~s2_frame_transmitter() + { + if (cstln) { + delete cstln; + } + if (csymbols) { + delete csymbols; + } + } + void run() { while (in.readable() >= 1) { plslot *pin = in.rd(); - if (!pin->is_pls) + + if (!pin->is_pls) { fail("Expected PLS pseudo-slot"); + } + s2_pls *pls = &pin->pls; const modcod_info *mcinfo = check_modcod(pls->modcod); int nslots = (pls->sf ? mcinfo->nslots_nf / 4 : mcinfo->nslots_nf); - if (in.readable() < 1 + nslots) + + if (in.readable() < 1 + nslots) { break; + } + // Require room for BBHEADER + slots + optional pilots. int nsymbols = ((1 + nslots) * plslot::LENGTH + (pls->pilots ? ((nslots - 1) / 16) * pilot_length : 0)); - if (out.writable() < nsymbols) + + if (out.writable() < nsymbols) { break; + } + update_cstln(mcinfo); int nw = run_frame(pls, mcinfo, pin + 1, nslots, out.wr()); - if (nw != nsymbols) + + if (nw != nsymbols) { fail("Bug: s2_frame_transmitter overflow"); + } + in.read(1 + nslots); out.written(nsymbols); } } - int run_frame(s2_pls *pls, const modcod_info *mcinfo, - const plslot *pin, int nslots, - complex *pout) + + int run_frame( + s2_pls *pls, + const modcod_info *mcinfo, + const plslot *pin, + int nslots, + complex *pout + ) { complex *pout0 = pout; // For sanity check // PLHEADER: SOF AND PLSCODE @@ -358,24 +420,33 @@ struct s2_frame_transmitter : runnable // Slots and pilots int till_next_pilot = pls->pilots ? 16 : nslots; uint8_t *scr = &scrambling.Rn[0]; + for (int S = 0; S < nslots; ++S, ++pin, --till_next_pilot) { if (till_next_pilot == 0) { // Send pilot - for (int s = 0; s < pilot_length; ++s, ++scr, ++pout) + for (int s = 0; s < pilot_length; ++s, ++scr, ++pout) { scramble(&qsymbols[0], *scr, pout); + } till_next_pilot = 16; } + // Send slot - if (pin->is_pls) + if (pin->is_pls) { fail("s2_frame_transmitter: bad input sequence"); + } + const hard_ss *ps = pin->symbols; - for (int s = 0; s < pin->LENGTH; ++s, ++ps, ++scr, ++pout) + + for (int s = 0; s < pin->LENGTH; ++s, ++ps, ++scr, ++pout) { scramble(&csymbols[*ps], *scr, pout); + } } + return pout - pout0; } + inline void scramble(const complex *src, uint8_t r, complex *dst) { switch (r) @@ -401,7 +472,9 @@ struct s2_frame_transmitter : runnable pipereader> in; pipewriter> out; cstln_lut *cstln; // nullptr initially - complex *csymbols; // Valid iff cstln is valid. RMS cstln_amp. + complex *csymbols; + // Valid iff cstln is valid. RMS cstln_amp. + void update_cstln(const modcod_info *mcinfo) { if (!cstln || cstln->nsymbols != mcinfo->nsymbols) @@ -410,14 +483,27 @@ struct s2_frame_transmitter : runnable { fprintf(stderr, "Warning: Variable MODCOD is inefficient\n"); delete cstln; + } + + if (sch->debug) { + fprintf(stderr, "Building constellation %d\n", mcinfo->nsymbols); + } + + // TBD Different Es/N0 for short frames ? + cstln = new cstln_lut( + mcinfo->c, + mcinfo->esn0_nf, + mcinfo->g1, + mcinfo->g2, + mcinfo->g3 + ); + + if (csymbols) { delete csymbols; } - if (sch->debug) - fprintf(stderr, "Building constellation %d\n", mcinfo->nsymbols); - // TBD Different Es/N0 for short frames ? - cstln = new cstln_lut(mcinfo->c, mcinfo->esn0_nf, - mcinfo->g1, mcinfo->g2, mcinfo->g3); + csymbols = new complex[cstln->nsymbols]; + for (int s = 0; s < cstln->nsymbols; ++s) { csymbols[s].re = cstln->symbols[s].re; @@ -425,6 +511,7 @@ struct s2_frame_transmitter : runnable } } } + complex qsymbols[4]; // RMS cstln_amp s2_sof sof; s2_plscodes plscodes; @@ -509,9 +596,7 @@ struct s2_frame_receiver : runnable // Constellation for PLS qpsk = new cstln_lut(cstln_base::QPSK); add_syncs(qpsk); - init_coarse_freq(); - #if TEST_DIVERSITY fprintf(stderr, "** DEBUG: Diversity test mode (slower)\n"); #endif @@ -520,6 +605,10 @@ struct s2_frame_receiver : runnable ~s2_frame_receiver() { delete qpsk; + + if (cstln) { + delete cstln; + } } void run() @@ -589,6 +678,7 @@ struct s2_frame_receiver : runnable complex *pin = in.rd(); complex p = *pin++; int nsamples = MAX_SYMBOLS_PER_FRAME * omega0; + for (int s = nsamples; s--; ++pin) { complex n = *pin; @@ -596,8 +686,10 @@ struct s2_frame_receiver : runnable diffcorr.im += p.re * n.im - p.im * n.re; p = n; } + in.read(nsamples); ++coarse_count; + if (coarse_count == 50) { float freqw = atan2f(diffcorr.im, diffcorr.re) * omega0; @@ -618,29 +710,34 @@ struct s2_frame_receiver : runnable opt_write(state_out, 0); mu = 0; phase16 = 0; - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "ACQ\n"); + } + state = FRAME_SEARCH; } void run_frame_search() { complex *psampled; - if (cstln_out && cstln_out->writable() >= 1024) + + if (cstln_out && cstln_out->writable() >= 1024) { psampled = cstln_out->wr(); - else + } else { psampled = nullptr; + } // Preserve float precision phase16 -= 65536 * floor(phase16 / 65536); - int nsymbols = MAX_SYMBOLS_PER_FRAME; // TBD Adjust after PLS decoding - sampler_state ss = {in.rd(), mu, phase16, freqw16, nullptr}; sampler->update_freq(ss.fw16 / omega0); - if (!in_power) + if (!in_power) { init_agc(ss.p, 64); + } + update_agc(); for (int s = 0; s < nsymbols; ++s) @@ -650,8 +747,9 @@ struct s2_frame_receiver : runnable complex p = p0 * agc_gain; // Constellation plot - if (psampled && s < 1024) + if (psampled && s < 1024) { *psampled++ = p; + } // Demodulate everything as QPSK. // Occasionally it locks onto 8PSK at offet 2pi/16. @@ -662,22 +760,28 @@ struct s2_frame_receiver : runnable { ps->hist = (ps->hist << 1) | ((ps->tobpsk >> symb) & 1); int errors = hamming_weight((ps->hist & sof.MASK) ^ sof.VALUE); + if (errors <= S2_MAX_ERR_SOF_INITIAL) { - if (sch->debug2) - fprintf(stderr, "Found SOF+%d at %d offset %f\n", - errors, s, ps->offset16); + if (sch->debug2) { + fprintf(stderr, "Found SOF+%d at %d offset %f\n", errors, s, ps->offset16); + } + ss.ph16 += ps->offset16; in.read(ss.p - in.rd()); mu = ss.mu; phase16 = ss.ph16; freqw16 = ss.fw16; - if (psampled) + + if (psampled) { cstln_out->written(psampled - cstln_out->wr()); + } + enter_frame_locked(); return; } } + ss.normalize(); } @@ -686,16 +790,21 @@ struct s2_frame_receiver : runnable mu = ss.mu; phase16 = ss.ph16; freqw16 = ss.fw16; - if (psampled) + + if (psampled) { cstln_out->written(psampled - cstln_out->wr()); + } } // State transtion void enter_frame_locked() { opt_write(state_out, 1); - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "LOCKED\n"); + } + state = FRAME_LOCKED; } @@ -708,12 +817,14 @@ struct s2_frame_receiver : runnable float ph16; // Carrier phase at next symbol, cycles*65536 float fw16; // Carrier frequency, cycles per symbol * 65536 uint8_t *scr; // Position in scrambling sequeence + void skip_symbols(int ns, float omega0) { mu += omega0 * ns; ph16 += fw16 * ns; scr += ns; } + void normalize() { ph16 = fmodf(ph16, 65536.0f); // Rounding direction irrelevant @@ -728,15 +839,19 @@ struct s2_frame_receiver : runnable void run_frame_locked() { complex *psampled; - if (cstln_out && cstln_out->writable() >= 1024) + if (cstln_out && cstln_out->writable() >= 1024) { psampled = cstln_out->wr(); - else + } else { psampled = nullptr; + } + complex *psampled_pls; - if (cstln_pls_out && cstln_pls_out->writable() >= 1024) + + if (cstln_pls_out && cstln_pls_out->writable() >= 1024) { psampled_pls = cstln_pls_out->wr(); - else + } else { psampled_pls = nullptr; + } #if TEST_DIVERSITY complex *psymbols = symbols_out ? symbols_out->wr() : nullptr; @@ -752,9 +867,9 @@ struct s2_frame_receiver : runnable update_agc(); // Read PLSCODE - uint64_t plscode = 0; complex pls_symbols[s2_plscodes::LENGTH]; + for (int s = 0; s < plscodes.LENGTH; ++s) { complex p = interp_next(&ss) * agc_gain; @@ -763,35 +878,43 @@ struct s2_frame_receiver : runnable *psymbols++ = p * scale_symbols; #endif pls_symbols[s] = p; - if (psampled_pls) + + if (psampled_pls) { *psampled_pls++ = p; + } + int bit = (p.im < 1); // TBD suboptimal plscode = (plscode << 1) | bit; } + int pls_index = -1; int pls_errors = S2_MAX_ERR_PLSCODE + 1; // dmin=32 // TBD: Optimiser + for (int i = 0; i < plscodes.COUNT; ++i) { int e = hamming_weight(plscode ^ plscodes.codewords[i]); + if (e < pls_errors) { pls_errors = e; pls_index = i; } } + if (pls_index < 0) { - if (sch->debug2) + if (sch->debug2) { fprintf(stderr, "Too many errors in plheader (%d)\n", pls_errors); + } + in.read(ss.p - in.rd()); enter_frame_search(); return; } // Adjust phase with PLS - complex pls_corr = conjprod(plscodes.symbols[pls_index], - pls_symbols, plscodes.LENGTH); + complex pls_corr = conjprod(plscodes.symbols[pls_index], pls_symbols, plscodes.LENGTH); ss.normalize(); align_phase(&ss, pls_corr); @@ -802,6 +925,7 @@ struct s2_frame_receiver : runnable xfprintf(stderr, "PLS: modcod %d, short=%d, pilots=%d (%d errors)\n", pls.modcod, pls.sf, pls.pilots, pls_errors); const modcod_info *mcinfo = &modcod_infos[pls.modcod]; + if (!mcinfo->nslots_nf) { fprintf(stderr, "Unsupported or corrupted MODCOD\n"); @@ -822,6 +946,7 @@ struct s2_frame_receiver : runnable if (mcinfo->c != m_modcodType) { m_modcodType = mcinfo->c; } + if (mcinfo->rate != m_modcodRate) { m_modcodRate = mcinfo->rate; } @@ -834,6 +959,7 @@ struct s2_frame_receiver : runnable fprintf(stderr, "Warning: Variable MODCOD is inefficient\n"); delete cstln; } + fprintf(stderr, "Creating LUT for %s ratecode %d\n", cstln_base::names[mcinfo->c], mcinfo->rate); cstln = new cstln_lut(mcinfo->c, mcinfo->esn0_nf, @@ -870,6 +996,7 @@ struct s2_frame_receiver : runnable // Read pilot int errors = 0; complex corr = 0; + for (int s = 0; s < pilot_length; ++s) { complex p0 = interp_next(&ss); @@ -880,18 +1007,27 @@ struct s2_frame_receiver : runnable *psymbols++ = p * scale_symbols; #endif (void)track_symbol(&ss, p, qpsk, 1); - if (psampled_pls) + + if (psampled_pls) { *psampled_pls++ = p; + } + complex d = descramble(&ss, p); - if (d.im < 0 || d.re < 0) + + if (d.im < 0 || d.re < 0) { ++errors; + } + corr.re += d.re + d.im; corr.im += d.im - d.re; } + if (errors > S2_MAX_ERR_PILOT) { - if (sch->debug2) + if (sch->debug2) { fprintf(stderr, "Too many errors in pilot (%d/36)\n", errors); + } + in.read(ss.p - in.rd()); enter_frame_search(); return; @@ -901,9 +1037,11 @@ struct s2_frame_receiver : runnable align_phase(&ss, corr); till_next_pls = 16; } + // Read slot pout->is_pls = false; complex p; // Export last symbols for cstln_out + for (int s = 0; s < pout->LENGTH; ++s) { p = interp_next(&ss) * agc_gain; @@ -922,8 +1060,10 @@ struct s2_frame_receiver : runnable #endif pout->symbols[s] = *symb; } - if (psampled) + + if (psampled) { *psampled++ = p; + } } // slots // Read SOF @@ -936,6 +1076,7 @@ struct s2_frame_receiver : runnable complex sof_corr = 0; uint32_t sofbits = 0; + for (int s = 0; s < sof.LENGTH; ++s) { complex p0 = interp_next(&ss); @@ -945,21 +1086,28 @@ struct s2_frame_receiver : runnable if (psymbols) *psymbols++ = p * scale_symbols; #endif - if (psampled_pls) + if (psampled_pls) { *psampled_pls++ = p; + } + int bit = (p.im < 0); // suboptimal sofbits = (sofbits << 1) | bit; sof_corr += conjprod(sof.symbols[s], p); } + int sof_errors = hamming_weight(sofbits ^ sof.VALUE); + if (sof_errors >= S2_MAX_ERR_SOF) { - if (sch->debug2) + if (sch->debug2) { fprintf(stderr, "Too many errors in SOF (%d/26)\n", sof_errors); + } + in.read(ss.p - in.rd()); enter_coarse_freq(); return; } + ss.normalize(); align_phase(&ss, sof_corr); @@ -974,13 +1122,17 @@ struct s2_frame_receiver : runnable freqw16 = ss.fw16; // Measurements - if (psampled) + if (psampled) { cstln_out->written(psampled - cstln_out->wr()); - if (psampled_pls) + } + + if (psampled_pls) { cstln_pls_out->written(psampled_pls - cstln_pls_out->wr()); + } #if TEST_DIVERSITY - if (psymbols) + if (psymbols) { symbols_out->written(psymbols - symbols_out->wr()); + } #endif if (meas_count >= meas_decimation) { @@ -994,8 +1146,10 @@ struct s2_frame_receiver : runnable int all_errors = pls_errors + pilot_errors + sof_errors; int max_errors = plscodes.LENGTH + sof.LENGTH; - if (pls.pilots) + + if (pls.pilots) { max_errors += ((S - 1) / 16) * pilot_length; + } xfprintf(stderr, "success fw= %f (%.0f Hz) mu= %f " "errors=%d/64+%d+%d/26 = %2d/%d\n", @@ -1013,10 +1167,14 @@ struct s2_frame_receiver : runnable void init_agc(const complex *buf, int n) { in_power = 0; - for (int i = 0; i < n; ++i) + + for (int i = 0; i < n; ++i) { in_power += cnorm2(buf[i]); + } + in_power /= n; } + void track_agc(const complex &p) { float in_p = p.re * p.re + p.im * p.im; @@ -1026,8 +1184,11 @@ struct s2_frame_receiver : runnable void update_agc() { float in_amp = gen_sqrt(in_power); - if (!in_amp) + + if (!in_amp) { return; + } + if (!strongpls || !cstln) { // Match RMS amplitude @@ -1044,6 +1205,7 @@ struct s2_frame_receiver : runnable { int r = *ss->scr++; complex res; + switch (r) { case 3: @@ -1061,6 +1223,7 @@ struct s2_frame_receiver : runnable default: res = p; } + return res; } @@ -1102,8 +1265,10 @@ struct s2_frame_receiver : runnable float err = atan2f(c.im,c.re) * (65536/(2*M_PI)); #else // Same performance as atan2f, faster - if (!c.re) + if (!c.re) { return; + } + float err = c.im / c.re * (65536 / (2 * M_PI)); #endif ss->ph16 += err; @@ -1114,19 +1279,38 @@ struct s2_frame_receiver : runnable { static struct { - float kph, kfw, kmu; - } gains[2] = { - {4e-2, 1e-4, (float) 0.001 / (cstln_amp * cstln_amp)}, - {4e-2, 1e-4, (float) 0.001 / (cstln_amp * cstln_amp)}}; + float kph; + float kfw; + float kmu; + } + gains[2] = + { + { + 4e-2, + 1e-4, + (float) 0.001 / (cstln_amp * cstln_amp) + }, + { + 4e-2, + 1e-4, + (float) 0.001 / (cstln_amp * cstln_amp) + } + }; + // Decision typename cstln_lut::result *cr = c->lookup(p.re, p.im); // Carrier tracking ss->ph16 += cr->phase_error * gains[mode].kph; ss->fw16 += cr->phase_error * gains[mode].kfw; - if (ss->fw16 < min_freqw16) + + if (ss->fw16 < min_freqw16) { ss->fw16 = min_freqw16; - if (ss->fw16 > max_freqw16) + } + + if (ss->fw16 > max_freqw16) { ss->fw16 = max_freqw16; + } + // Phase tracking hist[2] = hist[1]; hist[1] = hist[0]; @@ -1141,11 +1325,16 @@ struct s2_frame_receiver : runnable (hist[0].c.im - hist[2].c.im) * hist[1].p.im); float mucorr = muerr * gains[mode].kmu; const float max_mucorr = 0.1; + // TBD Optimize out statically - if (mucorr < -max_mucorr) + if (mucorr < -max_mucorr) { mucorr = -max_mucorr; - if (mucorr > max_mucorr) + } + + if (mucorr > max_mucorr) { mucorr = max_mucorr; + } + ss->mu += mucorr; // Error vector for MER complex ev(p.re - cp->re, p.im - cp->im); @@ -1166,6 +1355,7 @@ struct s2_frame_receiver : runnable float agc_bw; cstln_lut *qpsk; static const int MAXSYNCS = 8; + struct sync { uint16_t nsmask; // bitmask of cstln orders for which this sync is used @@ -1173,10 +1363,12 @@ struct s2_frame_receiver : runnable float offset16; // Phase offset 0..65536 uint32_t hist; // For SOF detection } syncs[MAXSYNCS], *current_sync; + int nsyncs; s2_plscodes plscodes; cstln_lut *cstln; // Initialize synchronizers for an arbitrary constellation. + void add_syncs(cstln_lut *c) { int random_decision = 0; @@ -1189,34 +1381,43 @@ struct s2_frame_receiver : runnable #endif for (int r = 0; r < nrot; ++r) { - if (nsyncs == MAXSYNCS) + if (nsyncs == MAXSYNCS) { fail("Bug: too many syncs"); + } + sync *s = &syncs[nsyncs++]; s->offset16 = 65536.0 * r / nrot; float angle = -2 * M_PI * r / nrot; s->tobpsk = 0; + for (int i = c->nsymbols; i--;) { complex p = c->symbols[i]; float im = p.re * sinf(angle) + p.im * cosf(angle); int bit; + if (im > 1) + { bit = 0; + } else if (im < -1) + { bit = 1; + } else { bit = random_decision; random_decision ^= 1; } // Near 0 + s->tobpsk = (s->tobpsk << 1) | bit; } + s->hist = 0; } } trig16 trig; - modcod_info *mcinfo; pipereader> in; pipewriter> out; int meas_count; @@ -1260,6 +1461,7 @@ struct s2_interleaver : runnable out(_out, 1 + 360) { } + void run() { while (in.readable() >= 1) @@ -1267,8 +1469,11 @@ struct s2_interleaver : runnable const s2_pls *pls = &in.rd()->pls; const modcod_info *mcinfo = check_modcod(pls->modcod); int nslots = pls->sf ? mcinfo->nslots_nf / 4 : mcinfo->nslots_nf; - if (out.writable() < 1 + nslots) + + if (out.writable() < 1 + nslots) { return; + } + const hard_sb *pbytes = in.rd()->bytes; // Output pseudo slot with PLS. plslot *ppls = out.wr(); @@ -1277,17 +1482,22 @@ struct s2_interleaver : runnable out.written(1); // Interleave plslot *pout = out.wr(); + if (mcinfo->nsymbols == 4) + { serialize_qpsk(pbytes, nslots, pout); + } else { int bps = log2(mcinfo->nsymbols); int rows = pls->framebits() / bps; - if (mcinfo->nsymbols == 8 && mcinfo->rate == FEC35) + if (mcinfo->nsymbols == 8 && mcinfo->rate == FEC35) { interleave(bps, rows, pbytes, nslots, false, pout); - else + } else { interleave(bps, rows, pbytes, nslots, true, pout); + } } + in.read(1); out.written(nslots); } @@ -1313,8 +1523,10 @@ struct s2_interleaver : runnable } if ( nacc ) fail("Bug: s2_interleaver"); #else - if (nslots % 2) + if (nslots % 2) { fatal("Bug: Truncated byte"); + } + for (; nslots; nslots -= 2) { hard_sb b; @@ -1322,6 +1534,7 @@ struct s2_interleaver : runnable // Slot 0 (mod 2) pout->is_pls = false; ps = pout->symbols; + for (int i = 0; i < 22; ++i) { b = *pin++; @@ -1330,6 +1543,7 @@ struct s2_interleaver : runnable *ps++ = (b >> 2) & 3; *ps++ = (b)&3; } + b = *pin++; *ps++ = (b >> 6); *ps++ = (b >> 4) & 3; @@ -1339,6 +1553,7 @@ struct s2_interleaver : runnable ps = pout->symbols; *ps++ = (b >> 2) & 3; *ps++ = (b)&3; + for (int i = 0; i < 22; ++i) { b = *pin++; @@ -1347,6 +1562,7 @@ struct s2_interleaver : runnable *ps++ = (b >> 2) & 3; *ps++ = (b)&3; } + ++pout; } #endif @@ -1398,7 +1614,9 @@ struct s2_interleaver : runnable { void (*func)(int rows, const hard_sb *pin, int nslots, plslot *pout) = 0; + if (msb_first) + { switch (bps) { case 2: @@ -1416,7 +1634,9 @@ struct s2_interleaver : runnable default: fail("Bad bps"); } + } else + { switch (bps) { case 2: @@ -1434,19 +1654,29 @@ struct s2_interleaver : runnable default: fail("Bad bps"); } + } + (*func)(rows, pin, nslots, pout); } + template static void interleave(int rows, const hard_sb *pin, int nslots, plslot *pout) { - if (BPS == 4 && rows == 4050 && MSB_FIRST) + if (BPS == 4 && rows == 4050 && MSB_FIRST) { return interleave4050(pin, nslots, pout); - if (rows % 8) + } + + if (rows % 8) { fatal("modcod/framesize combination not supported\n"); + } + int stride = rows / 8; // Offset to next column, in bytes - if (nslots % 4) + + if (nslots % 4) { fatal("Bug: Truncated byte"); + } + // plslot::symbols[] are not packed across slots, // so we need tos split bytes at boundaries. for (; nslots; nslots -= 4) @@ -1456,11 +1686,13 @@ struct s2_interleaver : runnable // Slot 0 (mod 4): 88+2 pout->is_pls = false; ps = pout->symbols; + for (int i = 0; i < 11; ++i) { split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 8); } + split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 2); ++pout; @@ -1468,11 +1700,13 @@ struct s2_interleaver : runnable pout->is_pls = false; ps = pout->symbols; pop_symbols(accs, &ps, 6); + for (int i = 0; i < 10; ++i) { split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 8); } + split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 4); ++pout; @@ -1480,11 +1714,13 @@ struct s2_interleaver : runnable pout->is_pls = false; ps = pout->symbols; pop_symbols(accs, &ps, 4); + for (int i = 0; i < 10; ++i) { split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 8); } + split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 6); ++pout; @@ -1492,22 +1728,27 @@ struct s2_interleaver : runnable pout->is_pls = false; ps = pout->symbols; pop_symbols(accs, &ps, 2); + for (int i = 0; i < 11; ++i) { split_byte(pin++, stride, accs); pop_symbols(accs, &ps, 8); } + ++pout; } } + template static inline void split_byte(const hard_sb *pi, int stride, hard_sb accs[BPS]) { // TBD Pass stride as template parameter. - for (int b = 0; b < BPS; ++b, pi += stride) + for (int b = 0; b < BPS; ++b, pi += stride) { accs[b] = *pi; + } } + template static void pop_symbols(hard_sb accs[BPS], hard_ss **ps, int ns) { @@ -1516,12 +1757,18 @@ struct s2_interleaver : runnable hard_ss symb = 0; // Check unrolling and constant propagation. for (int b = 0; b < BPS; ++b) - if (MSB_FIRST) + { + if (MSB_FIRST) { symb = (symb << 1) | (accs[b] >> 7); - else + } else { symb = (symb << 1) | (accs[BPS - 1 - b] >> 7); - for (int b = 0; b < BPS; ++b) + } + } + + for (int b = 0; b < BPS; ++b) { accs[b] <<= 1; + } + *(*ps)++ = symb; } } @@ -1534,10 +1781,12 @@ struct s2_interleaver : runnable { hard_sb accs[4]; // One accumulator per column int nacc = 0; // Bits in each column accumulator + for (; nslots; --nslots, ++pout) { pout->is_pls = false; hard_ss *ps = pout->symbols; + for (int ns = pout->LENGTH; ns--; ++ps) { if (!nacc) @@ -1557,15 +1806,19 @@ struct s2_interleaver : runnable accs[2] = (pin[1012] << 4) | (pin[1013] >> 4); accs[3] = (pin[1518] << 6) | (pin[1519] >> 2); } + ++pin; nacc = 8; } + hard_ss symb = 0; + for (int b = 0; b < 4; ++b) { symb = (symb << 1) | (accs[b] >> 7); accs[b] <<= 1; } + --nacc; *ps = symb; } @@ -1598,27 +1851,39 @@ struct s2_deinterleaver : runnable while (in.readable() >= 1 && out.writable() >= 1) { plslot *pin = in.rd(); - if (!pin->is_pls) + + if (!pin->is_pls) { fail("s2_deinterleaver: bad input sequence"); + } + s2_pls *pls = &pin->pls; const modcod_info *mcinfo = check_modcod(pls->modcod); int nslots = pls->sf ? mcinfo->nslots_nf / 4 : mcinfo->nslots_nf; - if (in.readable() < 1 + nslots) + + if (in.readable() < 1 + nslots) { return; + } + fecframe *pout = out.wr(); pout->pls = *pls; SOFTBYTE *pbytes = pout->bytes; + if (mcinfo->nsymbols == 4) + { deserialize_qpsk(pin + 1, nslots, pbytes); + } else { int bps = log2(mcinfo->nsymbols); int rows = pls->framebits() / bps; - if (mcinfo->nsymbols == 8 && mcinfo->rate == FEC35) + + if (mcinfo->nsymbols == 8 && mcinfo->rate == FEC35) { deinterleave(bps, rows, pin + 1, nslots, false, pbytes); - else + } else { deinterleave(bps, rows, pin + 1, nslots, true, pbytes); + } } + in.read(1 + nslots); out.written(1); } @@ -1632,13 +1897,16 @@ struct s2_deinterleaver : runnable SOFTBYTE acc; softword_clear(&acc); // gcc warning int nacc = 0; + for (; nslots; --nslots, ++pin) { SOFTSYMB *ps = pin->symbols; + for (int ns = pin->LENGTH; ns--; ++ps) { pack_qpsk_symbol(*ps, &acc, nacc); nacc += 2; + if (nacc == 8) { // TBD unroll *pout++ = acc; @@ -1683,7 +1951,9 @@ struct s2_deinterleaver : runnable { void (*func)(int rows, const plslot *pin, int nslots, SOFTBYTE *pout) = 0; + if (msb_first) + { switch (bps) { case 2: @@ -1701,7 +1971,9 @@ struct s2_deinterleaver : runnable default: fail("Bad bps"); } + } else + { switch (bps) { case 2: @@ -1719,6 +1991,8 @@ struct s2_deinterleaver : runnable default: fail("Bad bps"); } + } + (*func)(rows, pin, nslots, pout); } @@ -1726,35 +2000,50 @@ struct s2_deinterleaver : runnable static void deinterleave(int rows, const plslot *pin, int nslots, SOFTBYTE *pout) { - if (BPS == 4 && rows == 4050 && MSB_FIRST) + if (BPS == 4 && rows == 4050 && MSB_FIRST) { return deinterleave4050(pin, nslots, pout); - if (rows % 8) + } + + if (rows % 8) { fatal("modcod/framesize combination not supported\n"); + } + int stride = rows / 8; // Offset to next column, in bytes SOFTBYTE accs[BPS]; - for (int b = 0; b < BPS; ++b) + + for (int b = 0; b < BPS; ++b) { softword_clear(&accs[b]); // gcc warning + } + int nacc = 0; + for (; nslots; --nslots, ++pin) { const SOFTSYMB *ps = pin->symbols; + for (int ns = pin->LENGTH; ns--; ++ps) { split_symbol(*ps, BPS, accs, nacc, MSB_FIRST); ++nacc; + if (nacc == 8) { // TBD Unroll, same as interleave() SOFTBYTE *po = pout; + // TBD Pass stride as template parameter. - for (int b = 0; b < BPS; ++b, po += stride) + for (int b = 0; b < BPS; ++b, po += stride) { *po = accs[b]; + } + ++pout; nacc = 0; } } } - if (nacc) + + if (nacc) { fail("Bug: s2_deinterleaver"); + } } #endif // reference @@ -1766,16 +2055,22 @@ struct s2_deinterleaver : runnable { const int rows = 4050; SOFTBYTE accs[4]; - for (int b = 0; b < 4; ++b) + + for (int b = 0; b < 4; ++b) { softword_clear(&accs[b]); // gcc warning + } + int nacc = 0; + for (; nslots; --nslots, ++pin) { const SOFTSYMB *ps = pin->symbols; + for (int ns = pin->LENGTH; ns--; ++ps) { split_symbol(*ps, 4, accs, nacc, true); ++nacc; + if (nacc == 8) { for (int b = 0; b < 8; ++b) @@ -1785,16 +2080,22 @@ struct s2_deinterleaver : runnable softwords_set(pout, rows * 2 + b, softword_get(accs[2], b)); softwords_set(pout, rows * 3 + b, softword_get(accs[3], b)); } + ++pout; nacc = 0; } } } - if (nacc != 2) + + if (nacc != 2) { fatal("Bug: Expected 2 leftover rows\n"); + } + // Pad with random symbol so that we can use accs[]. - for (int b = nacc; b < 8; ++b) + for (int b = nacc; b < 8; ++b) { split_symbol(pin->symbols[0], 4, accs, b, true); + } + for (int b = 0; b < nacc; ++b) { softwords_set(pout, rows * 0 + b, softword_get(accs[0], b)); @@ -1811,17 +2112,21 @@ struct s2_deinterleaver : runnable bool msb_first) { (void) nacc; + if (msb_first) { - for (int b = 0; b < bps; ++b) + for (int b = 0; b < bps; ++b) { accs[b] = (accs[b] << 1) | llr_harden(ps.bits[bps - 1 - b]); + } } else { - for (int b = 0; b < bps; ++b) + for (int b = 0; b < bps; ++b) { accs[b] = (accs[b] << 1) | llr_harden(ps.bits[b]); + } } } + // Fast variant template static inline void split_symbol(const llr_ss &ps, @@ -1829,52 +2134,68 @@ struct s2_deinterleaver : runnable { if (MSB_FIRST) { - for (int b = 0; b < BPS; ++b) + for (int b = 0; b < BPS; ++b) { accs[b] = (accs[b] << 1) | llr_harden(ps.bits[BPS - 1 - b]); + } } else { - for (int b = 0; b < BPS; ++b) + for (int b = 0; b < BPS; ++b) { accs[b] = (accs[b] << 1) | llr_harden(ps.bits[b]); + } } } // Spread LLR symbol across LLR columns. - static inline void split_symbol(const llr_ss &ps, int bps, - llr_sb accs[/*bps*/], int nacc, - bool msb_first) + static inline void split_symbol( + const llr_ss &ps, + int bps, + llr_sb accs[/*bps*/], + int nacc, + bool msb_first + ) { if (msb_first) { - for (int b = 0; b < bps; ++b) + for (int b = 0; b < bps; ++b) { accs[b].bits[nacc] = ps.bits[bps - 1 - b]; + } } else { - for (int b = 0; b < bps; ++b) + for (int b = 0; b < bps; ++b) { accs[b].bits[nacc] = ps.bits[b]; + } } } + // Fast variant template - static inline void split_symbol(const llr_ss &ps, - llr_sb accs[/*bps*/], int nacc) + static inline void split_symbol( + const llr_ss &ps, + llr_sb accs[/*bps*/], + int nacc) { if (MSB_FIRST) { - for (int b = 0; b < BPS; ++b) + for (int b = 0; b < BPS; ++b) { accs[b].bits[nacc] = ps.bits[BPS - 1 - b]; + } } else { - for (int b = 0; b < BPS; ++b) + for (int b = 0; b < BPS; ++b) { accs[b].bits[nacc] = ps.bits[b]; + } } } // Merge QPSK LLR symbol into hard byte. - static inline void pack_qpsk_symbol(const llr_ss &ps, - hard_sb *acc, int nacc) + static inline void pack_qpsk_symbol( + const llr_ss &ps, + hard_sb *acc, + int nacc + ) { (void) nacc; // TBD Must match LLR law, see softsymb_harden. @@ -1883,8 +2204,11 @@ struct s2_deinterleaver : runnable } // Merge QPSK LLR symbol into LLR byte. - static inline void pack_qpsk_symbol(const llr_ss &ps, - llr_sb *acc, int nacc) + static inline void pack_qpsk_symbol( + const llr_ss &ps, + llr_sb *acc, + int nacc + ) { acc->bits[nacc] = ps.bits[1]; acc->bits[nacc + 1] = ps.bits[0]; @@ -1906,7 +2230,9 @@ static const struct fec_info int kldpc; // LDPC message size (= BCH codeword size) (bits) int t; // BCH error correction const s2_ldpc_table *ldpc; -} fec_infos[2][FEC_COUNT] = { +} +fec_infos[2][FEC_COUNT] = +{ { // Normal frames - must respect enum code_rate order {32208, 32400, 12, &ldpc_nf_fec12}, // FEC12 (was [FEC12] = {...} and so on. Does not compile with MSVC) @@ -1955,14 +2281,17 @@ struct s2_ldpc_engines { typedef ldpc_engine s2_ldpc_engine; s2_ldpc_engine *ldpcs[2][FEC_COUNT]; // [shortframes][fec] + s2_ldpc_engines() { - memset(ldpcs, 0, sizeof(ldpcs)); + // memset(ldpcs, 0, sizeof(ldpcs)); // useless as initialization comes next + for (int sf = 0; sf <= 1; ++sf) { for (int fec = 0; fec < FEC_COUNT; ++fec) { const fec_info *fi = &fec_infos[sf][fec]; + if (!fi->ldpc) { ldpcs[sf][fec] = nullptr; @@ -1976,15 +2305,33 @@ struct s2_ldpc_engines } } } + + ~s2_ldpc_engines() + { + for (int sf = 0; sf <= 1; ++sf) + { + for (int fec = 0; fec < FEC_COUNT; ++fec) + { + if (ldpcs[sf][fec]) { + delete ldpcs[sf][fec]; + } + } + } + } + void print_node_stats() { for (int sf = 0; sf <= 1; ++sf) + { for (int fec = 0; fec < FEC_COUNT; ++fec) { s2_ldpc_engine *ldpc = ldpcs[sf][fec]; - if (ldpc) + + if (ldpc) { ldpc->print_node_stats(); + } } + } } }; // s2_ldpc_engines @@ -2002,9 +2349,11 @@ struct s2_bch_engines typedef bch_engine s2_bch_engine_nf8; // Short frames with 12 polynomials. typedef bch_engine s2_bch_engine_sf12; + s2_bch_engines() { bitvect bch_polys[2][12]; // [shortframes][polyindex] + // EN 302 307-1 5.3.1 Table 6a (polynomials for normal frames) bch_polys[0][0] = bitvect(0x1002d); // g1 bch_polys[0][1] = bitvect(0x10173); // g2 @@ -2018,6 +2367,7 @@ struct s2_bch_engines bch_polys[0][9] = bitvect(0x175a7); // g10 bch_polys[0][10] = bitvect(0x13a2d); // g11 bch_polys[0][11] = bitvect(0x11ae3); // g12 + // EN 302 307-1 5.3.1 Table 6b (polynomials for short frames) bch_polys[1][0] = bitvect(0x402b); // g1 bch_polys[1][1] = bitvect(0x4941); // g2 @@ -2031,8 +2381,16 @@ struct s2_bch_engines bch_polys[1][9] = bitvect(0x5a49); // g10 bch_polys[1][10] = bitvect(0x5811); // g11 bch_polys[1][11] = bitvect(0x65ef); // g12 + // Redundant with fec_infos[], but needs static template argument. - memset(bchs, 0, sizeof(bchs)); + + for (int sf = 0; sf <= 1; ++sf) + { + for (int fec = 0; fec < FEC_COUNT; ++fec) { + bchs[sf][fec] = nullptr; + } + } + bchs[0][FEC12] = new s2_bch_engine_nf12(bch_polys[0], 12); bchs[0][FEC23] = new s2_bch_engine_nf10(bch_polys[0], 10); bchs[0][FEC34] = new s2_bch_engine_nf12(bch_polys[0], 12); @@ -2044,6 +2402,7 @@ struct s2_bch_engines bchs[0][FEC13] = new s2_bch_engine_nf12(bch_polys[0], 12); bchs[0][FEC25] = new s2_bch_engine_nf12(bch_polys[0], 12); bchs[0][FEC35] = new s2_bch_engine_nf12(bch_polys[0], 12); + bchs[1][FEC12] = new s2_bch_engine_sf12(bch_polys[1], 12); bchs[1][FEC23] = new s2_bch_engine_sf12(bch_polys[1], 12); bchs[1][FEC34] = new s2_bch_engine_sf12(bch_polys[1], 12); @@ -2055,6 +2414,19 @@ struct s2_bch_engines bchs[1][FEC25] = new s2_bch_engine_sf12(bch_polys[1], 12); bchs[1][FEC35] = new s2_bch_engine_sf12(bch_polys[1], 12); } + + ~s2_bch_engines() + { + for (int sf = 0; sf <= 1; ++sf) + { + for (int fec = 0; fec < FEC_COUNT; ++fec) + { + if (bchs[sf][fec]) { + delete bchs[sf][fec]; + } + } + } + } }; // s2_bch_engines // S2 BASEBAND DESCRAMBLER AND FEC ENCODER @@ -2064,6 +2436,7 @@ struct s2_bch_engines struct s2_fecenc : runnable { typedef ldpc_engine s2_ldpc_engine; + s2_fecenc( scheduler *sch, pipebuf &_in, @@ -2073,9 +2446,11 @@ struct s2_fecenc : runnable in(_in), out(_out) { - if (sch->debug) + if (sch->debug) { s2ldpc.print_node_stats(); + } } + void run() { while (in.readable() >= 1 && out.writable() >= 1) @@ -2094,11 +2469,13 @@ struct s2_fecenc : runnable pout->pls = pin->pls; hard_sb *pbytes = pout->bytes; bbscrambling.transform(pin->bytes, fi->Kbch / 8, pbytes); + { // BCH size_t msgbytes = fi->Kbch / 8; bch_interface *bch = s2bch.bchs[pin->pls.sf][mcinfo->rate]; bch->encode(pbytes, msgbytes, pbytes + msgbytes); } + { // LDPC size_t msgbits = fi->kldpc; size_t cwbits = pin->pls.framebits(); @@ -2106,6 +2483,7 @@ struct s2_fecenc : runnable ldpc->encode(fi->ldpc, pbytes, msgbits, cwbits, pbytes + msgbits / 8); } } + pipereader in; pipewriter> out; s2_bbscrambling bbscrambling; @@ -2121,6 +2499,7 @@ template struct s2_fecdec : runnable { int bitflips; + s2_fecdec( scheduler *sch, pipebuf> &_in, pipebuf &_out, @@ -2134,9 +2513,11 @@ struct s2_fecdec : runnable bitcount(opt_writer(_bitcount, 1)), errcount(opt_writer(_errcount, 1)) { - if (sch->debug) + if (sch->debug) { s2ldpc.print_node_stats(); + } } + void run() { while (in.readable() >= 1 && out.writable() >= 1 && @@ -2147,6 +2528,7 @@ struct s2_fecdec : runnable const fec_info *fi = &fec_infos[pin->pls.sf][mcinfo->rate]; bool corrupted = false; bool residual_errors; + if (true) { // LDPC decode @@ -2154,10 +2536,14 @@ struct s2_fecdec : runnable size_t msgbits = fi->kldpc; s2_ldpc_engine *ldpc = s2ldpc.ldpcs[pin->pls.sf][mcinfo->rate]; int ncorr = ldpc->decode_bitflip(fi->ldpc, pin->bytes, msgbits, cwbits, bitflips); - if (sch->debug2) + + if (sch->debug2) { fprintf(stderr, "LDPCCORR = %d\n", ncorr); + } } + uint8_t *hardbytes = softbytes_harden(pin->bytes, fi->kldpc / 8, bch_buf); + if (true) { // BCH decode @@ -2165,14 +2551,19 @@ struct s2_fecdec : runnable // Decode with suitable BCH decoder for this MODCOD bch_interface *bch = s2bch.bchs[pin->pls.sf][mcinfo->rate]; int ncorr = bch->decode(hardbytes, cwbytes); - if (sch->debug2) + + if (sch->debug2) { fprintf(stderr, "BCHCORR = %d\n", ncorr); + } + corrupted = (ncorr < 0); residual_errors = (ncorr != 0); + // Report VER opt_write(bitcount, fi->Kbch); opt_write(errcount, (ncorr >= 0) ? ncorr : fi->Kbch); } + int bbsize = fi->Kbch / 8; // TBD Some decoders want the bad packets. #if 0 @@ -2189,8 +2580,11 @@ struct s2_fecdec : runnable bbscrambling.transform(hardbytes, bbsize, pout->bytes); out.written(1); } - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "%c", corrupted ? ':' : residual_errors ? '.' : '_'); + } + in.read(1); } } @@ -2252,6 +2646,9 @@ struct s2_fecdec_soft : runnable ~s2_fecdec_soft() { + if (ldpc) { + delete ldpc; + } if (aligned_buffer) { free(aligned_buffer); } @@ -2294,8 +2691,11 @@ struct s2_fecdec_soft : runnable // Decode with suitable BCH decoder for this MODCOD bch_interface *bch = s2bch.bchs[pin->pls.sf][mcinfo->rate]; int ncorr = bch->decode(hardbytes, cwbytes); - if (sch->debug2) + + if (sch->debug2) { fprintf(stderr, "BCHCORR = %d\n", ncorr); + } + corrupted = (ncorr < 0); residual_errors = (ncorr != 0); // Report VER @@ -2356,8 +2756,14 @@ template struct simplequeue { static const int SIZE = _SIZE; - simplequeue() { rd = wr = count = 0; } + + simplequeue() + { + rd = wr = count = 0; + } + bool full() { return count == SIZE; } + T *put() { T *res = &q[wr]; @@ -2365,8 +2771,10 @@ struct simplequeue ++count; return res; } + bool empty() { return count == 0; } const T *peek() { return &q[rd]; } + const T *get() { const T *res = &q[rd]; @@ -2374,7 +2782,8 @@ struct simplequeue --count; return res; } - // private: + +private: int rd, wr, count; T q[SIZE]; }; @@ -2406,15 +2815,18 @@ struct s2_fecdec_helper : runnable errcount(opt_writer(_errcount, 1)) { command = strdup(_command); - for (int mc = 0; mc < 32; ++mc) - for (int sf = 0; sf < 2; ++sf) + + for (int mc = 0; mc < 32; ++mc) { + for (int sf = 0; sf < 2; ++sf) { pools[mc][sf].procs = nullptr; + } + } } ~s2_fecdec_helper() { free(command); - killall(); + killall(); // also deletes pools[mc][sf].procs if necessary } void run() @@ -2496,10 +2908,11 @@ struct s2_fecdec_helper : runnable continue; // next worker } - if (nw < 0) + if (nw < 0) { fatal("write(LDPC helper"); - else if (nw != iosize) + } else if (nw != iosize) { fatal("partial write(LDPC helper)"); + } p->shift = i; helper_job *job = jobs.put(); @@ -2531,8 +2944,11 @@ struct s2_fecdec_helper : runnable fprintf(stderr, "s2_fecdec_helper::get_pool: allocate %d workers: modcod=%d sf=%d\n", nhelpers, pls->modcod, pls->sf); p->procs = new helper_instance[nhelpers]; - for (int i = 0; i < nhelpers; ++i) + + for (int i = 0; i < nhelpers; ++i) { spawn_helper(&p->procs[i], pls); + } + p->nprocs = nhelpers; p->shift = 0; } @@ -2557,12 +2973,17 @@ struct s2_fecdec_helper : runnable helper_instance *h = &p->procs[i]; fprintf(stderr, "s2_fecdec_helper::killall: killing %d\n", h->pid); int rc = kill(h->pid, SIGKILL); - if (rc < 0) { + + if (rc < 0) + { fatal("s2_fecdec_helper::killall"); - } else { + } + else + { int cs; waitpid(h->pid, &cs, 0); } + // close pipes close(h->fd_tx); close(h->fd_rx); @@ -2579,11 +3000,16 @@ struct s2_fecdec_helper : runnable // Spawn a helper process. void spawn_helper(helper_instance *h, const s2_pls *pls) { - if (sch->debug) + if (sch->debug) { fprintf(stderr, "Spawning LDPC helper: modcod=%d sf=%d\n", pls->modcod, pls->sf); + } + int tx[2], rx[2]; - if (pipe(tx) || pipe(rx)) + + if (pipe(tx) || pipe(rx)) { fatal("pipe"); + } + // Size the pipes so that the helper never runs out of work to do. int pipesize = 64800 * batch_size; // macOS does not have F_SETPIPE_SZ and there @@ -2609,10 +3035,12 @@ struct s2_fecdec_helper : runnable "*** Try echo %d > /proc/sys/fs/pipe-max-size\n", min_pipe_size, pipesize); - if (must_buffer) + + if (must_buffer) { fatal("F_SETPIPE_SZ"); - else + } else { fprintf(stderr, "*** Throughput will be suboptimal.\n"); + } } } #endif @@ -2654,11 +3082,16 @@ struct s2_fecdec_helper : runnable h->batch_size = batch_size; // TBD h->b_in = h->b_out = 0; int flags_tx = fcntl(h->fd_tx, F_GETFL); - if (fcntl(h->fd_tx, F_SETFL, flags_tx | O_NONBLOCK)) + + if (fcntl(h->fd_tx, F_SETFL, flags_tx | O_NONBLOCK)) { fatal("fcntl_tx(helper)"); + } + int flags_rx = fcntl(h->fd_rx, F_GETFL); - if (fcntl(h->fd_rx, F_SETFL, flags_rx | O_NONBLOCK)) + + if (fcntl(h->fd_rx, F_SETFL, flags_rx | O_NONBLOCK)) { fatal("fcntl_rx(helper)"); + } } // Receive a finished job. @@ -2690,8 +3123,11 @@ struct s2_fecdec_helper : runnable //size_t chkbytes = cwbytes - msgbytes; bch_interface *bch = s2bch.bchs[job->pls.sf][mcinfo->rate]; int ncorr = bch->decode(hardbytes, cwbytes); - if (sch->debug2) + + if (sch->debug2) { fprintf(stderr, "BCHCORR = %d\n", ncorr); + } + bool corrupted = (ncorr < 0); // Report VBER bitcount_q.push_back(fi->Kbch); @@ -2714,8 +3150,10 @@ struct s2_fecdec_helper : runnable bbscrambling.transform(hardbytes, fi->Kbch / 8, bbframe_q.back().bytes); //out.written(1); } - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "%c", corrupted ? '!' : ncorr ? '.' : '_'); + } } pipereader> in; @@ -2755,6 +3193,7 @@ struct s2_framer : runnable nremain = 0; remcrc = 0; // CRC for nonexistent previous packet } + void run() { while (out.writable() >= 1) @@ -2762,10 +3201,15 @@ struct s2_framer : runnable const modcod_info *mcinfo = check_modcod(pls.modcod); const fec_info *fi = &fec_infos[pls.sf][mcinfo->rate]; int framebytes = fi->Kbch / 8; - if (!framebytes) + + if (!framebytes) { fail("MODCOD/framesize combination not allowed"); - if (10 + nremain + 188 * in.readable() < framebytes) + } + + if (10 + nremain + 188 * in.readable() < framebytes) { break; // Not enough data to fill a frame + } + bbframe *pout = out.wr(); pout->pls = pls; uint8_t *buf = pout->bytes; @@ -2788,27 +3232,39 @@ struct s2_framer : runnable // Data field memcpy(buf, rembuf, nremain); // Leftover from previous runs buf += nremain; + while (buf < end) { tspacket *tsp = in.rd(); - if (tsp->data[0] != MPEG_SYNC) + + if (tsp->data[0] != MPEG_SYNC) { fail("Invalid TS"); + } + *buf++ = remcrc; // Replace SYNC with CRC of previous. remcrc = crc8.compute(tsp->data + 1, tspacket::SIZE - 1); int nused = end - buf; - if (nused > tspacket::SIZE - 1) + + if (nused > tspacket::SIZE - 1) { nused = tspacket::SIZE - 1; + } + memcpy(buf, tsp->data + 1, nused); buf += nused; + if (buf == end) { nremain = (tspacket::SIZE - 1) - nused; memcpy(rembuf, tsp->data + 1 + nused, nremain); } + in.read(1); } - if (buf != end) + + if (buf != end) { fail("Bug: s2_framer"); + } + out.written(1); } } @@ -2845,6 +3301,7 @@ struct s2_deframer : runnable locktime_out(opt_writer(_locktime_out, MAX_TS_PER_BBFRAME)) { } + void run() { while (in.readable() >= 1 && out.writable() >= MAX_TS_PER_BBFRAME && @@ -2857,6 +3314,7 @@ struct s2_deframer : runnable opt_write(state_out, 0); report_state = false; } + run_bbframe(in.rd()); in.read(1); } @@ -2874,6 +3332,7 @@ struct s2_deframer : runnable uint8_t crc = bbh[9]; uint8_t *data = bbh + 10; int ro_code = bbh[0] & 3; + if (sch->debug2) { static float ro_values[] = {0.35, 0.25, 0.20, 0}; @@ -2882,6 +3341,7 @@ struct s2_deframer : runnable crc, crcexp, (crc == crcexp) ? "OK" : "KO", bbh[0], bbh[1], ro_values[ro_code], upl, dfl, sync, syncd); } + if (crc != crcexp || upl != 188 * 8 || sync != 0x47 || dfl > fec_info::KBCH_MAX || syncd > dfl || (dfl & 7) || (syncd & 7)) { @@ -2889,19 +3349,24 @@ struct s2_deframer : runnable if (sch->debug) { fprintf(stderr, "Bad bbframe\n"); } + missing = -1; info_unlocked(); return; } + // TBD Handle packets as payload+finalCRC and do crc8 before pout int pos; // Start of useful data in this bbframe + if (missing < 0) { // Skip unusable data at beginning of bbframe pos = syncd / 8; + if (sch->debug) { fprintf(stderr, "Start TS at %d\n", pos); } + missing = 0; } else @@ -2912,12 +3377,14 @@ struct s2_deframer : runnable if (sch->debug) { fprintf(stderr, "Lost a bbframe ?\n"); } + missing = -1; info_unlocked(); return; } pos = 0; } + if (missing) { // Complete and output the partial TS packet in leftover[]. @@ -2931,6 +3398,7 @@ struct s2_deframer : runnable pos += missing; missing = 0; } + while (pos + 188 <= dfl / 8) { tspacket *pout = out.wr(); @@ -2940,7 +3408,9 @@ struct s2_deframer : runnable info_good_packet(); pos += 188; } + int remain = dfl / 8 - pos; + if (remain) { memcpy(leftover, data + pos, remain); @@ -2954,12 +3424,14 @@ struct s2_deframer : runnable info_is_locked(false); locktime = 0; } + void info_good_packet() { info_is_locked(true); ++locktime; opt_write(locktime_out, locktime); } + void info_is_locked(bool newstate) { if (newstate != current_state) diff --git a/plugins/channelrx/demoddatv/leansdr/filtergen.cpp b/plugins/channelrx/demoddatv/leansdr/filtergen.cpp index 2a7bf41ff..234a77253 100644 --- a/plugins/channelrx/demoddatv/leansdr/filtergen.cpp +++ b/plugins/channelrx/demoddatv/leansdr/filtergen.cpp @@ -11,10 +11,13 @@ namespace filtergen void dump_filter(const char *name, int ncoeffs, float *coeffs) { fprintf(stderr, "%s = [", name); - for (int i = 0; i < ncoeffs; ++i) + + for (int i = 0; i < ncoeffs; ++i) { fprintf(stderr, "%s %f", (i ? "," : ""), coeffs[i]); + } + fprintf(stderr, " ];\n"); } } // filtergen -} // leansdr \ No newline at end of file +} // leansdr diff --git a/plugins/channelrx/demoddatv/leansdr/filtergen.h b/plugins/channelrx/demoddatv/leansdr/filtergen.h index 75d6de142..7a3be82db 100644 --- a/plugins/channelrx/demoddatv/leansdr/filtergen.h +++ b/plugins/channelrx/demoddatv/leansdr/filtergen.h @@ -30,34 +30,50 @@ template void normalize_power(int n, T *coeffs, float gain = 1) { float s2 = 0; - for (int i = 0; i < n; ++i) + + for (int i = 0; i < n; ++i) { s2 = s2 + coeffs[i] * coeffs[i]; // TBD complex - if (s2) + } + + if (s2) { gain /= gen_sqrt(s2); - for (int i = 0; i < n; ++i) + } + + for (int i = 0; i < n; ++i) { coeffs[i] = coeffs[i] * gain; + } } template void normalize_dcgain(int n, T *coeffs, float gain = 1) { float s = 0; - for (int i = 0; i < n; ++i) + + for (int i = 0; i < n; ++i) { s = s + coeffs[i]; - if (s) + } + + if (s) { gain /= s; - for (int i = 0; i < n; ++i) + } + + for (int i = 0; i < n; ++i) { coeffs[i] = coeffs[i] * gain; + } } template void cancel_dcgain(int n, T *coeffs) { float s = 0; - for (int i = 0; i < n; ++i) + + for (int i = 0; i < n; ++i) { s = s + coeffs[i]; - for (int i = 0; i < n; ++i) + } + + for (int i = 0; i < n; ++i) { coeffs[i] -= s / n; + } } // Generate coefficients for a sinc filter. @@ -68,6 +84,7 @@ int lowpass(int order, float Fcut, T **coeffs, float gain = 1) { int ncoeffs = order + 1; *coeffs = new T[ncoeffs]; + for (int i = 0; i < ncoeffs; ++i) { float t = i - (ncoeffs - 1) * 0.5; @@ -80,6 +97,7 @@ int lowpass(int order, float Fcut, T **coeffs, float gain = 1) #endif (*coeffs)[i] = sinc * window; } + normalize_dcgain(ncoeffs, *coeffs, gain); return ncoeffs; } @@ -93,23 +111,31 @@ int root_raised_cosine(int order, float Fs, float rolloff, T **coeffs, float gai float B = rolloff, pi = M_PI; int ncoeffs = (order + 1) | 1; *coeffs = new T[ncoeffs]; + for (int i = 0; i < ncoeffs; ++i) { int t = i - ncoeffs / 2; float c; + if (t == 0) + { c = (1 - B + 4*B/pi); + } else { float tT = t * Fs; float den = pi * tT * (1 - (4 * B * tT) * (4 * B * tT)); - if (!den) + + if (!den) { c = B/sqrtf(2) * ( (1+2/pi)*sinf(pi/(4*B)) + (1-2/pi)*cosf(pi/(4*B)) ); - else + } else { c = ( sinf(pi*tT*(1-B)) + 4*B*tT*cosf(pi*tT*(1+B)) ) / den; + } } + (*coeffs)[i] = Fs * c * gain; } + return ncoeffs; } diff --git a/plugins/channelrx/demoddatv/leansdr/framework.cpp b/plugins/channelrx/demoddatv/leansdr/framework.cpp index c9017e151..20d98e30c 100644 --- a/plugins/channelrx/demoddatv/leansdr/framework.cpp +++ b/plugins/channelrx/demoddatv/leansdr/framework.cpp @@ -3,13 +3,11 @@ namespace leansdr { -void fatal(const char *s) -{ +void fatal(const char *s) { perror(s); } -void fail(const char *s) -{ +void fail(const char *s) { fprintf(stderr, "** %s\n", s); } diff --git a/plugins/channelrx/demoddatv/leansdr/framework.h b/plugins/channelrx/demoddatv/leansdr/framework.h index 3a0a3caec..ca1d5fa64 100644 --- a/plugins/channelrx/demoddatv/leansdr/framework.h +++ b/plugins/channelrx/demoddatv/leansdr/framework.h @@ -52,29 +52,24 @@ static const int MAX_READERS = 8; struct pipebuf_common { - virtual int sizeofT() - { + virtual int sizeofT() { return 0; } - virtual long long hash() - { + virtual long long hash() { return 0; } - virtual void dump(std::size_t *total_bufs) - { + virtual void dump(std::size_t *total_bufs) { (void)total_bufs; } const char *name; - pipebuf_common(const char *_name) : name(_name) - { + pipebuf_common(const char *_name) : name(_name) { } - virtual ~pipebuf_common() - { + virtual ~pipebuf_common() { } }; @@ -82,21 +77,18 @@ struct runnable_common { const char *name; - runnable_common(const char *_name) : name(_name) - { + runnable_common(const char *_name) : name(_name) { } - virtual ~runnable_common() - { + virtual ~runnable_common() { } - virtual void run() - { + virtual void run() { } - virtual void shutdown() - { + virtual void shutdown() { } + #ifdef DEBUG ~runnable_common() { @@ -132,22 +124,27 @@ struct scheduler void add_pipe(pipebuf_common *p) { - if (npipes == MAX_PIPES) + if (npipes == MAX_PIPES) { fail("MAX_PIPES"); + } + pipes[npipes++] = p; } void add_runnable(runnable_common *r) { - if (nrunnables == MAX_RUNNABLES) + if (nrunnables == MAX_RUNNABLES) { fail("MAX_RUNNABLES"); + } + runnables[nrunnables++] = r; } void step() { - for (int i = 0; i < nrunnables; ++i) + for (int i = 0; i < nrunnables; ++i) { runnables[i]->run(); + } } void run() @@ -158,23 +155,30 @@ struct scheduler { step(); unsigned long long h = hash(); - if (h == prev_hash) + + if (h == prev_hash) { break; + } + prev_hash = h; } } void shutdown() { - for (int i = 0; i < nrunnables; ++i) + for (int i = 0; i < nrunnables; ++i) { runnables[i]->shutdown(); + } } unsigned long long hash() { unsigned long long h = 0; - for (int i = 0; i < npipes; ++i) + + for (int i = 0; i < npipes; ++i) { h += (1 + i) * pipes[i]->hash(); + } + return h; } @@ -182,8 +186,11 @@ struct scheduler { fprintf(stderr, "\n"); std::size_t total_bufs = 0; - for (int i = 0; i < npipes; ++i) + + for (int i = 0; i < npipes; ++i) { pipes[i]->dump(&total_bufs); + } + fprintf(stderr, "Total buffer memory: %ld KiB\n", (unsigned long)total_bufs / 1024); } @@ -191,7 +198,12 @@ struct scheduler struct runnable : runnable_common { - runnable(scheduler *_sch, const char *name) : runnable_common(name), sch(_sch) + runnable( + scheduler *_sch, + const char *name + ) : + runnable_common(name), + sch(_sch) { sch->add_runnable(this); } @@ -209,7 +221,11 @@ struct pipebuf : pipebuf_common T *wr; T *end; - pipebuf(scheduler *sch, const char *name, unsigned long size) : + pipebuf( + scheduler *sch, + const char *name, + unsigned long size + ) : pipebuf_common(name), nrd(0), min_write(1), @@ -233,8 +249,10 @@ struct pipebuf : pipebuf_common int add_reader() { - if (nrd == MAX_READERS) + if (nrd == MAX_READERS) { fail("too many readers"); + } + rds[nrd] = wr; return nrd++; } @@ -242,13 +260,20 @@ struct pipebuf : pipebuf_common void pack() { T *rd = wr; + for (int i = 0; i < nrd; ++i) - if (rds[i] < rd) + { + if (rds[i] < rd) { rd = rds[i]; + } + } + memmove(buf, rd, (wr - rd) * sizeof(T)); wr -= rd - buf; - for (int i = 0; i < nrd; ++i) + + for (int i = 0; i < nrd; ++i) { rds[i] -= rd - buf; + } } long long hash() @@ -259,26 +284,42 @@ struct pipebuf : pipebuf_common void dump(std::size_t *total_bufs) { if (total_written < 10000) + { fprintf(stderr, ".%-16s : %4ld/%4ld", name, total_read, total_written); + } else if (total_written < 1000000) + { fprintf(stderr, ".%-16s : %3ldk/%3ldk", name, total_read / 1000, total_written / 1000); + } else + { fprintf(stderr, ".%-16s : %3ldM/%3ldM", name, total_read / 1000000, total_written / 1000000); + } + *total_bufs += (end - buf) * sizeof(T); unsigned long nw = end - wr; fprintf(stderr, " %6ld writable %c,", nw, (nw < min_write) ? '!' : ' '); T *rd = wr; + for (int j = 0; j < nrd; ++j) - if (rds[j] < rd) + { + if (rds[j] < rd) { rd = rds[j]; + } + } + fprintf(stderr, " %6d unread (", (int)(wr - rd)); - for (int j = 0; j < nrd; ++j) + + for (int j = 0; j < nrd; ++j) { fprintf(stderr, " %d", (int)(wr - rds[j])); + } + fprintf(stderr, " )\n"); } + unsigned long min_write; unsigned long total_written, total_read; #ifdef DEBUG @@ -296,19 +337,21 @@ struct pipewriter pipewriter(pipebuf &_buf, unsigned long min_write = 1) : buf(_buf) { - if (min_write > buf.min_write) + if (min_write > buf.min_write) { buf.min_write = min_write; + } } // Return number of items writable at this->wr, 0 if full. long writable() { - if (buf.end < buf.min_write + buf.wr) + if (buf.end < buf.min_write + buf.wr) { buf.pack(); + } + return buf.end - buf.wr; } - T *wr() - { + T *wr() { return buf.wr; } @@ -346,8 +389,9 @@ bool opt_writable(pipewriter *p, int n = 1) template void opt_write(pipewriter *p, T val) { - if (p) + if (p) { p->write(val); + } } template @@ -357,16 +401,13 @@ struct pipereader int id; pipereader(pipebuf &_buf) : buf(_buf), id(_buf.add_reader()) - { - } + {} - long readable() - { + long readable() { return buf.wr - buf.rds[id]; } - T *rd() - { + T *rd() { return buf.rds[id]; } @@ -386,71 +427,59 @@ struct pipereader template T gen_sqrt(T x); -inline float gen_sqrt(float x) -{ +inline float gen_sqrt(float x) { return sqrtf(x); } -inline unsigned int gen_sqrt(unsigned int x) -{ +inline unsigned int gen_sqrt(unsigned int x) { return sqrtl(x); } -inline long double gen_sqrt(long double x) -{ +inline long double gen_sqrt(long double x) { return sqrtl(x); } template T gen_abs(T x); -inline float gen_abs(float x) -{ +inline float gen_abs(float x) { return fabsf(x); } -inline int gen_abs(int x) -{ +inline int gen_abs(int x) { return abs(x); } -inline long int gen_abs(long int x) -{ +inline long int gen_abs(long int x) { return labs(x); } template T gen_hypot(T x, T y); -inline float gen_hypot(float x, float y) -{ +inline float gen_hypot(float x, float y) { return hypotf(x, y); } -inline long double gen_hypot(long double x, long double y) -{ +inline long double gen_hypot(long double x, long double y) { return hypotl(x, y); } template T gen_atan2(T y, T x); -inline float gen_atan2(float y, float x) -{ +inline float gen_atan2(float y, float x) { return atan2f(y, x); } -inline long double gen_atan2(long double y, long double x) -{ +inline long double gen_atan2(long double y, long double x) { return atan2l(y, x); } template -T min(const T &x, const T &y) -{ +T min(const T &x, const T &y) { return (x < y) ? x : y; } template -T max(const T &x, const T &y) -{ +T max(const T &x, const T &y) { return (x < y) ? y : x; } diff --git a/plugins/channelrx/demoddatv/leansdr/generic.h b/plugins/channelrx/demoddatv/leansdr/generic.h index 11d13c88e..5fbe0da68 100644 --- a/plugins/channelrx/demoddatv/leansdr/generic.h +++ b/plugins/channelrx/demoddatv/leansdr/generic.h @@ -43,67 +43,105 @@ namespace leansdr template struct file_reader : runnable { - file_reader(scheduler *sch, int _fdin, pipebuf &_out) - : runnable(sch, _out.name), - loop(false), - filler(NULL), - fdin(_fdin), out(_out) + bool loop; + + file_reader( + scheduler *sch, + int _fdin, + pipebuf &_out + ) : + runnable(sch, _out.name), + loop(false), + filler(nullptr), + fdin(_fdin), + out(_out) { } + + ~file_reader() + { + delete[] filler; + } + void run() { size_t size = out.writable() * sizeof(T); - if (!size) + + if (!size) { return; + } again: ssize_t nr = read(fdin, out.wr(), size); + if (nr < 0 && errno == EWOULDBLOCK) { if (filler) { - if (sch->debug) + if (sch->debug) { fprintf(stderr, "U"); + } + out.write(*filler); } + return; } - if (nr < 0) + + if (nr < 0) { fatal("read(file_reader)"); + } + if (!nr) { - if (!loop) + if (!loop) { return; - if (sch->debug) + } + + if (sch->debug) { fprintf(stderr, "%s looping\n", name); + } + off_t res = lseek(fdin, 0, SEEK_SET); - if (res == (off_t)-1) + + if (res == (off_t)-1) { fatal("lseek"); + } + goto again; } // Always stop at element boundary (may block) size_t partial = nr % sizeof(T); size_t remain = partial ? sizeof(T) - partial : 0; + while (remain) { - if (sch->debug) + if (sch->debug) { fprintf(stderr, "+"); + } + ssize_t nr2 = read(fdin, (char *)out.wr() + nr, remain); - if (nr2 <= 0) + + if (nr2 <= 0) { fatal("partial read"); + } + nr += nr2; remain -= nr2; } out.written(nr / sizeof(T)); } - bool loop; + void set_realtime(T &_filler) { int flags = fcntl(fdin, F_GETFL); - if (fcntl(fdin, F_SETFL, flags | O_NONBLOCK)) + + if (fcntl(fdin, F_SETFL, flags | O_NONBLOCK)) { fatal("fcntl"); + } + filler = new T(_filler); } @@ -118,22 +156,39 @@ struct file_reader : runnable template struct file_writer : runnable { - file_writer(scheduler *sch, pipebuf &_in, int _fdout) : runnable(sch, _in.name), - in(_in), fdout(_fdout) + file_writer( + scheduler *sch, + pipebuf &_in, + int _fdout + ) : + runnable(sch, _in.name), + in(_in), + fdout(_fdout) { } + void run() { int size = in.readable() * sizeof(T); - if (!size) + + if (!size) { return; + } + int nw = write(fdout, in.rd(), size); - if (!nw) + + if (!nw) { fatal("pipe"); - if (nw < 0) + } + + if (nw < 0) { fatal("write"); - if (nw % sizeof(T)) + } + + if (nw % sizeof(T)) { fatal("partial write"); + } + in.read(nw / sizeof(T)); } @@ -148,17 +203,28 @@ struct file_writer : runnable template struct file_printer : runnable { - file_printer(scheduler *sch, const char *_format, - pipebuf &_in, int _fdout, - int _decimation = 1) : runnable(sch, _in.name), - scale(1), decimation(_decimation), - in(_in), format(_format), fdout(_fdout), phase(0) + file_printer( + scheduler *sch, + const char *_format, + pipebuf &_in, + int _fdout, + int _decimation = 1 + ) : + runnable(sch, _in.name), + scale(1), + decimation(_decimation), + in(_in), + format(_format), + fdout(_fdout), + phase(0) { } + void run() { int n = in.readable(); T *pin = in.rd(), *pend = pin + n; + for (; pin < pend; ++pin) { if (++phase >= decimation) @@ -166,15 +232,22 @@ struct file_printer : runnable phase -= decimation; char buf[256]; int len = snprintf(buf, sizeof(buf), format, (*pin) * scale); - if (len < 0) + + if (len < 0) { fatal("obsolete glibc"); + } + int nw = write(fdout, buf, len); - if (nw != len) + + if (nw != len) { fatal("partial write"); + } } } + in.read(n); } + T scale; int decimation; @@ -192,42 +265,62 @@ struct file_printer : runnable template struct file_carrayprinter : runnable { - file_carrayprinter(scheduler *sch, - const char *_head, - const char *_format, - const char *_sep, - const char *_tail, - pipebuf> &_in, int _fdout) : runnable(sch, _in.name), - scale(1), fixed_size(0), in(_in), - head(_head), format(_format), sep(_sep), tail(_tail), - fout(fdopen(_fdout, "w")) + file_carrayprinter( + scheduler *sch, + const char *_head, + const char *_format, + const char *_sep, + const char *_tail, + pipebuf> &_in, + int _fdout + ) : + runnable(sch, _in.name), + scale(1), + fixed_size(0), + in(_in), + head(_head), + format(_format), + sep(_sep), + tail(_tail), + fout(fdopen(_fdout, "w")) { } + void run() { int n, nmin = fixed_size ? fixed_size : 1; + while ((n = in.readable()) >= nmin) { - if (fixed_size) + if (fixed_size) { n = fixed_size; + } + if (fout) { fprintf(fout, head, n); complex *pin = in.rd(); + for (int i = 0; i < n; ++i) { - if (i) + if (i) { fprintf(fout, "%s", sep); + } + fprintf(fout, format, pin[i].re * scale, pin[i].im * scale); } + fprintf(fout, "%s", tail); } + fflush(fout); in.read(n); } } + T scale; int fixed_size; // Number of elements per batch, or 0. + private: pipereader> in; const char *head, *format, *sep, *tail; @@ -237,18 +330,32 @@ struct file_carrayprinter : runnable template struct file_vectorprinter : runnable { - file_vectorprinter(scheduler *sch, - const char *_head, - const char *_format, - const char *_sep, - const char *_tail, - pipebuf &_in, int _fdout, int _n = N) : runnable(sch, _in.name), scale(1), in(_in), - head(_head), format(_format), sep(_sep), tail(_tail), n(_n) + file_vectorprinter( + scheduler *sch, + const char *_head, + const char *_format, + const char *_sep, + const char *_tail, + pipebuf &_in, + int _fdout, + int _n = N + ) : + runnable(sch, _in.name), + scale(1), + in(_in), + head(_head), + format(_format), + sep(_sep), + tail(_tail), + n(_n) { fout = fdopen(_fdout, "w"); - if (!fout) + + if (!fout) { fatal("fdopen"); + } } + void run() { while (in.readable() >= 1) @@ -256,17 +363,23 @@ struct file_vectorprinter : runnable fprintf(fout, head, n); T(*pin) [N] = in.rd(); + for (int i = 0; i < n; ++i) { - if (i) + if (i) { fprintf(fout, "%s", sep); + } + fprintf(fout, format, (*pin)[i] * scale); } + fprintf(fout, "%s", tail); in.read(1); } + fflush(fout); } + T scale; private: @@ -282,18 +395,29 @@ struct file_vectorprinter : runnable template struct itemcounter : runnable { - itemcounter(scheduler *sch, pipebuf &_in, pipebuf &_out) - : runnable(sch, "itemcounter"), - in(_in), out(_out) + itemcounter( + scheduler *sch, + pipebuf &_in, + pipebuf &_out + ) : + runnable(sch, "itemcounter"), + in(_in), + out(_out) { } + void run() { - if (out.writable() < 1) + if (out.writable() < 1) { return; + } + unsigned long count = in.readable(); - if (!count) + + if (!count) { return; + } + out.write(count); in.read(count); } @@ -310,18 +434,28 @@ struct decimator : runnable { int d; - decimator(scheduler *sch, int _d, pipebuf &_in, pipebuf &_out) - : runnable(sch, "decimator"), - d(_d), - in(_in), out(_out) + decimator( + scheduler *sch, + int _d, + pipebuf &_in, + pipebuf &_out + ) : + runnable(sch, "decimator"), + d(_d), + in(_in), + out(_out) { } + void run() { long count = min(in.readable() / d, out.writable()); T *pin = in.rd(), *pend = pin + count * d, *pout = out.wr(); - for (; pin < pend; pin += d, ++pout) + + for (; pin < pend; pin += d, ++pout) { *pout = *pin; + } + in.read(count * d); out.written(count); } @@ -339,29 +473,40 @@ struct rate_estimator : runnable { int sample_size; - rate_estimator(scheduler *sch, - pipebuf &_num, pipebuf &_den, - pipebuf &_rate) - : runnable(sch, "rate_estimator"), - sample_size(10000), - num(_num), den(_den), rate(_rate), - acc_num(0), acc_den(0) + rate_estimator( + scheduler *sch, + pipebuf &_num, + pipebuf &_den, + pipebuf &_rate + ) : + runnable(sch, "rate_estimator"), + sample_size(10000), + num(_num), + den(_den), + rate(_rate), + acc_num(0), + acc_den(0) { } void run() { - if (rate.writable() < 1) + if (rate.writable() < 1) { return; + } + int count = min(num.readable(), den.readable()); int *pnum = num.rd(), *pden = den.rd(); + for (int n = count; n--; ++pnum, ++pden) { acc_num += *pnum; acc_den += *pden; } + num.read(count); den.read(count); + if (acc_den >= sample_size) { rate.write((float)acc_num / acc_den); @@ -380,14 +525,23 @@ struct rate_estimator : runnable template struct serializer : runnable { - serializer(scheduler *sch, pipebuf &_in, pipebuf &_out) - : nin(max((size_t)1, sizeof(Tin) / sizeof(Tout))), - nout(max((size_t)1, sizeof(Tout) / sizeof(Tin))), - in(_in), out(_out, nout) + serializer( + scheduler *sch, + pipebuf &_in, + pipebuf &_out + ) : + nin(max((size_t)1, + sizeof(Tin) / sizeof(Tout))), + nout(max((size_t)1, + sizeof(Tout) / sizeof(Tin))), + in(_in), + out(_out, nout) { - if (nin * sizeof(Tin) != nout * sizeof(Tout)) + if (nin * sizeof(Tin) != nout * sizeof(Tout)) { fail("serializer: incompatible sizes"); + } } + void run() { while (in.readable() >= nin && out.writable() >= nout) @@ -409,11 +563,20 @@ struct serializer : runnable template struct buffer_reader : runnable { - buffer_reader(scheduler *sch, T *_data, int _count, pipebuf &_out) - : runnable(sch, "buffer_reader"), - data(_data), count(_count), out(_out), pos(0) + buffer_reader( + scheduler *sch, + T *_data, + int _count, + pipebuf &_out + ) : + runnable(sch, "buffer_reader"), + data(_data), + count(_count), + out(_out), + pos(0) { } + void run() { int n = min(out.writable(), (unsigned long)(count - pos)); @@ -434,11 +597,20 @@ struct buffer_reader : runnable template struct buffer_writer : runnable { - buffer_writer(scheduler *sch, pipebuf &_in, T *_data, int _count) - : runnable(sch, "buffer_reader"), - in(_in), data(_data), count(_count), pos(0) + buffer_writer( + scheduler *sch, + pipebuf &_in, + T *_data, + int _count + ) : + runnable(sch, "buffer_reader"), + in(_in), + data(_data), + count(_count), + pos(0) { } + void run() { int n = min(in.readable(), (unsigned long)(count - pos)); diff --git a/plugins/channelrx/demoddatv/leansdr/hdlc.h b/plugins/channelrx/demoddatv/leansdr/hdlc.h index 2b6e60e6d..6606db728 100644 --- a/plugins/channelrx/demoddatv/leansdr/hdlc.h +++ b/plugins/channelrx/demoddatv/leansdr/hdlc.h @@ -27,17 +27,25 @@ namespace leansdr struct hdlc_dec { - hdlc_dec(int _minframesize, // Including CRC, excluding HDLC flags. - int _maxframesize, - bool _invert) : minframesize(_minframesize), - maxframesize(_maxframesize), - invertmask(_invert ? 0xff : 0), - framebuf(new u8[maxframesize]), - debug(false) + hdlc_dec( + int _minframesize, // Including CRC, excluding HDLC flags. + int _maxframesize, + bool _invert + ) : + minframesize(_minframesize), + maxframesize(_maxframesize), + invertmask(_invert ? 0xff : 0), + framebuf(new u8[maxframesize]), + debug(false) { reset(); } + ~hdlc_dec() + { + delete[] framebuf; + } + void reset() { shiftreg = 0; @@ -51,7 +59,7 @@ struct hdlc_dec } // Decode (*ppin)[count] as MSB-packed HDLC bitstream. - // Return pointer to buffer[*pdatasize], or NULL if no valid frame. + // Return pointer to buffer[*pdatasize], or nullptr if no valid frame. // Return number of discarded bytes in *discarded. // Return number of checksum errors in *fcs_errors. // *ppin will have increased by at least 1 (unless count==0). @@ -92,31 +100,41 @@ struct hdlc_dec if (nbits_out != 7) { // Not at byte boundary - if (debug) + if (debug) { fprintf(stderr, "^"); + } + ++*hdlc_errors; } else { // Checksum crc16 ^= 0xffff; + if (framesize < 2 || framesize < minframesize || crc16 != crc16_check) { - if (debug) + if (debug) { fprintf(stderr, "!"); + } + ++*hdlc_errors; + // Do not report random noise as FCS errors - if (framesize >= minframesize) + if (framesize >= minframesize) { ++*fcs_errors; + } } else { - if (debug) + if (debug) { fprintf(stderr, "_"); + } + // This will trigger output, but we finish the byte first. *pdatasize = framesize - 2; } } + nbits_out = 0; begin_frame(); // Keep processing up to 7 remaining bits from byte_in. @@ -126,16 +144,20 @@ struct hdlc_dec { // 11111110 HDLC invalid if (framesize) { - if (debug) + if (debug) { fprintf(stderr, "^"); + } + ++*hdlc_errors; } + inframe = false; } else { // Data bit byte_out = (byte_out >> 1) | bit_in; // HDLC is LSB first ++nbits_out; + if (nbits_out == 8) { if (framesize < maxframesize) @@ -143,11 +165,13 @@ struct hdlc_dec framebuf[framesize++] = byte_out; crc16_byte(byte_out); } + nbits_out = 0; } } } // inframe } // bits + if (*pdatasize != -1) { // Found a complete frame @@ -157,7 +181,7 @@ struct hdlc_dec } *ppin = pin; - return NULL; + return nullptr; } private: @@ -181,8 +205,7 @@ struct hdlc_dec { crc16 ^= data; - for (int bit = 8; bit--;) - { + for (int bit = 8; bit--;) { crc16 = (crc16 & 1) ? (crc16 >> 1) ^ crc16_poly : (crc16 >> 1); } } @@ -196,38 +219,42 @@ struct hdlc_dec struct hdlc_sync : runnable { - hdlc_sync(scheduler *sch, pipebuf &_in, // Packed bits - pipebuf &_out, // Bytes - int _minframesize, // Including CRC, excluding HDLC flags. - int _maxframesize, - // Status - pipebuf *_lock_out = NULL, - pipebuf *_framecount_out = NULL, - pipebuf *_fcserrcount_out = NULL, - pipebuf *_hdlcbytecount_out = NULL, - pipebuf *_databytecount_out = NULL) : runnable(sch, "hdlc_sync"), - minframesize(_minframesize), - maxframesize(_maxframesize), - chunk_size(maxframesize + 2), - in(_in), - out(_out, _maxframesize + chunk_size), - lock_out(opt_writer(_lock_out)), - framecount_out(opt_writer(_framecount_out)), - fcserrcount_out(opt_writer(_fcserrcount_out)), - hdlcbytecount_out(opt_writer(_hdlcbytecount_out)), - databytecount_out(opt_writer(_databytecount_out)), - cur_sync(0), - resync_phase(0), - lock_state(false), - resync_period(32), - header16(false) + hdlc_sync( + scheduler *sch, pipebuf &_in, // Packed bits + pipebuf &_out, // Bytes + int _minframesize, // Including CRC, excluding HDLC flags. + int _maxframesize, + // Status + pipebuf *_lock_out = nullptr, + pipebuf *_framecount_out = nullptr, + pipebuf *_fcserrcount_out = nullptr, + pipebuf *_hdlcbytecount_out = nullptr, + pipebuf *_databytecount_out = nullptr + ) : + runnable(sch, "hdlc_sync"), + minframesize(_minframesize), + maxframesize(_maxframesize), + chunk_size(maxframesize + 2), + in(_in), + out(_out, _maxframesize + chunk_size), + lock_out(opt_writer(_lock_out)), + framecount_out(opt_writer(_framecount_out)), + fcserrcount_out(opt_writer(_fcserrcount_out)), + hdlcbytecount_out(opt_writer(_hdlcbytecount_out)), + databytecount_out(opt_writer(_databytecount_out)), + cur_sync(0), + resync_phase(0), + lock_state(false), + resync_period(32), + header16(false) { for (int s = 0; s < NSYNCS; ++s) { syncs[s].dec = new hdlc_dec(minframesize, maxframesize, s != 0); - for (int h = 0; h < NERRHIST; ++h) + for (int h = 0; h < NERRHIST; ++h) { syncs[s].errhist[h] = 0; + } } syncs[cur_sync].dec->debug = sch->debug; @@ -236,7 +263,11 @@ struct hdlc_sync : runnable void run() { - if (!opt_writable(lock_out) || !opt_writable(framecount_out) || !opt_writable(fcserrcount_out) || !opt_writable(hdlcbytecount_out) || !opt_writable(databytecount_out)) + if (!opt_writable(lock_out) || + !opt_writable(framecount_out) || + !opt_writable(fcserrcount_out) || + !opt_writable(hdlcbytecount_out) || + !opt_writable(databytecount_out)) { return; } @@ -253,8 +284,9 @@ struct hdlc_sync : runnable // Once every resync_phase, try all decoders for (int s = 0; s < NSYNCS; ++s) { - if (s != cur_sync) + if (s != cur_sync) { syncs[s].dec->reset(); + } syncs[s].errhist[errslot] = 0; @@ -289,22 +321,27 @@ struct hdlc_sync : runnable { total_errors[s] = 0; - for (int h = 0; h < NERRHIST; ++h) + for (int h = 0; h < NERRHIST; ++h) { total_errors[s] += syncs[s].errhist[h]; + } } int best = cur_sync; for (int s = 0; s < NSYNCS; ++s) - if (total_errors[s] < total_errors[best]) + { + if (total_errors[s] < total_errors[best]) { best = s; + } + } if (best != cur_sync) { lock_state = false; - if (sch->debug) + if (sch->debug) { fprintf(stderr, "[%d:%d->%d:%d]", cur_sync, total_errors[cur_sync], best, total_errors[best]); + } // No verbose messages on candidate syncs syncs[cur_sync].dec->debug = false; @@ -336,12 +373,14 @@ struct hdlc_sync : runnable in.read(chunk_size); hdlcbytecount += chunk_size; - if (++resync_phase >= resync_period) + if (++resync_phase >= resync_period) { resync_phase = 0; + } } // Work to do - if (lock_state != previous_lock_state) + if (lock_state != previous_lock_state) { opt_write(lock_out, lock_state ? 1 : 0); + } opt_write(framecount_out, framecount); opt_write(fcserrcount_out, fcserrcount); @@ -388,8 +427,7 @@ struct hdlc_sync : runnable public: int resync_period; bool header16; // Output length prefix -}; -// hdlc_sync +}; // hdlc_sync } // namespace leansdr diff --git a/plugins/channelrx/demoddatv/leansdr/iess.h b/plugins/channelrx/demoddatv/leansdr/iess.h index 66111f14b..e003a26e2 100644 --- a/plugins/channelrx/demoddatv/leansdr/iess.h +++ b/plugins/channelrx/demoddatv/leansdr/iess.h @@ -28,22 +28,28 @@ namespace leansdr struct etr192_descrambler : runnable { - etr192_descrambler(scheduler *sch, - pipebuf &_in, // Packed scrambled bits - pipebuf &_out) // Packed bits - : runnable(sch, "etr192_dec"), - in(_in), out(_out), - shiftreg(0), counter(0) + etr192_descrambler( + scheduler *sch, + pipebuf &_in, // Packed scrambled bits + pipebuf &_out // Packed bits + ) : + runnable(sch, "etr192_dec"), + in(_in), + out(_out), + shiftreg(0), + counter(0) { } void run() { int count = min(in.readable(), out.writable()); + for (u8 *pin = in.rd(), *pend = pin + count, *pout = out.wr(); pin < pend; ++pin, ++pout) { u8 byte_in = *pin, byte_out = 0; + for (int b = 8; b--; byte_in <<= 1) { // Levels before clock transition @@ -61,8 +67,10 @@ struct etr192_descrambler : runnable counter = reset_counter ? 0 : (counter + 1) & 31; byte_out = (byte_out << 1) | bit_out; } + *pout = byte_out; } + in.read(count); out.written(count); } diff --git a/plugins/channelrx/demoddatv/leansdr/ldpc.h b/plugins/channelrx/demoddatv/leansdr/ldpc.h index 7cb6891f8..ca510bee5 100644 --- a/plugins/channelrx/demoddatv/leansdr/ldpc.h +++ b/plugins/channelrx/demoddatv/leansdr/ldpc.h @@ -51,12 +51,6 @@ struct ldpc_table template struct ldpc_engine { - - ldpc_engine() - : vnodes(NULL), cnodes(NULL) - { - } - // vnodes: Value/variable nodes (message bits) // cnodes: Check nodes (parity bits) @@ -68,14 +62,18 @@ struct ldpc_engine Taddr *edges; int nedges; static const int CHUNK = 4; // Grow edges[] in steps of CHUNK. + void append(Taddr a) { if (nedges % CHUNK == 0) { // Full ? edges = (Taddr *)realloc(edges, (nedges + CHUNK) * sizeof(Taddr)); - if (!edges) + + if (!edges) { fatal("realloc"); + } } + edges[nedges++] = a; } }; @@ -83,23 +81,44 @@ struct ldpc_engine node *vnodes; // [k] node *cnodes; // [n-k] + ldpc_engine() : + vnodes(nullptr), + cnodes(nullptr) + { + } + // Initialize from a S2-style table. - ldpc_engine(const ldpc_table *table, int _k, int _n) - : k(_k), n(_n) + ldpc_engine( + const ldpc_table *table, + int _k, + int _n + ) : + k(_k), + n(_n) { // Sanity checks - if (360 % SWSIZE) + if (360 % SWSIZE) { fatal("Bad LDPC word size"); - if (k % SWSIZE) + } + + if (k % SWSIZE) { fatal("Bad LDPC k"); - if (n % SWSIZE) + } + + if (n % SWSIZE) { fatal("Bad LDPC n"); - if (k != table->nrows * 360) + } + + if (k != table->nrows * 360) { fatal("Bad table"); + } + int n_k = n - k; - if (table->q * 360 != n_k) + + if (table->q * 360 != n_k) { fatal("Bad q"); + } vnodes = new node[k]; memset(vnodes, 0, sizeof(node) * k); @@ -117,16 +136,23 @@ struct ldpc_engine // Process 360 bits per row. int q = table->q; int qoffs = 0; + for (int mw = 360; mw--; ++m, qoffs += q) { const Taddr *pa = prow->cols; + for (int nc = prow->ncols; nc--; ++pa) { int a = (int)*pa + qoffs; - if (a >= n_k) + + if (a >= n_k) { a -= n_k; // Modulo n-k. Note qoffs<360*q. - if (a >= n_k) + } + + if (a >= n_k) { fail("Invalid LDPC table"); + } + vnodes[m].append(a); cnodes[a].append(m); } @@ -134,6 +160,16 @@ struct ldpc_engine } } + ~ldpc_engine() + { + if (vnodes) { + delete[] vnodes; + } + if (cnodes) { + delete[] cnodes; + } + } + void print_node_stats() { int nedges = count_edges(vnodes, k); @@ -145,8 +181,11 @@ struct ldpc_engine int count_edges(node *nodes, int nnodes) { int c = 0; - for (int i = 0; i < nnodes; ++i) + + for (int i = 0; i < nnodes; ++i) { c += nodes[i].nedges; + } + return c; } @@ -196,24 +235,41 @@ struct ldpc_engine } #endif - void encode(const ldpc_table *table, const SOFTWORD *msg, - int k, int n, SOFTWORD *parity, int integrate = true) + void encode( + const ldpc_table *table, + const SOFTWORD *msg, + int k, + int n, + SOFTWORD *parity, + int integrate = true + ) { // Sanity checks - if (360 % SWSIZE) + if (360 % SWSIZE) { fatal("Bad LDPC word size"); - if (k % SWSIZE) - fatal("Bad LDPC k"); - if (n % SWSIZE) - fatal("Bad LDPC n"); - if (k != table->nrows * 360) - fatal("Bad table"); - int n_k = n - k; - if (table->q * 360 != n_k) - fatal("Bad q"); + } - for (int i = 0; i < n_k / SWSIZE; ++i) + if (k % SWSIZE) { + fatal("Bad LDPC k"); + } + + if (n % SWSIZE) { + fatal("Bad LDPC n"); + } + + if (k != table->nrows * 360) { + fatal("Bad table"); + } + + int n_k = n - k; + + if (table->q * 360 != n_k) { + fatal("Bad q"); + } + + for (int i = 0; i < n_k / SWSIZE; ++i) { softword_zero(&parity[i]); + } // Iterate over rows for (const typename ldpc_table::row *prow = table->rows; // quirk @@ -223,29 +279,39 @@ struct ldpc_engine // Process 360 bits per row, in words of SWSIZE bits int q = table->q; int qoffs = 0; + for (int mw = 360 / SWSIZE; mw--; ++msg) { SOFTWORD msgword = *msg; + for (int wbit = 0; wbit < SWSIZE; ++wbit, qoffs += q) { SOFTBIT msgbit = softword_get(msgword, wbit); - if (!softbit_harden(msgbit)) + + if (!softbit_harden(msgbit)) { continue; + } + const Taddr *pa = prow->cols; + for (int nc = prow->ncols; nc--; ++pa) { int a = (int)*pa + qoffs; // Note: qoffs < 360*q=n-k - if (a >= n_k) + + if (a >= n_k) { a -= n_k; // TBD not predictable + } + softwords_flip(parity, a); } } } } - if (integrate) + if (integrate) { integrate_bits(parity, parity, n_k / SWSIZE); + } } // Flip bits connected to parity errors, one at a time, @@ -257,17 +323,25 @@ struct ldpc_engine typedef int64_t score_t; - score_t compute_scores(SOFTWORD *m, SOFTWORD *p, SOFTWORD *q, int nc, - score_t *score, int k) + score_t compute_scores( + SOFTWORD *m, + SOFTWORD *p, + SOFTWORD *q, + int nc, + score_t *score, + int k) { int total = 0; memset(score, 0, k * sizeof(*score)); + for (int c = 0; c < nc; ++c) { SOFTBIT err = softwords_xor(p, q, c); + if (softbit_harden(err)) { Taddr *pe = cnodes[c].edges; + for (int e = cnodes[c].nedges; e--; ++pe) { int v = *pe; @@ -279,15 +353,21 @@ struct ldpc_engine } } } + return total; } - int decode_bitflip(const ldpc_table *table, SOFTWORD *cw, - int k, int n, - int max_bitflips) + int decode_bitflip( + const ldpc_table *table, + SOFTWORD *cw, + int k, + int n, + int max_bitflips) { - if (!vnodes) + if (!vnodes) { fail("LDPC graph not initialized"); + } + int n_k = n - k; // Compute the expected check bits (without the final mixing) @@ -312,16 +392,19 @@ struct ldpc_engine } bool progress = true; + while (progress && nflipped < max_bitflips) { progress = false; // Try to flip parity bits. // Due to differential decoding, they appear as consecutive errors. SOFTBIT prev_err = softwords_xor(expected, received, 0); + for (int b = 0; b < n - k - 1; ++b) { prev_err = softwords_xor(expected, received, b); //TBD SOFTBIT err = softwords_xor(expected, received, b + 1); + if (softbit_harden(prev_err) && softbit_harden(err)) { lfprintf(stderr, "flip parity %d\n", b); @@ -330,9 +413,11 @@ struct ldpc_engine ++nflipped; // Counts as one flip before differential decoding. progress = true; int dtot = 0; + // Depenalize adjacent message bits. { Taddr *pe = cnodes[b].edges; + for (int e = cnodes[b].nedges; e--; ++pe) { int d = prev_err * softwords_weight(cw, *pe) * PPCM / vnodes[*pe].nedges; @@ -340,8 +425,10 @@ struct ldpc_engine dtot -= d; } } + { Taddr *pe = cnodes[b + 1].edges; + for (int e = cnodes[b + 1].nedges; e--; ++pe) { int d = err * softwords_weight(cw, *pe) * PPCM / vnodes[*pe].nedges; @@ -349,6 +436,7 @@ struct ldpc_engine dtot -= d; } } + tots += dtot; #if 1 // Also update the codeword in-place. @@ -359,60 +447,87 @@ struct ldpc_engine } prev_err = err; } // c nodes + score_t maxs = -(1 << 30); - for (int v = 0; v < k; ++v) - if (score[v] > maxs) - maxs = score[v]; - if (!maxs) - break; - lfprintf(stderr, "maxs %d\n", (int)maxs); - // Try to flip each message bits with maximal score + for (int v = 0; v < k; ++v) { - if (score[v] < score_threshold) + if (score[v] > maxs) { + maxs = score[v]; + } + } + + if (!maxs) { + break; + } + + lfprintf(stderr, "maxs %d\n", (int)maxs); + // Try to flip each message bits with maximal score + + for (int v = 0; v < k; ++v) + { + if (score[v] < score_threshold) { continue; + } + // if ( score[v] < maxs*9/10 ) continue; - if (score[v] < maxs - 4) + if (score[v] < maxs - 4) { continue; + } + lfprintf(stderr, " flip %d score=%d\n", (int)v, (int)score[v]); // Update expected parities and scores that depend on them. score_t dtot = 0; + for (int commit = 0; commit <= 1; ++commit) { Taddr *pe = vnodes[v].edges; + for (int e = vnodes[v].nedges; e--; ++pe) { Taddr c = *pe; SOFTBIT was_bad = softwords_xor(expected, received, c); + if (softbit_harden(was_bad)) { Taddr *pe = cnodes[c].edges; + for (int e = cnodes[c].nedges; e--; ++pe) { int d = was_bad * softwords_weight(cw, *pe) * PPCM / vnodes[*pe].nedges; - if (commit) + + if (commit) { score[*pe] -= d; - else + } else { dtot -= d; + } } } + softwords_flip(expected, c); SOFTBIT is_bad = softwords_xor(expected, received, c); + if (softbit_harden(is_bad)) { Taddr *pe = cnodes[c].edges; + for (int e = cnodes[c].nedges; e--; ++pe) { int d = is_bad * softwords_weight(cw, *pe) * PPCM / vnodes[*pe].nedges; - if (commit) + + if (commit) { score[*pe] += d; - else + } else { dtot += d; + } } } - if (!commit) + + if (!commit) { softwords_flip(expected, c); + } } + if (!commit) { if (dtot >= 0) @@ -454,14 +569,17 @@ struct ldpc_engine { SOFTBIT sum; softbit_clear(&sum); + for (int i = 0; i < nwords; ++i) { SOFTWORD w = in[i]; + for (int b = 0; b < SWSIZE; ++b) { sum = softbit_xor(sum, softword_get(w, b)); softword_write(w, b, sum); } + out[i] = w; } } @@ -492,15 +610,18 @@ struct ldpc_engine { SOFTBIT prev; softbit_clear(&prev); + for (int i = 0; i < nwords; ++i, ++in, ++out) { SOFTWORD w = *in; + for (int b = 0; b < SWSIZE; ++b) { SOFTBIT n = softword_get(w, b); softword_write(w, b, softbit_xor(prev, n)); prev = n; } + *out = w; } } diff --git a/plugins/channelrx/demoddatv/leansdr/math.cpp b/plugins/channelrx/demoddatv/leansdr/math.cpp index 29131a192..afb21be40 100644 --- a/plugins/channelrx/demoddatv/leansdr/math.cpp +++ b/plugins/channelrx/demoddatv/leansdr/math.cpp @@ -48,9 +48,8 @@ unsigned char parity(uint64_t x) int log2i(uint64_t x) { int n = -1; - for (; x; ++n, x >>= 1) - ; + for (; x; ++n, x >>= 1); return n; } -} // leansdr \ No newline at end of file +} // leansdr diff --git a/plugins/channelrx/demoddatv/leansdr/math.h b/plugins/channelrx/demoddatv/leansdr/math.h index 64bcbaaca..0d33c552a 100644 --- a/plugins/channelrx/demoddatv/leansdr/math.h +++ b/plugins/channelrx/demoddatv/leansdr/math.h @@ -27,25 +27,31 @@ template struct complex { T re, im; + complex() {} + complex(T x) : re(x), im(0) {} complex(T x, T y) : re(x), im(y) {} + inline void operator+=(const complex &x) { re += x.re; im += x.im; } + inline void operator*=(const complex &c) { T tre = re * c.re - im * c.im; im = re * c.im + im * c.re; re = tre; } + inline void operator-=(const complex &x) { re-=x.re; im-=x.im; } + inline void operator*=(const T &k) { re *= k; @@ -86,8 +92,11 @@ template T dotprod(const T *u, const T *v, int n) { T acc = 0; - while (n--) + + while (n--) { acc += (*u++) * (*v++); + } + return acc; } @@ -101,8 +110,11 @@ template T cnorm2(const complex *p, int n) { T res = 0; - for (; n--; ++p) + + for (; n--; ++p) { res += cnorm2(*p); + } + return res; } @@ -110,8 +122,10 @@ T cnorm2(const complex *p, int n) template inline complex conjprod(const complex &u, const complex &v) { - return complex(u.re * v.re + u.im * v.im, - u.re * v.im - u.im * v.re); + return complex( + u.re * v.re + u.im * v.im, + u.re * v.im - u.im * v.re + ); } // Return sum(conj(u[i])*v[i]) @@ -119,8 +133,11 @@ template complex conjprod(const complex *u, const complex *v, int n) { complex acc = 0; - while (n--) + + while (n--) { acc += conjprod(*u++, *v++); + } + return acc; } @@ -140,6 +157,7 @@ int log2i(uint64_t x); struct trig16 { complex lut[65536]; // TBD static and shared + trig16() { for (int a = 0; a < 65536; ++a) @@ -149,10 +167,12 @@ struct trig16 lut[a].im = sinf(af); } } + inline const complex &expi(uint16_t a) const { return lut[a]; } + // a must fit in a int32_t, otherwise behaviour is undefined inline const complex &expi(float a) const { @@ -162,7 +182,8 @@ struct trig16 // Modulo with signed result in [-m/2..m/2[ -inline float fmodfs(float v, float m) { +inline float fmodfs(float v, float m) +{ v = fmodf(v, m); return (v>=m/2) ? v-m : (v<-m/2) ? v+m : v; } @@ -170,22 +191,41 @@ inline float fmodfs(float v, float m) { // Simple statistics template -struct statistics { - statistics() { reset(); } - void reset() { vm1=vm2=0; count=0; vmin=vmax=99;/*comp warning*/ } - void add(const T &v) { +struct statistics +{ + statistics() { + reset(); + } + + void reset() + { + vm1 = vm2 = 0; + count = 0; + vmin = vmax = 99;/*comp warning*/ + } + + void add(const T &v) + { vm1 += v; vm2 += v*v; - if ( count == 0 ) { vmin = vmax = v; } - else if ( v < vmin ) { vmin = v; } - else if ( v > vmax ) { vmax = v; } + + if ( count == 0 ) { + vmin = vmax = v; + } else if ( + v < vmin ) { vmin = v; + } else if ( v > vmax ) { + vmax = v; + } + ++count; } + T average() { return vm1 / count; } T variance() { return vm2/count - (vm1/count)*(vm1/count); } T stddev() { return gen_sqrt(variance()); } T min() { return vmin; } T max() { return vmax; } + private: T vm1, vm2; // Moments T vmin, vmax; // Range diff --git a/plugins/channelrx/demoddatv/leansdr/rs.h b/plugins/channelrx/demoddatv/leansdr/rs.h index 7af6746e4..c3c7b8ccc 100644 --- a/plugins/channelrx/demoddatv/leansdr/rs.h +++ b/plugins/channelrx/demoddatv/leansdr/rs.h @@ -47,50 +47,65 @@ namespace leansdr template struct gf2x_p { + static const Te alpha = ALPHA; + gf2x_p() { - if (ALPHA != 2) + if (ALPHA != 2) { fail("alpha!=2 not implemented"); + } + // Precompute log and exp tables. Tp alpha_i = 1; + for (Tp i = 0; i < (1 << N); ++i) { lut_exp[i] = alpha_i; lut_exp[((1 << N) - 1) + i] = alpha_i; lut_log[alpha_i] = i; alpha_i <<= 1; // Multiply by alpha=[X] i.e. increase degrees - if (alpha_i & (1 << N)) + + if (alpha_i & (1 << N)) { alpha_i ^= P; // Modulo P iteratively + } } } - static const Te alpha = ALPHA; + inline Te add(Te x, Te y) { return x ^ y; } // Addition modulo 2 inline Te sub(Te x, Te y) { return x ^ y; } // Subtraction modulo 2 + inline Te mul(Te x, Te y) { - if (!x || !y) + if (!x || !y) { return 0; + } + return lut_exp[lut_log[x] + lut_log[y]]; } + inline Te div(Te x, Te y) { //if ( ! y ) fail("div"); // TODO - if (!x) + if (!x) { return 0; + } + return lut_exp[lut_log[x] + ((1 << N) - 1) - lut_log[y]]; } + inline Te inv(Te x) { // if ( ! x ) fail("inv"); return lut_exp[((1 << N) - 1) - lut_log[x]]; } + inline Te exp(Te x) { return lut_exp[x]; } inline Te log(Te x) { return lut_log[x]; } private: Te lut_exp[(1 << N) * 2]; // Wrap to avoid indexing modulo 2^N-1 Te lut_log[1 << N]; -}; +}; // gf2x_p // Reed-Solomon for RS(204,188) shortened from RS(255,239). @@ -99,21 +114,23 @@ struct rs_engine // EN 300 421, section 4.4.2, Field Generator Polynomial // p(X) = X^8 + X^4 + X^3 + X^2 + 1 gf2x_p gf; - u8 G[17]; // { G_16, ..., G_0 } rs_engine() { // EN 300 421, section 4.4.2, Code Generator Polynomial // G(X) = (X-alpha^0)*...*(X-alpha^15) - for (int i = 0; i <= 16; ++i) + for (int i = 0; i <= 16; ++i) { G[i] = (i == 16) ? 1 : 0; // Init G=1 + } + for (int d = 0; d < 16; ++d) { // Multiply by (X-alpha^d) // G := X*G - alpha^d*G - for (int i = 0; i <= 16; ++i) + for (int i = 0; i <= 16; ++i) { G[i] = gf.sub((i == 16) ? 0 : G[i + 1], gf.mul(gf.exp(d), G[i])); + } } #if DEBUG_RS fprintf(stderr, "RS generator:"); @@ -132,20 +149,28 @@ struct rs_engine bool syndromes(const u8 *poly, u8 *synd) { bool corrupted = false; + for (int i = 0; i < 16; ++i) { synd[i] = eval_poly_rev(poly, 204, gf.exp(i)); - if (synd[i]) + + if (synd[i]) { corrupted = true; + } } + return corrupted; } + u8 eval_poly_rev(const u8 *poly, int n, u8 x) { // poly[0]*x^(n-1) + .. + poly[n-1]*x^0 with Hörner method. u8 acc = 0; - for (int i = 0; i < n; ++i) + + for (int i = 0; i < n; ++i) { acc = gf.add(gf.mul(acc, x), poly[i]); + } + return acc; } @@ -154,8 +179,11 @@ struct rs_engine { // poly[0]*x^0 + .. + poly[deg]*x^deg with Hörner method. u8 acc = 0; - for (; deg >= 0; --deg) + + for (; deg >= 0; --deg) { acc = gf.add(gf.mul(acc, x), poly[deg]); + } + return acc; } @@ -178,12 +206,15 @@ struct rs_engine for (int d = 0; d < 188; ++d) { // Clear monomial of degree d - if (!p[d]) + if (!p[d]) { continue; + } + u8 k = gf.div(p[d], G[0]); // p(X) := p(X) - k*G(X)*X^(188-d) - for (int i = 0; i <= 16; ++i) + for (int i = 0; i <= 16; ++i) { p[d + i] = gf.sub(p[d + i], gf.mul(k, G[i])); + } } #if DEBUG_RS fprintf(stderr, "coded:"); @@ -198,8 +229,12 @@ struct rs_engine // If pin[] is provided, errors will be fixed in the original // message too and syndromes will be updated. - bool correct(u8 synd[16], u8 pout[188], - u8 pin[204] = NULL, int *bits_corrected = NULL) + bool correct( + u8 synd[16], + u8 pout[188], + u8 pin[204] = nullptr, + int *bits_corrected = nullptr + ) { // Berlekamp - Massey // http://en.wikipedia.org/wiki/Berlekamp%E2%80%93Massey_algorithm#Code_sample @@ -208,11 +243,15 @@ struct rs_engine int L = 0; int m = 1; u8 b = 1; + for (int n = 0; n < 16; ++n) { u8 d = synd[n]; - for (int i = 1; i <= L; ++i) + + for (int i = 1; i <= L; ++i) { d ^= gf.mul(C[i], synd[n - i]); + } + if (!d) { ++m; @@ -221,8 +260,11 @@ struct rs_engine { u8 T[16]; memcpy(T, C, sizeof(T)); - for (int i = 0; i < 16 - m; ++i) + + for (int i = 0; i < 16 - m; ++i) { C[m + i] ^= gf.mul(d, gf.mul(gf.inv(b), B[i])); + } + L = n + 1 - L; memcpy(B, T, sizeof(B)); b = d; @@ -230,8 +272,10 @@ struct rs_engine } else { - for (int i = 0; i < 16 - m; ++i) + for (int i = 0; i < 16 - m; ++i) { C[m + i] ^= gf.mul(d, gf.mul(gf.inv(b), B[i])); + } + ++m; } } @@ -254,11 +298,17 @@ struct rs_engine // Compute Omega u8 omega[16]; memset(omega, 0, sizeof(omega)); + // TODO loops for (int i = 0; i < 16; ++i) + { for (int j = 0; j < 16; ++j) - if (i + j < 16) + { + if (i + j < 16) { omega[i + j] ^= gf.mul(synd[i], C[j]); + } + } + } #if DEBUG_RS fprintf(stderr, "omega="); for (int i = 0; i < 16; ++i) @@ -268,8 +318,10 @@ struct rs_engine // Compute Lambda' u8 Cprime[15]; - for (int i = 0; i < 15; ++i) + + for (int i = 0; i < 15; ++i) { Cprime[i] = (i & 1) ? 0 : C[i + 1]; + } #if DEBUG_RS fprintf(stderr, "Cprime="); for (int i = 0; i < 15; ++i) @@ -280,10 +332,12 @@ struct rs_engine // Find zeroes of C by exhaustive search? // TODO Chien method int roots_found = 0; + for (int i = 0; i < 255; ++i) { u8 r = gf.exp(i); // Candidate root alpha^0..alpha^254 u8 v = eval_poly(C, L, r); + if (!v) { // r is a root X_k^-1 of the error locator polynomial. @@ -298,25 +352,35 @@ struct rs_engine u8 num = gf.mul(xk, eval_poly(omega, L, r)); u8 den = eval_poly(Cprime, 14, r); u8 e = gf.div(num, den); + // Subtract e from coefficient of degree loc. // Note: Coeffients listed by decreasing degree in pin[] and pout[]. - if (bits_corrected) + if (bits_corrected) { *bits_corrected += hamming_weight(e); - if (loc >= 16) + } + + if (loc >= 16) { pout[203 - loc] ^= e; - if (pin) + } + + if (pin) { pin[203 - loc] ^= e; + } } - if (++roots_found == L) + + if (++roots_found == L) { break; + } } } - if (pin) + + if (pin) { return syndromes(pin, synd); - else + } else { return false; + } } -}; +}; // rs_engine } // namespace leansdr diff --git a/plugins/channelrx/demoddatv/leansdr/sdr.cpp b/plugins/channelrx/demoddatv/leansdr/sdr.cpp index 42721b642..79f6f8f9d 100644 --- a/plugins/channelrx/demoddatv/leansdr/sdr.cpp +++ b/plugins/channelrx/demoddatv/leansdr/sdr.cpp @@ -19,8 +19,9 @@ const char *cstln_base::names[] = void softsymb_harden(llr_ss *ss) { - for (int b = 0; b < 8; ++b) + for (int b = 0; b < 8; ++b) { ss->bits[b] = (ss->bits[b] < 0) ? -127 : 127; + } } void softsymb_harden(hard_ss *ss) @@ -30,8 +31,9 @@ void softsymb_harden(hard_ss *ss) void softsymb_harden(eucl_ss *ss) { - for (int s = 0; s < ss->MAX_SYMBOLS; ++s) + for (int s = 0; s < ss->MAX_SYMBOLS; ++s) { ss->dists2[s] = (s == ss->nearest) ? 0 : 1; + } } @@ -59,8 +61,9 @@ void to_softsymb(const full_ss *fss, hard_ss *ss) void to_softsymb(const full_ss *fss, eucl_ss *ss) { - for (int s = 0; s < ss->MAX_SYMBOLS; ++s) + for (int s = 0; s < ss->MAX_SYMBOLS; ++s) { ss->dists2[s] = fss->dists2[s]; + } uint16_t best = 65535, best2 = 65535; @@ -87,10 +90,15 @@ void to_softsymb(const full_ss *fss, llr_ss *ss) { float v = (1.0f - fss->p[b]) / (fss->p[b] + 1e-6); int r = logf(v) * 5; // TBD Optimal scaling vs saturation ? - if (r < -127) + + if (r < -127) { r = -127; - if (r > 127) + } + + if (r > 127) { r = 127; + } + ss->bits[b] = r; } } diff --git a/plugins/channelrx/demoddatv/leansdr/sdr.h b/plugins/channelrx/demoddatv/leansdr/sdr.h index 472e8fca0..d0679b209 100644 --- a/plugins/channelrx/demoddatv/leansdr/sdr.h +++ b/plugins/channelrx/demoddatv/leansdr/sdr.h @@ -115,17 +115,24 @@ struct auto_notch : runnable data[i].re = pin[i].re; data[i].im = pin[i].im; m2 += (float)pin[i].re * pin[i].re + (float)pin[i].im * pin[i].im; - if (gen_abs(pin[i].re) > m0) + + if (gen_abs(pin[i].re) > m0) { m0 = gen_abs(pin[i].re); - if (gen_abs(pin[i].im) > m0) + } + + if (gen_abs(pin[i].im) > m0) { m0 = gen_abs(pin[i].im); + } } if (agc_rms_setpoint && m2) { float rms = gen_sqrt(m2 / fft.size()); - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "(pow %f max %f)", rms, m0); + } + float new_gain = agc_rms_setpoint / rms; gain = gain * 0.9 + new_gain * 0.1; } @@ -133,21 +140,26 @@ struct auto_notch : runnable fft.inplace(data, true); float *amp = new float[fft.size()]; - for (int i = 0; i < fft.size(); ++i) + for (int i = 0; i < fft.size(); ++i) { amp[i] = hypotf(data[i].re, data[i].im); + } for (slot *s = __slots; s < __slots + nslots; ++s) { int iamax = 0; for (int i = 0; i < fft.size(); ++i) - if (amp[i] > amp[iamax]) + { + if (amp[i] > amp[iamax]) { iamax = i; + } + } if (iamax != s->i) { - if (sch->debug) + if (sch->debug) { fprintf(stderr, "%s: slot %d new peak %d -> %d\n", name, (int)(s - __slots), s->i, iamax); + } s->i = iamax; s->estim.re = 0; @@ -164,11 +176,13 @@ struct auto_notch : runnable amp[iamax] = 0; - if (iamax - 1 >= 0) + if (iamax - 1 >= 0) { amp[iamax - 1] = 0; + } - if (iamax + 1 < fft.size()) + if (iamax + 1 < fft.size()) { amp[iamax + 1] = 0; + } } delete[] amp; @@ -179,8 +193,9 @@ struct auto_notch : runnable { complex *pin = in.rd(), *pend = pin + fft.size(), *pout = out.wr(); - for (slot *s = __slots; s < __slots + nslots; ++s) + for (slot *s = __slots; s < __slots + nslots; ++s) { s->ej = s->expj; + } for (; pin < pend; ++pin, ++pout) { @@ -189,13 +204,16 @@ struct auto_notch : runnable for (slot *s = __slots; s < __slots + nslots; ++s->ej, ++s) { - complex bb(pin->re * s->ej->re + pin->im * s->ej->im, - -pin->re * s->ej->im + pin->im * s->ej->re); + complex bb( + pin->re * s->ej->re + pin->im * s->ej->im, + -pin->re * s->ej->im + pin->im * s->ej->re + ); s->estim.re = bb.re * k + s->estim.re * (1 - k); s->estim.im = bb.im * k + s->estim.im * (1 - k); complex sub( s->estim.re * s->ej->re - s->estim.im * s->ej->im, - s->estim.re * s->ej->im + s->estim.im * s->ej->re); + s->estim.re * s->ej->im + s->estim.im * s->ej->re + ); out.re -= sub.re; out.im -= sub.im; } @@ -237,7 +255,8 @@ struct ss_estimator : runnable ss_estimator( scheduler *sch, - pipebuf> &_in, pipebuf &_out + pipebuf> &_in, + pipebuf &_out ) : runnable(sch, "SS estimator"), window_size(1024), @@ -260,11 +279,13 @@ struct ss_estimator : runnable complex *p = in.rd(), *pend = p + window_size; float s = 0; - for (; p < pend; ++p) + for (; p < pend; ++p) { s += (float)p->re * p->re + (float)p->im * p->im; + } out.write(sqrtf(s / window_size)); } + in.read(window_size); } } @@ -317,10 +338,14 @@ struct ss_amp_estimator : runnable float mag2 = (float)p->re * p->re + (float)p->im * p->im; s2 += mag2; float mag = sqrtf(mag2); - if (mag < amin) + + if (mag < amin) { amin = mag; - if (mag > amax) + } + + if (mag > amax) { amax = mag; + } } out_ss.write(sqrtf(s2 / window_size)); @@ -373,13 +398,15 @@ struct simple_agc : runnable complex *pin = in.rd(), *pend = pin + chunk_size; float amp2 = 0; - for (; pin < pend; ++pin) + for (; pin < pend; ++pin) { amp2 += pin->re * pin->re + pin->im * pin->im; + } amp2 /= chunk_size; - if (!estimated) + if (!estimated) { estimated = amp2; + } estimated = estimated * (1 - bw) + amp2 * bw; float gain = estimated ? out_rms / sqrtf(estimated) : 0; @@ -746,8 +773,10 @@ struct cstln_lut : cstln_base complex polar(float r, int n, float i) { float a = i * 2 * M_PI / n; - return complex(r * cosf(a) * cstln_amp, - r * sinf(a) * cstln_amp); + return complex( + r * cosf(a) * cstln_amp, + r * sinf(a) * cstln_amp + ); } // Helper function for some constellation tables @@ -758,8 +787,10 @@ struct cstln_lut : cstln_base for (int j = 0; j < 4; ++j) { float phi = a[j] * M_PI; - symbols[i + j] = complex(r * cosf(phi) * cstln_amp, - r * sinf(phi) * cstln_amp); + symbols[i + j] = complex( + r * cosf(phi) * cstln_amp, + r * sinf(phi) * cstln_amp + ); } } @@ -806,8 +837,9 @@ struct cstln_lut : cstln_base // Shared scope so that we don't have to reset dists2[nsymbols..] to -1. struct full_ss fss; - for (int s = 0; s < 256; ++s) + for (int s = 0; s < 256; ++s) { fss.dists2[s] = -1; + } for (int I = -R / 2; I < R / 2; ++I) { @@ -831,14 +863,14 @@ struct cstln_lut : cstln_base { float d2 = ((I - symbols[s].re) * (I - symbols[s].re) + (Q - symbols[s].im) * (Q - symbols[s].im)); - if (d2 < fss.dists2[fss.nearest]) + if (d2 < fss.dists2[fss.nearest]) { fss.nearest = s; + } fss.dists2[s] = d2; float p = expf(-d2 / (2 * sigma * sigma)) / (sqrtf(2 * M_PI) * sigma); - for (int bit = 0; bit < 8; ++bit) - { + for (int bit = 0; bit < 8; ++bit) { probs[bit][(s >> bit) & 1] += p; } } @@ -847,9 +879,12 @@ struct cstln_lut : cstln_base for (int b = 0; b < 8; ++b) { float p = probs[b][1] / (probs[b][0] + probs[b][1]); + // Avoid trouble when sigma is unrealistically low. - if (!isnormal(p)) + if (!isnormal(p)) { p = 0; + } + fss.p[b] = p; } @@ -857,8 +892,10 @@ struct cstln_lut : cstln_base to_softsymb(&fss, &pr->ss); // Always record nearest symbol and phase error for C&T. pr->symbol = fss.nearest; - float ph_symbol = atan2f(symbols[pr->symbol].im, - symbols[pr->symbol].re); + float ph_symbol = atan2f( + symbols[pr->symbol].im, + symbols[pr->symbol].re + ); float ph_err = atan2f(Q, I) - ph_symbol; pr->phase_error = (int32_t)(ph_err * 65536 / (2 * M_PI)); // Mod 65536 } @@ -879,15 +916,19 @@ struct cstln_lut : cstln_base { result *pr = &lut[I & (R - 1)][Q & (R - 1)]; uint8_t v; - if (bit < bps) + + if (bit < bps) { v = softsymb_to_dump(pr->ss, bit); - else + } else { v = 128 + pr->phase_error / 64; + } + // Highlight the constellation symbols. for (int s = 0; s < nsymbols; ++s) { - if (symbols[s].re == I && symbols[s].im == Q) + if (symbols[s].re == I && symbols[s].im == Q) { v ^= 128; + } } fputc(v, f); @@ -901,8 +942,9 @@ struct cstln_lut : cstln_base { for (int i = 0; i < R; ++i) { - for (int q = 0; q < R; ++q) + for (int q = 0; q < R; ++q) { softsymb_harden(&lut[i][q].ss); + } } } @@ -917,9 +959,9 @@ struct cstln_lut : cstln_base template struct sampler_interface { - virtual ~sampler_interface() - { + virtual ~sampler_interface() { } + virtual complex interp(const complex *pin, float mu, float phase) = 0; virtual void update_freq(float freqw, int weight = 0) @@ -927,6 +969,7 @@ struct sampler_interface (void) freqw; (void) weight; } // 65536 = 1 Hz + virtual int readahead() = 0; }; @@ -936,8 +979,7 @@ struct sampler_interface template struct nearest_sampler : sampler_interface { - int readahead() - { + int readahead() { return 0; } @@ -957,8 +999,7 @@ struct nearest_sampler : sampler_interface template struct linear_sampler : sampler_interface { - int readahead() - { + int readahead() { return 1; } @@ -998,13 +1039,12 @@ struct fir_sampler : sampler_interface do_update_freq(0); // In case application never calls update_freq() } - ~fir_sampler() + virtual ~fir_sampler() { delete[] shifted_coeffs; } - int readahead() - { + int readahead() { return ncoeffs - 1; } @@ -1020,15 +1060,17 @@ struct fir_sampler : sampler_interface // Special case for heavily oversampled signals, // where filtering is expensive. // gcc-4.9.2 can vectorize this form with NEON on ARM. - while (pc < pcend) + while (pc < pcend) { acc += (*pc++) * (*pin++); + } } else { // Not vectorized because the coefficients are not // guaranteed to be contiguous in memory. - for (; pc < pcend; pc += subsampling, ++pin) + for (; pc < pcend; pc += subsampling, ++pin) { acc += (*pc) * (*pin); + } } // Derotate @@ -1055,8 +1097,10 @@ struct fir_sampler : sampler_interface void do_update_freq(float freqw) { float f = freqw / subsampling; - for (int i = 0; i < ncoeffs; ++i) + + for (int i = 0; i < ncoeffs; ++i) { shifted_coeffs[i] = trig.expi(-f * (i - ncoeffs / 2)) * coeffs[i]; + } } trig16 trig; @@ -1199,15 +1243,16 @@ struct cstln_receiver : runnable void run() { - if (!cstln) + if (!cstln) { fail("constellation not set"); + } // Magic constants that work with the qa recordings. float freq_alpha = 0.04; float freq_beta = 0.0012 / omega * pll_adjustment; float gain_mu = 0.02 / (cstln_amp * cstln_amp) * 2; - int max_meas = chunk_size / meas_decimation + 1; + // Large margin on output_size because mu adjustments // can lead to more than chunk_size/min_omega symbols. while (in.readable() >= chunk_size + sampler->readahead() && @@ -1259,14 +1304,20 @@ struct cstln_receiver : runnable cstln_point = &cstln->symbols[cr->symbol]; hist[0].c.re = cstln_point->re; hist[0].c.im = cstln_point->im; - float muerr = ((hist[0].p.re - hist[2].p.re) * hist[1].c.re + (hist[0].p.im - hist[2].p.im) * hist[1].c.im) - ((hist[0].c.re - hist[2].c.re) * hist[1].p.re + (hist[0].c.im - hist[2].c.im) * hist[1].p.im); + float muerr = ((hist[0].p.re - hist[2].p.re) * hist[1].c.re + (hist[0].p.im - hist[2].p.im) * hist[1].c.im) + - ((hist[0].c.re - hist[2].c.re) * hist[1].p.re + (hist[0].c.im - hist[2].c.im) * hist[1].p.im); float mucorr = muerr * gain_mu; const float max_mucorr = 0.1; + // TBD Optimize out statically - if (mucorr < -max_mucorr) + if (mucorr < -max_mucorr) { mucorr = -max_mucorr; - if (mucorr > max_mucorr) + } + + if (mucorr > max_mucorr) { mucorr = max_mucorr; + } + mu += mucorr; mu += omega; // Next symbol time; } // mu<1 @@ -1288,8 +1339,9 @@ struct cstln_receiver : runnable if (cstln_point) { // Output the last interpolated PSK symbol, max once per chunk_size - if (cstln_out) + if (cstln_out) { cstln_out->write(s); + } // AGC // For APSK we must do AGC on the symbols, not the whole signal. @@ -1297,12 +1349,15 @@ struct cstln_receiver : runnable float insp = sg.re * sg.re + sg.im * sg.im; est_insp = insp * kest + est_insp * (1 - kest); - if (est_insp) + if (est_insp) { agc_gain = cstln_amp / gen_sqrt(est_insp); + } // SS and MER - complex ev(s.re - cstln_point->re, - s.im - cstln_point->im); + complex ev( + s.re - cstln_point->re, + s.im - cstln_point->im + ); float sig_power, ev_power; if (cstln->nsymbols == 2) @@ -1329,8 +1384,9 @@ struct cstln_receiver : runnable if (!allow_drift) { - if (freqw < min_freqw || freqw > max_freqw) + if (freqw < min_freqw || freqw > max_freqw) { freqw = (max_freqw + min_freqw) / 2; + } } // Output measurements @@ -1342,12 +1398,18 @@ struct cstln_receiver : runnable while (meas_count >= meas_decimation) { meas_count -= meas_decimation; - if (freq_out) + + if (freq_out) { freq_out->write(freq_tap); - if (ss_out) + } + + if (ss_out) { ss_out->write(sqrtf(est_insp)); - if (mer_out) + } + + if (mer_out) { mer_out->write(est_ep ? 10 * log10f(est_sp / est_ep) : 0); + } } } // Work to do @@ -1462,8 +1524,9 @@ struct fast_qpsk_receiver : runnable signed long freq_alpha = 0.04 * 65536; signed long freq_beta = 0.0012 * 256 * 65536 / omega * pll_adjustment; - if (!freq_beta) + if (!freq_beta) { fail("Excessive oversampling"); + } float gain_mu = 0.02 / (cstln_amp * cstln_amp) * 2; @@ -1562,11 +1625,16 @@ struct fast_qpsk_receiver : runnable #endif float mucorr = muerr * gain_mu; const float max_mucorr = 0.1; + // TBD Optimize out statically - if (mucorr < -max_mucorr) + if (mucorr < -max_mucorr) { mucorr = -max_mucorr; - if (mucorr > max_mucorr) + } + + if (mucorr > max_mucorr) { mucorr = max_mucorr; + } + mu += mucorr; mu += omega; // Next symbol time; } // mu<1 @@ -1580,17 +1648,19 @@ struct fast_qpsk_receiver : runnable in.read(pin - pin0); out.written(pout - pout0); - if (symbol_arg && cstln_out) + if (symbol_arg && cstln_out) { // Output the last interpolated PSK symbol, max once per chunk_size cstln_out->write(s); + } // This is best done periodically ouside the inner loop, // but will cause non-deterministic output. if (!allow_drift) { - if (freqw < min_freqw || freqw > max_freqw) + if (freqw < min_freqw || freqw > max_freqw) { freqw = (max_freqw + min_freqw) / 2; + } } // Output measurements @@ -1601,8 +1671,9 @@ struct fast_qpsk_receiver : runnable { meas_count -= meas_decimation; - if (freq_out) + if (freq_out) { freq_out->write((float)freqw / 65536); + } } } // Work to do @@ -1702,8 +1773,9 @@ struct cstln_transmitter : runnable void run() { - if (!cstln) + if (!cstln) { fail("constellation not set"); + } int count = min(in.readable(), out.writable()); u8 *pin = in.rd(), *pend = pin + count; @@ -1745,8 +1817,10 @@ struct rotator : runnable index(0) { int ifreq = freq * 65536; - if (sch->debug) + + if (sch->debug) { fprintf(stderr, "Rotate: req=%f real=%f\n", freq, ifreq / 65536.0); + } for (int i = 0; i < 65536; ++i) { @@ -2038,15 +2112,17 @@ struct spectrum : runnable { complex *pin = in.rd(); - for (int i = 0; i < fft.n; ++i, pin += decim) + for (int i = 0; i < fft.n; ++i, pin += decim) { data[i] = *pin; + } } fft.inplace(data, true); float power[NFFT]; - for (int i = 0; i < fft.n; ++i) + for (int i = 0; i < fft.n; ++i) { power[i] = (float)data[i].re * data[i].re + (float)data[i].im * data[i].im; + } if (!avgpower) { diff --git a/plugins/channelrx/demoddatv/leansdr/softword.h b/plugins/channelrx/demoddatv/leansdr/softword.h index e38b8a9dd..b78908e36 100644 --- a/plugins/channelrx/demoddatv/leansdr/softword.h +++ b/plugins/channelrx/demoddatv/leansdr/softword.h @@ -24,47 +24,58 @@ inline bool softword_get(const hard_sb &p, int b) { return p & (1 << (7 - b)); } + inline void softword_set(hard_sb *p, int b, bool v) { hard_sb mask = 1 << (7 - b); *p = ((*p) & ~mask) | (v << (7 - b)); } + inline void softword_clear(hard_sb *p) { *p = 0; } + inline bool softword_weight(const bool &l) { (void) l; return true; } + inline void softbit_set(bool *p, bool v) { *p = v; } inline bool softbit_harden(bool b) { return b; } inline uint8_t softbyte_harden(const hard_sb &b) { return b; } inline bool softbit_xor(bool x, bool y) { return x ^ y; } inline void softbit_clear(bool *p) { *p = false; } + inline bool softwords_xor(const hard_sb p1[], const hard_sb p2[], int b) { return (p1[b / 8] ^ p2[b / 8]) & (1 << (7 - (b & 7))); } + inline void softword_zero(hard_sb *p) { *p = 0; } + inline void softwords_set(hard_sb p[], int b) { p[b / 8] |= 1 << (7 - (b & 7)); } + inline void softword_write(hard_sb &p, int b, bool v) { hard_sb mask = 1 << (7 - b); p = (p & ~mask) | (hard_sb)v << (7 - b); } + inline void softwords_write(hard_sb p[], int b, bool v) { softword_write(p[b / 8], b & 7, v); } + inline void softwords_flip(hard_sb p[], int b) { p[b / 8] ^= 1 << (7 - (b & 7)); } + uint8_t *softbytes_harden(hard_sb p[], int nbytes, uint8_t storage[]) { (void) nbytes; @@ -83,15 +94,22 @@ float prob(llr_t l) { return (127.0 + l) / 254; } + llr_t llr(float p) { int r = -127 + 254 * p; - if (r < -127) + + if (r < -127) { r = -127; - if (r > 127) + } + + if (r > 127) { r = 127; + } + return r; } + inline llr_t llr_xor(llr_t lx, llr_t ly) { float px = prob(lx), py = prob(ly); @@ -102,25 +120,31 @@ inline llr_t softword_get(const llr_sb &p, int b) { return p.bits[b]; } + // Special case to avoid computing b/8*8+b%7. Assumes llr_sb[] packed. inline llr_t softwords_get(const llr_sb p[], int b) { return p[0].bits[b]; // Beyond bounds on purpose } + inline void softword_set(llr_sb *p, int b, llr_t v) { p->bits[b] = v; } + inline void softword_clear(llr_sb *p) { memset(p->bits, 0, sizeof(p->bits)); } + // inline llr_t softwords_get(const llr_sb p[], int b) { // return softword_get(p[b/8], b&7); // } + inline llr_t softword_weight(llr_t l) { return abs(l); } inline void softbit_set(llr_t *p, bool v) { *p = v ? -127 : 127; } inline bool softbit_harden(llr_t l) { return (l < 0); } + inline uint8_t softbyte_harden(const llr_sb &b) { // Without conditional jumps @@ -134,37 +158,47 @@ inline uint8_t softbyte_harden(const llr_sb &b) ((b.bits[7] & 128) >> 7)); return r; } + inline llr_t softbit_xor(llr_t x, llr_t y) { return llr_xor(x, y); } inline void softbit_clear(llr_t *p) { *p = -127; } + inline llr_t softwords_xor(const llr_sb p1[], const llr_sb p2[], int b) { return llr_xor(p1[b / 8].bits[b & 7], p2[b / 8].bits[b & 7]); } + inline void softword_zero(llr_sb *p) { memset(p, -127, sizeof(*p)); } + inline void softwords_set(llr_sb p[], int b) { p[b / 8].bits[b & 7] = 127; } + inline void softword_write(llr_sb &p, int b, llr_t v) { p.bits[b] = v; } + inline void softwords_write(llr_sb p[], int b, llr_t v) { softword_write(p[b / 8], b & 7, v); } + inline void softwords_flip(llr_sb p[], int b) { llr_t *l = &p[b / 8].bits[b & 7]; *l = -*l; } + uint8_t *softbytes_harden(llr_sb p[], int nbytes, uint8_t storage[]) { - for (uint8_t *q = storage; nbytes--; ++p, ++q) + for (uint8_t *q = storage; nbytes--; ++p, ++q) { *q = softbyte_harden(*p); + } + return storage; } @@ -175,11 +209,13 @@ inline SOFTBIT softwords_get(const SOFTBYTE p[], int b) { return softword_get(p[b / 8], b & 7); } + template inline void softwords_set(SOFTBYTE p[], int b, SOFTBIT v) { softword_set(&p[b / 8], b & 7, v); } + template inline SOFTBIT softwords_weight(const SOFTBYTE p[], int b) { diff --git a/plugins/channelrx/demoddatv/leansdr/viterbi.h b/plugins/channelrx/demoddatv/leansdr/viterbi.h index 4e3e50699..ae4f6c005 100644 --- a/plugins/channelrx/demoddatv/leansdr/viterbi.h +++ b/plugins/channelrx/demoddatv/leansdr/viterbi.h @@ -57,17 +57,20 @@ struct trellis trellis() { for (TS s = 0; s < NSTATES; ++s) - for (int cs = 0; cs < NCS; ++cs) + { + for (int cs = 0; cs < NCS; ++cs) { states[s].branches[cs].pred = NOSTATE; + } + } } // TBD Polynomial width should be a template parameter ? void init_convolutional(const uint16_t G[]) { - if (NCS & (NCS - 1)) - { + if (NCS & (NCS - 1)) { fprintf(stderr, "NCS must be a power of 2\n"); } + // Derive number of polynomials from NCS. int nG = log2i(NCS); @@ -79,20 +82,29 @@ struct trellis uint64_t shiftreg = s; // TBD type // Reverse bits TUS us_rev = 0; + for (int b = 1; b < NUS; b *= 2) - if (us & b) + { + if (us & b) { us_rev |= (NUS / 2 / b); + } + } + shiftreg |= us_rev * NSTATES; uint32_t cs = 0; // TBD type - for (int g = 0; g < nG; ++g) + + for (int g = 0; g < nG; ++g) { cs = (cs << 1) | parity(shiftreg & G[g]); + } + shiftreg /= NUS; // Shift bits for 1 uncoded symbol // [us] at state [s] emits [cs] and leads to state [shiftreg]. typename state::branch *b = &states[shiftreg].branches[cs]; - if (b->pred != NOSTATE) - { + + if (b->pred != NOSTATE) { fprintf(stderr, "Invalid convolutional code\n"); } + b->pred = s; b->us = us; } @@ -104,14 +116,18 @@ struct trellis for (int s = 0; s < NSTATES; ++s) { fprintf(stderr, "State %02x:", s); + for (int cs = 0; cs < NCS; ++cs) { typename state::branch *b = &states[s].branches[cs]; - if (b->pred == NOSTATE) + + if (b->pred == NOSTATE) { fprintf(stderr, " - "); - else + } else { fprintf(stderr, " %02x+%x", b->pred, b->us); + } } + fprintf(stderr, "\n"); } } @@ -125,8 +141,8 @@ template TPM cost; // Metric of best path leading to this state TP path; // Best path leading to this state }; + typedef state statebank[NSTATES]; state statebanks[2][NSTATES]; statebank *states, *newstates; // Alternate between banks @@ -152,46 +169,57 @@ struct viterbi_dec : viterbi_dec_interface { states = &statebanks[0]; newstates = &statebanks[1]; - for (TS s = 0; s < NSTATES; ++s) + + for (TS s = 0; s < NSTATES; ++s) { (*states)[s].cost = 0; + } + // Determine max value that can fit in TPM max_tpm = (TPM)0 - 1; + if (max_tpm < 0) { // TPM is signed - for (max_tpm = 0; max_tpm * 2 + 1 > max_tpm; max_tpm = max_tpm * 2 + 1) - ; + for (max_tpm = 0; max_tpm * 2 + 1 > max_tpm; max_tpm = max_tpm * 2 + 1); } } // Update with full metric - TUS update(TBM costs[NCS], TPM *quality = NULL) + TUS update(TBM costs[NCS], TPM *quality = nullptr) { TPM best_tpm = max_tpm, best2_tpm = max_tpm; TS best_state = 0; + // Update all states for (int s = 0; s < NSTATES; ++s) { TPM best_m = max_tpm; - typename trellis::state::branch *best_b = NULL; + typename trellis::state::branch *best_b = nullptr; + // Select best branch for (int cs = 0; cs < NCS; ++cs) { typename trellis::state::branch *b = &trell->states[s].branches[cs]; - if (b->pred == trell->NOSTATE) + + if (b->pred == trell->NOSTATE) { continue; + } + TPM m = (*states)[b->pred].cost + costs[cs]; + if (m <= best_m) { // <= guarantees one match best_m = m; best_b = b; } } + (*newstates)[s].path = (*states)[best_b->pred].path; (*newstates)[s].path.append(best_b->us); (*newstates)[s].cost = best_m; + // Select best and second-best states if (best_m < best_tpm) { @@ -200,7 +228,9 @@ struct viterbi_dec : viterbi_dec_interface best_tpm = best_m; } else if (best_m < best2_tpm) + { best2_tpm = best_m; + } } // Swap banks { @@ -208,9 +238,11 @@ struct viterbi_dec : viterbi_dec_interface states = newstates; newstates = tmp; } + // Prevent overflow of path metrics - for (TS s = 0; s < NSTATES; ++s) + for (TS s = 0; s < NSTATES; ++s) { (*states)[s].cost -= best_tpm; + } #if 0 // Observe that the min-max range remains bounded fprintf(stderr,"-%2d = [", best_tpm); @@ -218,8 +250,9 @@ struct viterbi_dec : viterbi_dec_interface fprintf(stderr," ]\n"); #endif // Return difference between best and second-best as quality metric. - if (quality) + if (quality) { *quality = best2_tpm - best_tpm; + } // Return uncoded symbol of best path return (*states)[best_state].path.read(); } @@ -228,23 +261,29 @@ struct viterbi_dec : viterbi_dec_interface // The costs provided must be negative. // The other symbols will be assigned a cost of 0. - TUS update(int nm, TCS cs[], TBM costs[], TPM *quality = NULL) + TUS update(int nm, TCS cs[], TBM costs[], TPM *quality = nullptr) { TPM best_tpm = max_tpm, best2_tpm = max_tpm; TS best_state = 0; + // Update all states for (int s = 0; s < NSTATES; ++s) { // Select best branch among those for with metrics are provided TPM best_m = max_tpm; - typename trellis::state::branch *best_b = NULL; + typename trellis::state::branch *best_b = nullptr; + for (int im = 0; im < nm; ++im) { typename trellis::state::branch *b = &trell->states[s].branches[cs[im]]; - if (b->pred == trell->NOSTATE) + + if (b->pred == trell->NOSTATE) { continue; + } + TPM m = (*states)[b->pred].cost + costs[im]; + if (m <= best_m) { // <= guarantees one match best_m = m; @@ -260,9 +299,13 @@ struct viterbi_dec : viterbi_dec_interface { typename trellis::state::branch *b = &trell->states[s].branches[cs]; - if (b->pred == trell->NOSTATE) + + if (b->pred == trell->NOSTATE) { continue; + } + TPM m = (*states)[b->pred].cost; + if (m <= best_m) { best_m = m; @@ -270,9 +313,11 @@ struct viterbi_dec : viterbi_dec_interface } } } + (*newstates)[s].path = (*states)[best_b->pred].path; (*newstates)[s].path.append(best_b->us); (*newstates)[s].cost = best_m; + // Select best states if (best_m < best_tpm) { @@ -281,7 +326,9 @@ struct viterbi_dec : viterbi_dec_interface best_tpm = best_m; } else if (best_m < best2_tpm) + { best2_tpm = best_m; + } } // Swap banks { @@ -290,8 +337,9 @@ struct viterbi_dec : viterbi_dec_interface newstates = tmp; } // Prevent overflow of path metrics - for (TS s = 0; s < NSTATES; ++s) + for (TS s = 0; s < NSTATES; ++s) { (*states)[s].cost -= best_tpm; + } #if 0 // Observe that the min-max range remains bounded fprintf(stderr,"-%2d = [", best_tpm); @@ -299,8 +347,10 @@ struct viterbi_dec : viterbi_dec_interface fprintf(stderr," ]\n"); #endif // Return difference between best and second-best as quality metric. - if (quality) + if (quality) { *quality = best2_tpm - best_tpm; + } + // Return uncoded symbol of best path return (*states)[best_state].path.read(); } @@ -308,17 +358,21 @@ struct viterbi_dec : viterbi_dec_interface // Update with single-symbol metric. // cost must be negative. - TUS update(TCS cs, TBM cost, TPM *quality = NULL) - { + TUS update(TCS cs, TBM cost, TPM *quality = nullptr) { return update(1, &cs, &cost, quality); } void dump() { fprintf(stderr, "["); + for (TS s = 0; s < NSTATES; ++s) - if (states[s].cost) + { + if (states[s].cost) { fprintf(stderr, " %02x:%d", s, states[s].cost); + } + } + fprintf(stderr, "\n"); }