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.
277 lines
12 KiB
277 lines
12 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/>. // |
|
/////////////////////////////////////////////////////////////////////////////////// |
|
|
|
// Demapping of many cells at once, 4 at a time with SSE2 or NEON (AArch64). The same arithmetic as demapCell() |
|
// and the receiver's outlier limiting, cell by cell, so the same LLRs |
|
|
|
#include <algorithm> |
|
#include <cstdlib> |
|
#include <cmath> |
|
#include <cstring> |
|
#include <limits> |
|
|
|
#include "dvbt2fec.h" |
|
#include "dvbt2demap.h" |
|
|
|
#if defined(__aarch64__) || defined(_M_ARM64) |
|
#include <arm_neon.h> |
|
#define DEMAP_NEON |
|
#elif defined(__SSE2__) || defined(_M_X64) || (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) |
|
#include <emmintrin.h> |
|
#define DEMAP_SSE2 |
|
#endif |
|
|
|
namespace dvbt2 { |
|
|
|
float CellDemapper::normalisation(int eta) |
|
{ |
|
static const float values[] = {1.0f, 1.0f, std::sqrt(2.0f), 1.0f, std::sqrt(10.0f), 1.0f, std::sqrt(42.0f), 1.0f, std::sqrt(170.0f)}; |
|
return values[eta]; |
|
} |
|
|
|
#if defined(DEMAP_SSE2) |
|
|
|
struct F4 |
|
{ |
|
typedef __m128 T; |
|
static T load(const float *p) { return _mm_loadu_ps(p); } |
|
static void store(float *p, T a) { _mm_storeu_ps(p, a); } |
|
static T set1(float x) { return _mm_set1_ps(x); } |
|
static T add(T a, T b) { return _mm_add_ps(a, b); } |
|
static T sub(T a, T b) { return _mm_sub_ps(a, b); } |
|
static T mul(T a, T b) { return _mm_mul_ps(a, b); } |
|
static T div(T a, T b) { return _mm_div_ps(a, b); } |
|
static T min(T a, T b) { return _mm_min_ps(a, b); } |
|
static T max(T a, T b) { return _mm_max_ps(a, b); } |
|
static T abs(T a) { return _mm_andnot_ps(_mm_set1_ps(-0.0f), a); } |
|
static T greater(T a, T b) { return _mm_cmpgt_ps(a, b); } |
|
static T select(T mask, T a, T b) { return _mm_or_ps(_mm_and_ps(mask, a), _mm_andnot_ps(mask, b)); } |
|
|
|
// Exact for |a| < 2^31: truncate, then one less where that rounded up |
|
static T floor(T a) |
|
{ |
|
T t = _mm_cvtepi32_ps(_mm_cvttps_epi32(a)); |
|
return _mm_sub_ps(t, _mm_and_ps(_mm_cmpgt_ps(t, a), _mm_set1_ps(1.0f))); |
|
} |
|
|
|
// Round to nearest, ties to even (the default rounding mode, as std::nearbyint), clamped to +/-llrLimit |
|
static void toInt8(T a, int8_t out[4]) |
|
{ |
|
a = _mm_min_ps(_mm_max_ps(a, _mm_set1_ps(-llrLimit)), _mm_set1_ps(llrLimit)); |
|
__m128i i = _mm_cvtps_epi32(a); |
|
i = _mm_packs_epi32(i, i); |
|
i = _mm_packs_epi16(i, i); |
|
int32_t bytes = _mm_cvtsi128_si32(i); |
|
std::memcpy(out, &bytes, 4); |
|
} |
|
}; |
|
|
|
#elif defined(DEMAP_NEON) |
|
|
|
struct F4 |
|
{ |
|
typedef float32x4_t T; |
|
static T load(const float *p) { return vld1q_f32(p); } |
|
static void store(float *p, T a) { vst1q_f32(p, a); } |
|
static T set1(float x) { return vdupq_n_f32(x); } |
|
static T add(T a, T b) { return vaddq_f32(a, b); } |
|
static T sub(T a, T b) { return vsubq_f32(a, b); } |
|
static T mul(T a, T b) { return vmulq_f32(a, b); } |
|
static T div(T a, T b) { return vdivq_f32(a, b); } |
|
static T min(T a, T b) { return vminq_f32(a, b); } |
|
static T max(T a, T b) { return vmaxq_f32(a, b); } |
|
static T abs(T a) { return vabsq_f32(a); } |
|
static T greater(T a, T b) { return vreinterpretq_f32_u32(vcgtq_f32(a, b)); } |
|
static T select(T mask, T a, T b) { return vbslq_f32(vreinterpretq_u32_f32(mask), a, b); } |
|
static T floor(T a) { return vrndmq_f32(a); } |
|
|
|
static void toInt8(T a, int8_t out[4]) |
|
{ |
|
a = vminq_f32(vmaxq_f32(a, vdupq_n_f32(-llrLimit)), vdupq_n_f32(llrLimit)); |
|
int32x4_t i = vcvtnq_s32_f32(a); |
|
int16x4_t h = vmovn_s32(i); |
|
int8x8_t b = vmovn_s16(vcombine_s16(h, h)); |
|
int32_t bytes = vget_lane_s32(vreinterpret_s32_s8(b), 0); |
|
std::memcpy(out, &bytes, 4); |
|
} |
|
}; |
|
|
|
#else |
|
|
|
// One lane, for other processors |
|
struct F4 |
|
{ |
|
struct T { float v[4]; }; |
|
static T load(const float *p) { T r; for (int i = 0; i < 4; i++) r.v[i] = p[i]; return r; } |
|
static void store(float *p, T a) { for (int i = 0; i < 4; i++) p[i] = a.v[i]; } |
|
static T set1(float x) { T r; for (int i = 0; i < 4; i++) r.v[i] = x; return r; } |
|
static T add(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] += b.v[i]; return a; } |
|
static T sub(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] -= b.v[i]; return a; } |
|
static T mul(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] *= b.v[i]; return a; } |
|
static T div(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] /= b.v[i]; return a; } |
|
static T min(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] = std::min(a.v[i], b.v[i]); return a; } |
|
static T max(T a, T b) { for (int i = 0; i < 4; i++) a.v[i] = std::max(a.v[i], b.v[i]); return a; } |
|
static T abs(T a) { for (int i = 0; i < 4; i++) a.v[i] = std::fabs(a.v[i]); return a; } |
|
static T greater(T a, T b) { T r; for (int i = 0; i < 4; i++) r.v[i] = a.v[i] > b.v[i] ? 1.0f : 0.0f; return r; } |
|
static T select(T mask, T a, T b) { for (int i = 0; i < 4; i++) a.v[i] = mask.v[i] != 0.0f ? a.v[i] : b.v[i]; return a; } |
|
static T floor(T a) { for (int i = 0; i < 4; i++) a.v[i] = std::floor(a.v[i]); return a; } |
|
|
|
static void toInt8(T a, int8_t out[4]) |
|
{ |
|
for (int i = 0; i < 4; i++) { |
|
out[i] = (int8_t) std::max(-llrLimit, std::min(llrLimit, std::nearbyint(a.v[i]))); |
|
} |
|
} |
|
}; |
|
|
|
#endif |
|
|
|
template <bool axes> |
|
void CellDemapper::demap(const float *re, const float *im, float *scales, float *scalesQ, int nbCells, int eta, float outlierThreshold, |
|
const int *bitMap, int8_t *llrs, float *errors) |
|
{ |
|
const int m = eta / 2; |
|
const float norm = normalisation(eta); |
|
const float norm2 = norm * norm; |
|
const float maxLevel = (float) ((1 << m) - 1); |
|
const F4::T vNorm = F4::set1(norm); |
|
const F4::T vNorm2 = F4::set1(norm2); |
|
const F4::T vMax = F4::set1(maxLevel); |
|
const F4::T vMin = F4::set1(-maxLevel); |
|
const F4::T vHalf = F4::set1(0.5f); |
|
const F4::T vTwo = F4::set1(2.0f); |
|
const F4::T vOne = F4::set1(1.0f); |
|
const F4::T vQuarter = F4::set1(0.25f); |
|
const F4::T vThreshold = F4::set1(outlierThreshold); |
|
const F4::T vInfinity = F4::set1(std::numeric_limits<float>::infinity()); |
|
const F4::T vZero = F4::set1(0.0f); |
|
int8_t bytes[4]; |
|
int c = 0; |
|
|
|
// Whole groups of 4 cells, then the rest as a padded group |
|
for (; c < nbCells; c += 4) |
|
{ |
|
const int n = std::min(4, nbCells - c); |
|
float r4[4] = {0.0f, 0.0f, 0.0f, 0.0f}, i4[4] = {0.0f, 0.0f, 0.0f, 0.0f}, s4[4] = {0.0f, 0.0f, 0.0f, 0.0f}; |
|
float q4[4] = {0.0f, 0.0f, 0.0f, 0.0f}; |
|
std::copy(re + c, re + c + n, r4); |
|
std::copy(im + c, im + c + n, i4); |
|
std::copy(scales + c, scales + c + n, s4); |
|
|
|
if (axes) { |
|
std::copy(scalesQ + c, scalesQ + c + n, q4); |
|
} |
|
|
|
const F4::T x = F4::mul(F4::load(r4), vNorm); // Unnormalised units |
|
const F4::T y = F4::mul(F4::load(i4), vNorm); |
|
// A scale that isn't finite (NaN, or infinite, from no noise at all) is no information: as the clamps of NaN |
|
// differ between processors, its LLRs would too |
|
F4::T scale = F4::load(s4); |
|
scale = F4::select(F4::greater(vInfinity, F4::abs(scale)), scale, vZero); |
|
F4::T scaleQ = scale; |
|
|
|
if (axes) |
|
{ |
|
scaleQ = F4::load(q4); |
|
scaleQ = F4::select(F4::greater(vInfinity, F4::abs(scaleQ)), scaleQ, vZero); |
|
} |
|
|
|
// Error power to the nearest point, normalised. Clamping before the floor makes no difference to the level, |
|
// and keeps the floor exact however far out the cell is |
|
auto axisError = [&](F4::T v) { |
|
F4::T half = F4::max(vMin, F4::min(vMax, F4::mul(v, vHalf))); |
|
F4::T level = F4::add(F4::mul(vTwo, F4::floor(half)), vOne); |
|
level = F4::max(vMin, F4::min(vMax, level)); |
|
return F4::sub(v, level); |
|
}; |
|
const F4::T ei = axisError(x), eq = axisError(y); |
|
const F4::T ei2 = F4::mul(ei, ei), eq2 = F4::mul(eq, eq); |
|
const F4::T error = F4::div(F4::add(ei2, eq2), vNorm2); |
|
float e4[4]; |
|
F4::store(e4, error); |
|
std::copy(e4, e4 + n, errors + c); |
|
|
|
// A cell much further from the nearest point than noise would put it is less reliable than it appears. With |
|
// a scale for each axis, each axis's error is relative to its own noise, and both are reduced alike |
|
if (outlierThreshold > 0.0f) |
|
{ |
|
if (axes) |
|
{ |
|
const F4::T relative = F4::mul(F4::div(F4::add(F4::mul(ei2, scale), F4::mul(eq2, scaleQ)), vNorm2), vQuarter); |
|
const F4::T over = F4::greater(relative, vThreshold); |
|
const F4::T factor = F4::div(vThreshold, relative); |
|
scale = F4::select(over, F4::mul(scale, factor), scale); |
|
scaleQ = F4::select(over, F4::mul(scaleQ, factor), scaleQ); |
|
F4::store(q4, scaleQ); |
|
std::copy(q4, q4 + n, scalesQ + c); |
|
} |
|
else |
|
{ |
|
const F4::T relative = F4::mul(F4::mul(error, scale), vQuarter); // Over the noise per dimension |
|
const F4::T limited = F4::mul(scale, F4::div(vThreshold, relative)); |
|
scale = F4::select(F4::greater(relative, vThreshold), limited, scale); |
|
} |
|
|
|
F4::store(s4, scale); |
|
std::copy(s4, s4 + n, scales + c); |
|
} |
|
|
|
// LLRs of each axis's bits: the sign, then |d - threshold| recursively (table 14's Gray mapping) |
|
const F4::T sI = F4::div(scale, vNorm2); |
|
const F4::T sQ = axes ? F4::div(scaleQ, vNorm2) : sI; |
|
|
|
for (int axis = 0; axis < 2; axis++) |
|
{ |
|
const F4::T s = axis == 0 ? sI : sQ; |
|
F4::T v = axis == 0 ? x : y; |
|
F4::T d = F4::abs(v); |
|
float threshold = (float) (1 << (m - 1)); |
|
F4::toInt8(F4::mul(s, v), bytes); |
|
|
|
for (int j = 0; j < n; j++) { |
|
llrs[bitMap[(c + j) * eta + axis]] = bytes[j]; |
|
} |
|
|
|
for (int k = 1; k < m; k++) |
|
{ |
|
const F4::T t = F4::set1(threshold); |
|
F4::toInt8(F4::mul(s, F4::sub(d, t)), bytes); |
|
|
|
for (int j = 0; j < n; j++) { |
|
llrs[bitMap[(c + j) * eta + 2 * k + axis]] = bytes[j]; |
|
} |
|
|
|
d = F4::abs(F4::sub(d, t)); |
|
threshold *= 0.5f; |
|
} |
|
} |
|
} |
|
} |
|
|
|
void demapCells(const float *re, const float *im, float *scales, int nbCells, int eta, float outlierThreshold, |
|
const int *bitMap, int8_t *llrs, float *errors) |
|
{ |
|
CellDemapper::demap<false>(re, im, scales, nullptr, nbCells, eta, outlierThreshold, bitMap, llrs, errors); |
|
} |
|
|
|
void demapCellsAxes(const float *re, const float *im, float *scalesI, float *scalesQ, int nbCells, int eta, |
|
float outlierThreshold, const int *bitMap, int8_t *llrs, float *errors) |
|
{ |
|
CellDemapper::demap<true>(re, im, scalesI, scalesQ, nbCells, eta, outlierThreshold, bitMap, llrs, errors); |
|
} |
|
|
|
} // namespace dvbt2
|
|
|