You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
400 lines
15 KiB
400 lines
15 KiB
/////////////////////////////////////////////////////////////////////////////////// |
|
// Copyright (C) 2026 Jon Beniston, M7RCE <jon@beniston.com> // |
|
// // |
|
// This program is free software; you can redistribute it and/or modify // |
|
// it under the terms of the GNU General Public License as published by // |
|
// the Free Software Foundation as version 3 of the License, or // |
|
// (at your option) any later version. // |
|
// // |
|
// This program is distributed in the hope that it will be useful, // |
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of // |
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // |
|
// GNU General Public License V3 for more details. // |
|
// // |
|
// You should have received a copy of the GNU General Public License // |
|
// along with this program. If not, see <http://www.gnu.org/licenses/>. // |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
// DVB-T OFDM demodulation's kernels, with AVX2 and FMA. Compiled with them enabled, so only used if the CPU has |
|
// them. The convolutions work 32 floats (16 carriers) a block, in four or eight accumulators, enough to keep the |
|
// FMAs busy. No library templates (std::complex, std::min) are used: code instantiated here, compiled for AVX2, |
|
// could be shared with other files that run on any CPU |
|
|
|
#include <cmath> |
|
|
|
#include <immintrin.h> |
|
#if defined(_MSC_VER) |
|
#include <intrin.h> |
|
#endif |
|
|
|
#include "dvbtofdmkernel.h" |
|
|
|
namespace dvbt { |
|
|
|
int convolveAVX2(const float *x, const float *wc, int half, bool symmetric, int f, int end, float *y) |
|
{ |
|
for (; f + 32 <= end; f += 32) |
|
{ |
|
const float *xc = x + f; |
|
const __m256 w0 = _mm256_set1_ps(wc[0]); |
|
__m256 a0 = _mm256_mul_ps(w0, _mm256_loadu_ps(xc)); |
|
__m256 a1 = _mm256_mul_ps(w0, _mm256_loadu_ps(xc + 8)); |
|
__m256 a2 = _mm256_mul_ps(w0, _mm256_loadu_ps(xc + 16)); |
|
__m256 a3 = _mm256_mul_ps(w0, _mm256_loadu_ps(xc + 24)); |
|
|
|
if (symmetric) |
|
{ |
|
for (int i = 1; i <= half; i++) |
|
{ |
|
const __m256 w = _mm256_broadcast_ss(&wc[i]); |
|
const float *lo = xc - 2 * i; |
|
const float *hi = xc + 2 * i; |
|
a0 = _mm256_fmadd_ps(w, _mm256_add_ps(_mm256_loadu_ps(lo), _mm256_loadu_ps(hi)), a0); |
|
a1 = _mm256_fmadd_ps(w, _mm256_add_ps(_mm256_loadu_ps(lo + 8), _mm256_loadu_ps(hi + 8)), a1); |
|
a2 = _mm256_fmadd_ps(w, _mm256_add_ps(_mm256_loadu_ps(lo + 16), _mm256_loadu_ps(hi + 16)), a2); |
|
a3 = _mm256_fmadd_ps(w, _mm256_add_ps(_mm256_loadu_ps(lo + 24), _mm256_loadu_ps(hi + 24)), a3); |
|
} |
|
} |
|
else |
|
{ |
|
for (int i = -half; i <= half; i++) |
|
{ |
|
if (i == 0) { |
|
continue; |
|
} |
|
|
|
const __m256 w = _mm256_broadcast_ss(&wc[i]); |
|
const float *xi = xc + 2 * i; |
|
a0 = _mm256_fmadd_ps(w, _mm256_loadu_ps(xi), a0); |
|
a1 = _mm256_fmadd_ps(w, _mm256_loadu_ps(xi + 8), a1); |
|
a2 = _mm256_fmadd_ps(w, _mm256_loadu_ps(xi + 16), a2); |
|
a3 = _mm256_fmadd_ps(w, _mm256_loadu_ps(xi + 24), a3); |
|
} |
|
} |
|
|
|
_mm256_storeu_ps(y + f, a0); |
|
_mm256_storeu_ps(y + f + 8, a1); |
|
_mm256_storeu_ps(y + f + 16, a2); |
|
_mm256_storeu_ps(y + f + 24, a3); |
|
} |
|
|
|
return f; |
|
} |
|
|
|
int convolve2AVX2(const float *x, const float *wc1, const float *wc2, int half, int f, int end, float *y1, float *y2) |
|
{ |
|
for (; f + 32 <= end; f += 32) |
|
{ |
|
const float *xc = x + f; |
|
__m256 a0 = _mm256_setzero_ps(), a1 = a0, a2 = a0, a3 = a0; |
|
__m256 b0 = a0, b1 = a0, b2 = a0, b3 = a0; |
|
|
|
for (int i = -half; i <= half; i++) |
|
{ |
|
const __m256 w1 = _mm256_broadcast_ss(&wc1[i]); |
|
const __m256 w2 = _mm256_broadcast_ss(&wc2[i]); |
|
const float *xi = xc + 2 * i; |
|
const __m256 x0 = _mm256_loadu_ps(xi); |
|
const __m256 x1 = _mm256_loadu_ps(xi + 8); |
|
const __m256 x2 = _mm256_loadu_ps(xi + 16); |
|
const __m256 x3 = _mm256_loadu_ps(xi + 24); |
|
a0 = _mm256_fmadd_ps(w1, x0, a0); |
|
a1 = _mm256_fmadd_ps(w1, x1, a1); |
|
a2 = _mm256_fmadd_ps(w1, x2, a2); |
|
a3 = _mm256_fmadd_ps(w1, x3, a3); |
|
b0 = _mm256_fmadd_ps(w2, x0, b0); |
|
b1 = _mm256_fmadd_ps(w2, x1, b1); |
|
b2 = _mm256_fmadd_ps(w2, x2, b2); |
|
b3 = _mm256_fmadd_ps(w2, x3, b3); |
|
} |
|
|
|
_mm256_storeu_ps(y1 + f, a0); |
|
_mm256_storeu_ps(y1 + f + 8, a1); |
|
_mm256_storeu_ps(y1 + f + 16, a2); |
|
_mm256_storeu_ps(y1 + f + 24, a3); |
|
_mm256_storeu_ps(y2 + f, b0); |
|
_mm256_storeu_ps(y2 + f + 8, b1); |
|
_mm256_storeu_ps(y2 + f + 16, b2); |
|
_mm256_storeu_ps(y2 + f + 24, b3); |
|
} |
|
|
|
return f; |
|
} |
|
|
|
// Four complex products at once |
|
static inline __m256 complexMultiply(__m256 a, __m256 b) |
|
{ |
|
const __m256 swapped = _mm256_permute_ps(a, 0xb1); // Imaginary, real |
|
return _mm256_fmaddsub_ps(a, _mm256_moveldup_ps(b), _mm256_mul_ps(swapped, _mm256_movehdup_ps(b))); |
|
} |
|
|
|
// One, (ar + j ai)(br + j bi), as std::complex<float> would |
|
static inline void complexMultiply(float ar, float ai, float br, float bi, float& yr, float& yi) |
|
{ |
|
yr = ar * br - ai * bi; |
|
yi = ar * bi + ai * br; |
|
} |
|
|
|
// Eight carriers (interleaved complex) are loaded as carriers 0, 1 | 4, 5 and 2, 3 | 6, 7, so a horizontal add of |
|
// the two gives a value per carrier in order, without moving data across the 128-bit halves, which is slow on |
|
// efficiency cores |
|
static inline __m256 load2(const float *lo, const float *hi) |
|
{ |
|
return _mm256_insertf128_ps(_mm256_castps128_ps256(_mm_loadu_ps(lo)), _mm_loadu_ps(hi), 1); |
|
} |
|
|
|
static inline void store2(float *lo, float *hi, __m256 v) |
|
{ |
|
_mm_storeu_ps(lo, _mm256_castps256_ps128(v)); |
|
_mm_storeu_ps(hi, _mm256_extractf128_ps(v, 1)); |
|
} |
|
|
|
static inline __m256 select(__m256 mask, __m256 a, __m256 b) // a where mask is set, else b |
|
{ |
|
return _mm256_or_ps(_mm256_and_ps(mask, a), _mm256_andnot_ps(mask, b)); |
|
} |
|
|
|
void rotateAVX2(const float *x, float *y, int n, const float *table, double phase, double blockStepRe, |
|
double blockStepIm) |
|
{ |
|
static const int block = 64; |
|
double baseRe = std::cos(phase), baseIm = std::sin(phase); // As std::polar(1.0, phase) |
|
|
|
for (int b = 0; b < n; b += block) |
|
{ |
|
const int m = n - b < block ? n - b : block; |
|
const float br = (float) baseRe, bi = (float) baseIm; |
|
const __m256 bv = _mm256_setr_ps(br, bi, br, bi, br, bi, br, bi); |
|
int i = 0; |
|
|
|
for (; i + 4 <= m; i += 4) |
|
{ |
|
const __m256 phasors = complexMultiply(_mm256_loadu_ps(table + 2 * i), bv); |
|
_mm256_storeu_ps(y + 2 * (b + i), complexMultiply(_mm256_loadu_ps(x + 2 * (b + i)), phasors)); |
|
} |
|
|
|
for (; i < m; i++) |
|
{ |
|
float pr, pi; |
|
complexMultiply(table[2 * i], table[2 * i + 1], br, bi, pr, pi); |
|
complexMultiply(x[2 * (b + i)], x[2 * (b + i) + 1], pr, pi, y[2 * (b + i)], y[2 * (b + i) + 1]); |
|
} |
|
|
|
// base *= blockStep, as std::complex<double> |
|
const double re = baseRe * blockStepRe - baseIm * blockStepIm; |
|
baseIm = baseRe * blockStepIm + baseIm * blockStepRe; |
|
baseRe = re; |
|
} |
|
} |
|
|
|
void equaliseAVX2(const float *y, const float *h, float derotateRe, float derotateIm, int n, float *e, float *power) |
|
{ |
|
const __m256 dv = _mm256_setr_ps(derotateRe, derotateIm, derotateRe, derotateIm, derotateRe, derotateIm, |
|
derotateRe, derotateIm); |
|
const __m256 conjugate = _mm256_castsi256_ps(_mm256_setr_epi32(0, (int) 0x80000000, 0, (int) 0x80000000, 0, |
|
(int) 0x80000000, 0, (int) 0x80000000)); |
|
const __m256 one = _mm256_set1_ps(1.0f); |
|
const __m256 zero = _mm256_setzero_ps(); |
|
int i = 0; |
|
|
|
for (; i + 8 <= n; i += 8) |
|
{ |
|
const float *yi = y + 2 * i; |
|
const float *hi = h + 2 * i; |
|
const __m256 h0 = load2(hi, hi + 8); // Carriers i, i+1 | i+4, i+5 |
|
const __m256 h1 = load2(hi + 4, hi + 12); // i+2, i+3 | i+6, i+7 |
|
const __m256 v0 = complexMultiply(complexMultiply(load2(yi, yi + 8), _mm256_xor_ps(h0, conjugate)), dv); |
|
const __m256 v1 = complexMultiply(complexMultiply(load2(yi + 4, yi + 12), _mm256_xor_ps(h1, conjugate)), dv); |
|
// |h|^2 in order. Duplicating each, low then high, gives the layout of v0 and v1 |
|
const __m256 p = _mm256_hadd_ps(_mm256_mul_ps(h0, h0), _mm256_mul_ps(h1, h1)); |
|
const __m256 inverse = _mm256_and_ps(_mm256_div_ps(one, p), _mm256_cmp_ps(p, zero, _CMP_GT_OQ)); |
|
float *ei = e + 2 * i; |
|
store2(ei, ei + 8, _mm256_mul_ps(v0, _mm256_unpacklo_ps(inverse, inverse))); |
|
store2(ei + 4, ei + 12, _mm256_mul_ps(v1, _mm256_unpackhi_ps(inverse, inverse))); |
|
_mm256_storeu_ps(power + i, p); |
|
} |
|
|
|
for (; i < n; i++) |
|
{ |
|
const float p = h[2 * i] * h[2 * i] + h[2 * i + 1] * h[2 * i + 1]; |
|
float ur, ui, vr, vi; |
|
complexMultiply(y[2 * i], y[2 * i + 1], h[2 * i], -h[2 * i + 1], ur, ui); |
|
complexMultiply(ur, ui, derotateRe, derotateIm, vr, vi); |
|
const float inverse = p > 0.0f ? 1.0f / p : 0.0f; |
|
e[2 * i] = vr * inverse; |
|
e[2 * i + 1] = vi * inverse; |
|
power[i] = p; |
|
} |
|
} |
|
|
|
int updateNoiseAVX2(const float *e, const float *power, const float *mask, int n, float scale, float maxPoint, |
|
float invScale2, float alpha, float *noise, float& total, float& count) |
|
{ |
|
const __m256 sv = _mm256_set1_ps(scale); |
|
const __m256 maxv = _mm256_set1_ps(maxPoint); |
|
const __m256 minv = _mm256_set1_ps(-maxPoint); |
|
const __m256 half = _mm256_set1_ps(0.5f); |
|
const __m256 four = _mm256_set1_ps(4.0f); |
|
const __m256i fouri = _mm256_set1_epi32(4); |
|
const __m256 two = _mm256_set1_ps(2.0f); |
|
const __m256 one = _mm256_set1_ps(1.0f); |
|
const __m256 zero = _mm256_setzero_ps(); |
|
const __m256 av = _mm256_set1_ps(alpha); |
|
const __m256 is2 = _mm256_set1_ps(invScale2); |
|
__m256 totals = zero, counts = zero; |
|
|
|
// The nearest point, 2 floor(x / 2) + 1, clamped. Adding 4 makes it positive, so truncation floors |
|
auto squaredError = [&](__m256 x) { |
|
x = _mm256_mul_ps(x, sv); |
|
const __m256 c = _mm256_max_ps(minv, _mm256_min_ps(maxv, x)); |
|
const __m256i f = _mm256_sub_epi32(_mm256_cvttps_epi32(_mm256_fmadd_ps(c, half, four)), fouri); |
|
const __m256 err = _mm256_sub_ps(x, _mm256_fmadd_ps(two, _mm256_cvtepi32_ps(f), one)); |
|
return _mm256_mul_ps(err, err); |
|
}; |
|
|
|
int k = 0; |
|
|
|
for (; k + 8 <= n; k += 8) |
|
{ |
|
const float *ek = e + 2 * k; |
|
const __m256 a = squaredError(load2(ek, ek + 8)); |
|
const __m256 b = squaredError(load2(ek + 4, ek + 12)); |
|
const __m256 measured = _mm256_mul_ps(_mm256_mul_ps(_mm256_hadd_ps(a, b), is2), _mm256_loadu_ps(power + k)); |
|
const __m256 old = _mm256_loadu_ps(noise + k); |
|
const __m256 averaged = _mm256_fmadd_ps(av, _mm256_sub_ps(measured, old), old); |
|
const __m256 updated = select(_mm256_cmp_ps(old, zero, _CMP_LT_OQ), measured, averaged); |
|
const __m256 result = select(_mm256_cmp_ps(_mm256_loadu_ps(mask + k), zero, _CMP_GT_OQ), updated, old); |
|
_mm256_storeu_ps(noise + k, result); |
|
totals = _mm256_add_ps(totals, _mm256_max_ps(result, zero)); |
|
counts = _mm256_add_ps(counts, _mm256_and_ps(_mm256_cmp_ps(result, zero, _CMP_GE_OQ), one)); |
|
} |
|
|
|
alignas(32) float t[8], c[8]; |
|
_mm256_store_ps(t, totals); |
|
_mm256_store_ps(c, counts); |
|
|
|
for (int i = 0; i < 8; i++) |
|
{ |
|
total += t[i]; |
|
count += c[i]; |
|
} |
|
|
|
return k; |
|
} |
|
|
|
int smoothNoiseAVX2(const float *noise, int n, float floor, float *smoothed, float *inverse) |
|
{ |
|
const __m256 floorv = _mm256_set1_ps(floor); |
|
const __m256 zero = _mm256_setzero_ps(); |
|
const __m256 one = _mm256_set1_ps(1.0f); |
|
int j = 0; |
|
|
|
for (; j + 8 <= n; j += 8) |
|
{ |
|
__m256 s = zero, c = zero; |
|
|
|
for (int d = -2; d <= 2; d++) |
|
{ |
|
const __m256 x = _mm256_loadu_ps(noise + j + d); |
|
s = _mm256_add_ps(s, _mm256_max_ps(x, zero)); |
|
c = _mm256_add_ps(c, _mm256_and_ps(_mm256_cmp_ps(x, zero, _CMP_GE_OQ), one)); |
|
} |
|
|
|
// None measured gives the floor |
|
const __m256 sm = _mm256_max_ps(floorv, _mm256_div_ps(s, _mm256_max_ps(c, one))); |
|
_mm256_storeu_ps(smoothed + j, sm); |
|
_mm256_storeu_ps(inverse + j, _mm256_div_ps(one, sm)); |
|
} |
|
|
|
return j; |
|
} |
|
|
|
void windowAVX2(const float *x, const float *window, int n, float *out) |
|
{ |
|
int j = 0; |
|
|
|
for (; j + 4 <= n; j += 4) |
|
{ |
|
const __m128 w = _mm_loadu_ps(window + j); // Each duplicated, for the real and imaginary parts |
|
const __m256 wv = _mm256_insertf128_ps(_mm256_castps128_ps256(_mm_unpacklo_ps(w, w)), _mm_unpackhi_ps(w, w), 1); |
|
_mm256_storeu_ps(out + 2 * j, _mm256_mul_ps(_mm256_loadu_ps(x + 2 * j), wv)); |
|
} |
|
|
|
for (; j < n; j++) |
|
{ |
|
out[2 * j] = x[2 * j] * window[j]; |
|
out[2 * j + 1] = x[2 * j + 1] * window[j]; |
|
} |
|
} |
|
|
|
float powerAVX2(const float *x, int n, float *power) |
|
{ |
|
__m256 peaks = _mm256_setzero_ps(); |
|
int b = 0; |
|
|
|
for (; b + 8 <= n; b += 8) |
|
{ |
|
const float *xb = x + 2 * b; |
|
const __m256 a = load2(xb, xb + 8); |
|
const __m256 c = load2(xb + 4, xb + 12); |
|
const __m256 p = _mm256_hadd_ps(_mm256_mul_ps(a, a), _mm256_mul_ps(c, c)); |
|
_mm256_storeu_ps(power + b, p); |
|
peaks = _mm256_max_ps(peaks, p); |
|
} |
|
|
|
alignas(32) float m[8]; |
|
_mm256_store_ps(m, peaks); |
|
float peak = 0.0f; |
|
|
|
for (int i = 0; i < 8; i++) { |
|
peak = m[i] > peak ? m[i] : peak; |
|
} |
|
|
|
for (; b < n; b++) |
|
{ |
|
power[b] = x[2 * b] * x[2 * b] + x[2 * b + 1] * x[2 * b + 1]; |
|
peak = power[b] > peak ? power[b] : peak; |
|
} |
|
|
|
return peak; |
|
} |
|
|
|
static inline int lowestBit(unsigned int x) |
|
{ |
|
#if defined(_MSC_VER) |
|
unsigned long index; |
|
_BitScanForward(&index, x); |
|
return (int) index; |
|
#else |
|
return __builtin_ctz(x); |
|
#endif |
|
} |
|
|
|
int findAboveAVX2(const float *values, int n, float threshold, int *indices) |
|
{ |
|
const __m256 tv = _mm256_set1_ps(threshold); |
|
int count = 0; |
|
int b = 0; |
|
|
|
for (; b + 8 <= n; b += 8) |
|
{ |
|
unsigned int mask = (unsigned int) _mm256_movemask_ps(_mm256_cmp_ps(_mm256_loadu_ps(values + b), tv, _CMP_GE_OQ)); |
|
|
|
while (mask) |
|
{ |
|
indices[count++] = b + lowestBit(mask); |
|
mask &= mask - 1; |
|
} |
|
} |
|
|
|
for (; b < n; b++) |
|
{ |
|
if (values[b] >= threshold) { |
|
indices[count++] = b; |
|
} |
|
} |
|
|
|
return count; |
|
} |
|
|
|
} // namespace dvbt
|
|
|