VE7UWU/sdrangel
1
0
Fork 0
mirror of https://github.com/f4exb/sdrangel.git synced 2025-04-06 03:29:12 -04:00
Code Issues Projects Releases Wiki Activity

DATV demod: leandvb: more memory management fixes and code formatting

Browse Source
This commit is contained in:
f4exb 2021-03-22 02:06:26 +01:00
parent debc5c74f1
commit 300fd37880
19 changed files with 1980 additions and 651 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
plugins/channelrx/demoddatv/ldpctool/tables_handler.cpp
View File

@ -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<DVB_S2_TABLE_B1>();
case 2:
@ -57,8 +60,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number)
return new LDPC<DVB_S2_TABLE_B11>();
}
}
if (prefix == 'C') {
switch (number) {
if (prefix == 'C')
{
switch (number)
{
case 1:
return new LDPC<DVB_S2_TABLE_C1>();
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<DVB_S2X_TABLE_B1>();
case 2:
@ -135,8 +145,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number)
return new LDPC<DVB_S2X_TABLE_B24>();
}
}
if (prefix == 'C') {
switch (number) {
if (prefix == 'C')
{
switch (number)
{
case 1:
return new LDPC<DVB_S2X_TABLE_C1>();
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<DVB_T2_TABLE_A1>();
case 2:
@ -177,8 +194,11 @@ LDPCInterface *create_ldpc(char *standard, char prefix, int number)
return new LDPC<DVB_T2_TABLE_A6>();
}
}
if (prefix == 'B') {
switch (number) {
if (prefix == 'B')
{
switch (number)
{
case 1:
return new LDPC<DVB_T2_TABLE_B1>();
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[];

99
plugins/channelrx/demoddatv/leansdr/bch.h
View File

@ -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 <typename T, int N, int NP, int DP, typename TGF, int GFTRUNCGEN>
struct bch_engine : bch_interface
{
bch_engine(const bitvect<T, NP> *polys, int _npolys)
: npolys(_npolys)
bch_engine(
const bitvect<T, NP> *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<T, DP>[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<T, N> 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<T, DP> *rem = new bitvect<T, DP>[npolys]; // npolys is not static hence 'bitvect<T, DP> 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;
}

207
plugins/channelrx/demoddatv/leansdr/dsp.h
View File

@ -33,22 +33,29 @@ namespace leansdr
template <typename Tin, int Zin, typename Tout, int Zout, int Gn, int Gd>
struct cconverter : runnable
{
cconverter(scheduler *sch, pipebuf<complex<Tin>> &_in,
pipebuf<complex<Tout>> &_out)
: runnable(sch, "cconverter"),
in(_in), out(_out)
cconverter(
scheduler *sch,
pipebuf<complex<Tin>> &_in,
pipebuf<complex<Tout>> &_out
) :
runnable(sch, "cconverter"),
in(_in),
out(_out)
{
}
void run()
{
unsigned long count = min(in.readable(), out.writable());
complex<Tin> *pin = in.rd(), *pend = pin + count;
complex<Tout> *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<T>[n];
omega_rev = new complex<T>[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<T> tmp = data[i];
@ -118,15 +134,18 @@ struct cfft_engine
data[r] = tmp;
}
}
complex<T> *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<T> &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 <typename T>
struct adder : runnable
{
adder(scheduler *sch,
pipebuf<T> &_in1, pipebuf<T> &_in2, pipebuf<T> &_out)
: runnable(sch, "adder"),
in1(_in1), in2(_in2), out(_out)
adder(
scheduler *sch,
pipebuf<T> &_in1,
pipebuf<T> &_in2,
pipebuf<T> &_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 <typename Tscale, typename Tin, typename Tout>
struct scaler : runnable
{
Tscale scale;
scaler(scheduler *sch, Tscale _scale,
pipebuf<Tin> &_in, pipebuf<Tout> &_out)
: runnable(sch, "scaler"),
scale(_scale),
in(_in), out(_out)
scaler(
scheduler *sch,
Tscale _scale,
pipebuf<Tin> &_in,
pipebuf<Tout> &_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 <typename T>
struct wgn_c : runnable
{
wgn_c(scheduler *sch, pipebuf<complex<T>> &_out)
: runnable(sch, "awgn"), stddev(1.0), out(_out)
wgn_c(
scheduler *sch,
pipebuf<complex<T>>
&_out
) :
runnable(sch, "awgn"),
stddev(1.0),
out(_out)
{
}
void run()
{
int n = out.writable();
complex<T> *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 <typename T>
struct naive_lowpass : runnable
{
naive_lowpass(scheduler *sch, pipebuf<T> &_in, pipebuf<T> &_out, int _w)
: runnable(sch, "lowpass"), in(_in), out(_out), w(_w)
naive_lowpass(
scheduler *sch,
pipebuf<T> &_in,
pipebuf<T> &_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 <typename T, typename Tc>
struct fir_filter : runnable
{
fir_filter(scheduler *sch, int _ncoeffs, Tc *_coeffs,
pipebuf<T> &_in, pipebuf<T> &_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<T> &_in,
pipebuf<T> &_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<T> in;
pipewriter<T> 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 <typename T, typename Tc>
struct fir_resampler : runnable
{
fir_resampler(scheduler *sch, int _ncoeffs, Tc *_coeffs,
pipebuf<T> &_in, pipebuf<T> &_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<T> &_in,
pipebuf<T> &_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<T> in;
pipewriter<T> out;
public:
float *freq_tap;
float tap_multiplier;
float freq_tol;
private:
unsigned int ncoeffs;
Tc *coeffs;
int interp, decim;
pipereader<T> in;
pipewriter<T> out;
T *shifted_coeffs;
float current_freq;
void set_freq(float f)
{
for (int i = 0; i < ncoeffs; ++i)

764
plugins/channelrx/demoddatv/leansdr/dvbs2.h
View File

File diff suppressed because it is too large Load Diff

7
plugins/channelrx/demoddatv/leansdr/filtergen.cpp
View File

@ -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
} // leansdr

46
plugins/channelrx/demoddatv/leansdr/filtergen.h
View File

@ -30,34 +30,50 @@ template <typename T>
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 <typename T>
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 <typename T>
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;
}

6
plugins/channelrx/demoddatv/leansdr/framework.cpp
View File

@ -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);
}

163
plugins/channelrx/demoddatv/leansdr/framework.h
View File

@ -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<T> &_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<T> *p, int n = 1)
template <typename T>
void opt_write(pipewriter<T> *p, T val)
{
if (p)
if (p) {
p->write(val);
}
}
template <typename T>
@ -357,16 +401,13 @@ struct pipereader
int id;
pipereader(pipebuf<T> &_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 <typename T>
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 <typename T>
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 <typename T>
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 <typename T>
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 <typename T>
T min(const T &x, const T &y)
{
T min(const T &x, const T &y) {
return (x < y) ? x : y;
}
template <typename T>
T max(const T &x, const T &y)
{
T max(const T &x, const T &y) {
return (x < y) ? y : x;
}

326
plugins/channelrx/demoddatv/leansdr/generic.h
View File

@ -43,67 +43,105 @@ namespace leansdr
template <typename T>
struct file_reader : runnable
{
file_reader(scheduler *sch, int _fdin, pipebuf<T> &_out)
: runnable(sch, _out.name),
loop(false),
filler(NULL),
fdin(_fdin), out(_out)
bool loop;
file_reader(
scheduler *sch,
int _fdin,
pipebuf<T> &_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 <typename T>
struct file_writer : runnable
{
file_writer(scheduler *sch, pipebuf<T> &_in, int _fdout) : runnable(sch, _in.name),
in(_in), fdout(_fdout)
file_writer(
scheduler *sch,
pipebuf<T> &_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 <typename T>
struct file_printer : runnable
{
file_printer(scheduler *sch, const char *_format,
pipebuf<T> &_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<T> &_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 <typename T>
struct file_carrayprinter : runnable
{
file_carrayprinter(scheduler *sch,
const char *_head,
const char *_format,
const char *_sep,
const char *_tail,
pipebuf<complex<T>> &_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<complex<T>> &_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<T> *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<complex<T>> in;
const char *head, *format, *sep, *tail;
@ -237,18 +330,32 @@ struct file_carrayprinter : runnable
template <typename T, int N>
struct file_vectorprinter : runnable
{
file_vectorprinter(scheduler *sch,
const char *_head,
const char *_format,
const char *_sep,
const char *_tail,
pipebuf<T[N]> &_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<T[N]> &_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 <typename Tin, typename Tout>
struct itemcounter : runnable
{
itemcounter(scheduler *sch, pipebuf<Tin> &_in, pipebuf<Tout> &_out)
: runnable(sch, "itemcounter"),
in(_in), out(_out)
itemcounter(
scheduler *sch,
pipebuf<Tin> &_in,
pipebuf<Tout> &_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<T> &_in, pipebuf<T> &_out)
: runnable(sch, "decimator"),
d(_d),
in(_in), out(_out)
decimator(
scheduler *sch,
int _d,
pipebuf<T> &_in,
pipebuf<T> &_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<int> &_num, pipebuf<int> &_den,
pipebuf<float> &_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<int> &_num,
pipebuf<int> &_den,
pipebuf<float> &_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 <typename Tin, typename Tout>
struct serializer : runnable
{
serializer(scheduler *sch, pipebuf<Tin> &_in, pipebuf<Tout> &_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<Tin> &_in,
pipebuf<Tout> &_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 <typename T>
struct buffer_reader : runnable
{
buffer_reader(scheduler *sch, T *_data, int _count, pipebuf<T> &_out)
: runnable(sch, "buffer_reader"),
data(_data), count(_count), out(_out), pos(0)
buffer_reader(
scheduler *sch,
T *_data,
int _count,
pipebuf<T> &_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 <typename T>
struct buffer_writer : runnable
{
buffer_writer(scheduler *sch, pipebuf<T> &_in, T *_data, int _count)
: runnable(sch, "buffer_reader"),
in(_in), data(_data), count(_count), pos(0)
buffer_writer(
scheduler *sch,
pipebuf<T> &_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));

140
plugins/channelrx/demoddatv/leansdr/hdlc.h
View File

@ -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<u8> &_in, // Packed bits
pipebuf<u8> &_out, // Bytes
int _minframesize, // Including CRC, excluding HDLC flags.
int _maxframesize,
// Status
pipebuf<int> *_lock_out = NULL,
pipebuf<int> *_framecount_out = NULL,
pipebuf<int> *_fcserrcount_out = NULL,
pipebuf<int> *_hdlcbytecount_out = NULL,
pipebuf<int> *_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<u8> &_in, // Packed bits
pipebuf<u8> &_out, // Bytes
int _minframesize, // Including CRC, excluding HDLC flags.
int _maxframesize,
// Status
pipebuf<int> *_lock_out = nullptr,
pipebuf<int> *_framecount_out = nullptr,
pipebuf<int> *_fcserrcount_out = nullptr,
pipebuf<int> *_hdlcbytecount_out = nullptr,
pipebuf<int> *_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

20
plugins/channelrx/demoddatv/leansdr/iess.h
View File

@ -28,22 +28,28 @@ namespace leansdr
struct etr192_descrambler : runnable
{
etr192_descrambler(scheduler *sch,
pipebuf<u8> &_in, // Packed scrambled bits
pipebuf<u8> &_out) // Packed bits
: runnable(sch, "etr192_dec"),
in(_in), out(_out),
shiftreg(0), counter(0)
etr192_descrambler(
scheduler *sch,
pipebuf<u8> &_in, // Packed scrambled bits
pipebuf<u8> &_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);
}

227
plugins/channelrx/demoddatv/leansdr/ldpc.h
View File

@ -51,12 +51,6 @@ struct ldpc_table
template <typename SOFTBIT, typename SOFTWORD, int SWSIZE, typename Taddr>
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<Taddr> *table, int _k, int _n)
: k(_k), n(_n)
ldpc_engine(
const ldpc_table<Taddr> *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<Taddr> *table, const SOFTWORD *msg,
int k, int n, SOFTWORD *parity, int integrate = true)
void encode(
const ldpc_table<Taddr> *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<Taddr>::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<Taddr> *table, SOFTWORD *cw,
int k, int n,
int max_bitflips)
int decode_bitflip(
const ldpc_table<Taddr> *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<SOFTBIT, SOFTWORD>(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<SOFTBIT, SOFTWORD>(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<SOFTBIT, SOFTWORD>(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<SOFTBIT, SOFTWORD>(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;
}
}

5
plugins/channelrx/demoddatv/leansdr/math.cpp
View File

@ -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
} // leansdr

66
plugins/channelrx/demoddatv/leansdr/math.h
View File

@ -27,25 +27,31 @@ template <typename T>
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<T> &x)
{
re += x.re;
im += x.im;
}
inline void operator*=(const complex<T> &c)
{
T tre = re * c.re - im * c.im;
im = re * c.im + im * c.re;
re = tre;
}
inline void operator-=(const complex<T> &x)
{
re-=x.re;
im-=x.im;
}
inline void operator*=(const T &k)
{
re *= k;
@ -86,8 +92,11 @@ template <typename T>
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 <typename T>
T cnorm2(const complex<T> *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<T> *p, int n)
template <typename T>
inline complex<T> conjprod(const complex<T> &u, const complex<T> &v)
{
return complex<T>(u.re * v.re + u.im * v.im,
u.re * v.im - u.im * v.re);
return complex<T>(
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 <typename T>
complex<T> conjprod(const complex<T> *u, const complex<T> *v, int n)
{
complex<T> acc = 0;
while (n--)
while (n--) {
acc += conjprod(*u++, *v++);
}
return acc;
}
@ -140,6 +157,7 @@ int log2i(uint64_t x);
struct trig16
{
complex<float> 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<float> &expi(uint16_t a) const
{
return lut[a];
}
// a must fit in a int32_t, otherwise behaviour is undefined
inline const complex<float> &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<typename T>
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

120
plugins/channelrx/demoddatv/leansdr/rs.h
View File

@ -47,50 +47,65 @@ namespace leansdr
template <typename Te, typename Tp, Tp P, int N, Te ALPHA>
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<unsigned char, unsigned short, 0x11d, 8, 2> 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

18
plugins/channelrx/demoddatv/leansdr/sdr.cpp
View File

@ -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;
}
}

212
plugins/channelrx/demoddatv/leansdr/sdr.h
View File

@ -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<T> *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<float> bb(pin->re * s->ej->re + pin->im * s->ej->im,
-pin->re * s->ej->im + pin->im * s->ej->re);
complex<float> 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<float> 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<complex<T>> &_in, pipebuf<T> &_out
pipebuf<complex<T>> &_in,
pipebuf<T> &_out
) :
runnable(sch, "SS estimator"),
window_size(1024),
@ -260,11 +279,13 @@ struct ss_estimator : runnable
complex<T> *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<T> *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<signed char> polar(float r, int n, float i)
{
float a = i * 2 * M_PI / n;
return complex<signed char>(r * cosf(a) * cstln_amp,
r * sinf(a) * cstln_amp);
return complex<signed char>(
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<signed char>(r * cosf(phi) * cstln_amp,
r * sinf(phi) * cstln_amp);
symbols[i + j] = complex<signed char>(
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 <typename T>
struct sampler_interface
{
virtual ~sampler_interface()
{
virtual ~sampler_interface() {
}
virtual complex<T> interp(const complex<T> *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 <typename T>
struct nearest_sampler : sampler_interface<T>
{
int readahead()
{
int readahead() {
return 0;
}
@ -957,8 +999,7 @@ struct nearest_sampler : sampler_interface<T>
template <typename T>
struct linear_sampler : sampler_interface<T>
{
int readahead()
{
int readahead() {
return 1;
}
@ -998,13 +1039,12 @@ struct fir_sampler : sampler_interface<T>
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<T>
// 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<T>
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<float> ev(s.re - cstln_point->re,
s.im - cstln_point->im);
complex<float> 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<T> *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)
{

42
plugins/channelrx/demoddatv/leansdr/softword.h
View File

@ -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 <typename SOFTBIT, typename SOFTBYTE>
inline void softwords_set(SOFTBYTE p[], int b, SOFTBIT v)
{
softword_set(&p[b / 8], b & 7, v);
}
template <typename SOFTBIT, typename SOFTBYTE>
inline SOFTBIT softwords_weight(const SOFTBYTE p[], int b)
{

110
plugins/channelrx/demoddatv/leansdr/viterbi.h
View File

@ -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 <typename TUS,
struct viterbi_dec_interface
{
virtual ~viterbi_dec_interface() {}
virtual TUS update(TBM *costs, TPM *quality = NULL) = 0;
virtual TUS update(TCS s, TBM cost, TPM *quality = NULL) = 0;
virtual TUS update(TBM *costs, TPM *quality = nullptr) = 0;
virtual TUS update(TCS s, TBM cost, TPM *quality = nullptr) = 0;
};
template <typename TS, int NSTATES,
@ -144,6 +160,7 @@ struct viterbi_dec : viterbi_dec_interface<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
{
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<TS, NSTATES, TUS, NUS, NCS>::state::branch *best_b = NULL;
typename trellis<TS, NSTATES, TUS, NUS, NCS>::state::branch *best_b = nullptr;
// Select best branch
for (int cs = 0; cs < NCS; ++cs)
{
typename trellis<TS, NSTATES, TUS, NUS, NCS>::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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
// 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<TS, NSTATES, TUS, NUS, NCS>::state::branch *best_b = NULL;
typename trellis<TS, NSTATES, TUS, NUS, NCS>::state::branch *best_b = nullptr;
for (int im = 0; im < nm; ++im)
{
typename trellis<TS, NSTATES, TUS, NUS, NCS>::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<TUS, TCS, TBM, TPM>
{
typename trellis<TS, NSTATES, TUS, NUS, NCS>::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<TUS, TCS, TBM, TPM>
}
}
}
(*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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
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<TUS, TCS, TBM, TPM>
// 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");
}