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.
 
 
 
 
 
 

327 lines
12 KiB

///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2026 Jon Beniston, M7RCE <jon@beniston.com> //
// Some code by AI //
// //
// 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/>. //
///////////////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <utility>
#if defined(__SSE2__) || defined(_M_X64) || (defined(_M_IX86_FP) && (_M_IX86_FP >= 2))
#define MODEMDR_VITERBI_SSE2 1
#include <emmintrin.h>
#elif defined(__aarch64__) || defined(_M_ARM64)
#define MODEMDR_VITERBI_NEON 1
#include <arm_neon.h>
#endif
#include "viterbi.h"
namespace modemdr
{
namespace
{
unsigned int parity(unsigned int v)
{
v ^= v >> 4;
v ^= v >> 2;
v ^= v >> 1;
return v & 1U;
}
// Set in the environment to use the scalar pass, to check the two agree
bool scalarRequested()
{
static const bool scalar = (std::getenv("MODEMDR_SCALAR") != nullptr) || (std::getenv("MODEMDAB_SCALAR") != nullptr);
return scalar;
}
} // anonymous namespace
Viterbi::Viterbi(const std::vector<unsigned int>& generators) :
m_outputs(static_cast<int>(std::min<std::size_t>(generators.size(), MaximumOutputs))),
m_generators(generators.begin(), generators.begin() + m_outputs)
{
for (int k = 0; k < MaximumOutputs; k++)
{
for (int j = 0; j < 32; j++) {
m_signs[k][j] = (k < m_outputs) && ((registerOutputs(2U * j) >> k) & 1U) ? -1.0f : 1.0f;
}
}
}
const std::vector<unsigned int>& Viterbi::dabGenerators()
{
static const std::vector<unsigned int> generators{0133, 0171, 0145, 0133};
return generators;
}
const std::vector<unsigned int>& Viterbi::drmGenerators()
{
static const std::vector<unsigned int> generators{0133, 0171, 0145, 0133, 0171, 0145};
return generators;
}
unsigned int Viterbi::registerOutputs(unsigned int reg) const
{
unsigned int out = 0;
for (int k = 0; k < m_outputs; k++) {
out |= parity(reg & m_generators[k]) << k;
}
return out;
}
// The trellis as butterflies: the predecessors of states j and j + 32 are states 2j and 2j + 1.
// The outputs of (input 1, state 2j) are those of (input 0, state 2j + 1), and flipping the oldest
// bit inverts them all: one branch metric m and its negation serve the four transitions.
// new[j] = max(old[2j] + m, old[2j + 1] - m)
// new[j + 32] = max(old[2j] - m, old[2j + 1] + m)
void Viterbi::forward(const float *soft, int steps, std::uint64_t *decisions, std::uint64_t *close, float closeLimit) const
{
#ifdef MODEMDR_VITERBI_SSE2
if (scalarRequested())
{
forwardScalar(soft, steps, decisions, close, closeLimit);
return;
}
alignas(16) float a[States];
alignas(16) float b[States];
float *metrics = a;
float *next = b;
const bool closeWanted = (close != nullptr) && (closeLimit > 0.0f);
const __m128 absMask = _mm_castsi128_ps(_mm_set1_epi32(0x7fffffff));
const __m128 limit = _mm_set1_ps(closeLimit);
const int n = m_outputs;
std::fill(a, a + States, -1.0e30f);
a[0] = 0.0f;
for (int t = 0; t < steps; t++)
{
__m128 s[MaximumOutputs];
for (int k = 0; k < n; k++) {
s[k] = _mm_set1_ps(soft[n * t + k]);
}
std::uint64_t stepDecisions = 0;
std::uint64_t closeStates = 0;
for (int block = 0; block < 8; block++)
{
const __m128 lo = _mm_load_ps(&metrics[8 * block]);
const __m128 hi = _mm_load_ps(&metrics[8 * block + 4]);
const __m128 even = _mm_shuffle_ps(lo, hi, _MM_SHUFFLE(2, 0, 2, 0));
const __m128 odd = _mm_shuffle_ps(lo, hi, _MM_SHUFFLE(3, 1, 3, 1));
const int j = 4 * block;
__m128 m = _mm_mul_ps(s[0], _mm_load_ps(&m_signs[0][j]));
for (int k = 1; k < n; k++) {
m = _mm_add_ps(m, _mm_mul_ps(s[k], _mm_load_ps(&m_signs[k][j])));
}
const __m128 evenPlus = _mm_add_ps(even, m);
const __m128 evenMinus = _mm_sub_ps(even, m);
const __m128 oddPlus = _mm_add_ps(odd, m);
const __m128 oddMinus = _mm_sub_ps(odd, m);
_mm_store_ps(&next[j], _mm_max_ps(evenPlus, oddMinus));
_mm_store_ps(&next[j + 32], _mm_max_ps(evenMinus, oddPlus));
if (closeWanted)
{
const __m128 lowDelta = _mm_and_ps(_mm_sub_ps(evenPlus, oddMinus), absMask);
const __m128 highDelta = _mm_and_ps(_mm_sub_ps(evenMinus, oddPlus), absMask);
closeStates |= (static_cast<std::uint64_t>(_mm_movemask_ps(_mm_cmplt_ps(lowDelta, limit))) << j)
| (static_cast<std::uint64_t>(_mm_movemask_ps(_mm_cmplt_ps(highDelta, limit))) << (j + 32));
}
const std::uint64_t low = static_cast<std::uint64_t>(_mm_movemask_ps(_mm_cmpgt_ps(oddMinus, evenPlus)));
const std::uint64_t high = static_cast<std::uint64_t>(_mm_movemask_ps(_mm_cmpgt_ps(oddPlus, evenMinus)));
stepDecisions |= (low << j) | (high << (j + 32));
}
decisions[t] = stepDecisions;
if (closeWanted) {
close[t] = closeStates;
}
// Keep the metrics near zero
if ((t & 31) == 31)
{
const __m128 reference = _mm_set1_ps(next[0]);
for (int i = 0; i < States; i += 4) {
_mm_store_ps(&next[i], _mm_sub_ps(_mm_load_ps(&next[i]), reference));
}
}
std::swap(metrics, next);
}
#elif defined(MODEMDR_VITERBI_NEON)
// As the SSE2 pass: the same operations in the same order, so the same decisions
if (scalarRequested())
{
forwardScalar(soft, steps, decisions, close, closeLimit);
return;
}
alignas(16) float a[States];
alignas(16) float b[States];
float *metrics = a;
float *next = b;
const bool closeWanted = (close != nullptr) && (closeLimit > 0.0f);
const float32x4_t limit = vdupq_n_f32(closeLimit);
static const std::uint32_t laneBitValues[4] = {1U, 2U, 4U, 8U};
const uint32x4_t laneBits = vld1q_u32(laneBitValues);
// A comparison's lanes as bits, lane i in bit i (SSE2's movemask)
const auto mask = [&laneBits](uint32x4_t compared) {
return static_cast<std::uint64_t>(vaddvq_u32(vandq_u32(compared, laneBits)));
};
const int n = m_outputs;
std::fill(a, a + States, -1.0e30f);
a[0] = 0.0f;
for (int t = 0; t < steps; t++)
{
float32x4_t s[MaximumOutputs];
for (int k = 0; k < n; k++) {
s[k] = vdupq_n_f32(soft[n * t + k]);
}
std::uint64_t stepDecisions = 0;
std::uint64_t closeStates = 0;
for (int block = 0; block < 8; block++)
{
const float32x4x2_t pairs = vld2q_f32(&metrics[8 * block]);
const float32x4_t even = pairs.val[0];
const float32x4_t odd = pairs.val[1];
const int j = 4 * block;
// Products with signs of +-1 are exact: fused or not, the sums are the same
float32x4_t m = vmulq_f32(s[0], vld1q_f32(&m_signs[0][j]));
for (int k = 1; k < n; k++) {
m = vaddq_f32(m, vmulq_f32(s[k], vld1q_f32(&m_signs[k][j])));
}
const float32x4_t evenPlus = vaddq_f32(even, m);
const float32x4_t evenMinus = vsubq_f32(even, m);
const float32x4_t oddPlus = vaddq_f32(odd, m);
const float32x4_t oddMinus = vsubq_f32(odd, m);
vst1q_f32(&next[j], vmaxq_f32(evenPlus, oddMinus));
vst1q_f32(&next[j + 32], vmaxq_f32(evenMinus, oddPlus));
if (closeWanted)
{
closeStates |= (mask(vcltq_f32(vabdq_f32(evenPlus, oddMinus), limit)) << j)
| (mask(vcltq_f32(vabdq_f32(evenMinus, oddPlus), limit)) << (j + 32));
}
stepDecisions |= (mask(vcgtq_f32(oddMinus, evenPlus)) << j) | (mask(vcgtq_f32(oddPlus, evenMinus)) << (j + 32));
}
decisions[t] = stepDecisions;
if (closeWanted) {
close[t] = closeStates;
}
// Keep the metrics near zero
if ((t & 31) == 31)
{
const float32x4_t reference = vdupq_n_f32(next[0]);
for (int i = 0; i < States; i += 4) {
vst1q_f32(&next[i], vsubq_f32(vld1q_f32(&next[i]), reference));
}
}
std::swap(metrics, next);
}
#else
forwardScalar(soft, steps, decisions, close, closeLimit);
#endif
}
void Viterbi::forwardScalar(const float *soft, int steps, std::uint64_t *decisions, std::uint64_t *close, float closeLimit) const
{
// The same butterflies as the SSE2 and NEON passes, in plain arrays that compilers vectorize
float a[States];
float b[States];
float *metrics = a;
float *next = b;
float m[32];
const bool closeWanted = (close != nullptr) && (closeLimit > 0.0f);
const int n = m_outputs;
std::fill(a, a + States, -1.0e30f);
a[0] = 0.0f;
for (int t = 0; t < steps; t++)
{
const float *s = soft + n * t;
for (int j = 0; j < 32; j++)
{
float sum = s[0] * m_signs[0][j];
for (int k = 1; k < n; k++) {
sum += s[k] * m_signs[k][j];
}
m[j] = sum;
}
std::uint64_t stepDecisions = 0;
std::uint64_t closeStates = 0;
for (int j = 0; j < 32; j++)
{
const float evenPlus = metrics[2 * j] + m[j];
const float evenMinus = metrics[2 * j] - m[j];
const float oddPlus = metrics[2 * j + 1] + m[j];
const float oddMinus = metrics[2 * j + 1] - m[j];
const bool low = oddMinus > evenPlus;
const bool high = oddPlus > evenMinus;
next[j] = low ? oddMinus : evenPlus;
next[j + 32] = high ? oddPlus : evenMinus;
if (closeWanted)
{
closeStates |= (static_cast<std::uint64_t>(std::fabs(evenPlus - oddMinus) < closeLimit) << j)
| (static_cast<std::uint64_t>(std::fabs(evenMinus - oddPlus) < closeLimit) << (j + 32));
}
stepDecisions |= (static_cast<std::uint64_t>(low) << j) | (static_cast<std::uint64_t>(high) << (j + 32));
}
decisions[t] = stepDecisions;
if (closeWanted) {
close[t] = closeStates;
}
if ((t & 31) == 31)
{
const float reference = next[0];
for (int i = 0; i < States; i++) {
next[i] -= reference;
}
}
std::swap(metrics, next);
}
}
void Viterbi::traceback(const std::uint64_t *decisions, int steps, int informationBits, std::uint8_t *bits, std::uint8_t *states)
{
int state = 0;
for (int t = steps - 1; t >= 0; t--)
{
if (states) {
states[t] = static_cast<std::uint8_t>(state);
}
if (t < informationBits) {
bits[t] = static_cast<std::uint8_t>(state >> 5);
}
const int d = static_cast<int>((decisions[t] >> state) & 1U);
state = ((state & 31) << 1) | d;
}
}
} // namespace modemdr