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.
286 lines
9.7 KiB
286 lines
9.7 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 "cpufeatures.h" |
|
#include "resampler.h" |
|
#include "resamplerkernel.h" |
|
|
|
#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(_M_ARM64) |
|
#include <arm_neon.h> |
|
#define RESAMPLER_NEON |
|
#elif defined(USE_SSE2) || defined(__SSE2__) || defined(_M_X64) |
|
#include <emmintrin.h> |
|
#define RESAMPLER_SSE2 |
|
#endif |
|
|
|
namespace dvbt { |
|
|
|
const double Resampler::m_pi = 3.141592653589793; |
|
const double Resampler::m_attenuation = 70.0; |
|
const double Resampler::m_kaiserBeta = 0.1102 * (m_attenuation - 8.7); |
|
|
|
double Resampler::besselI0(double x) |
|
{ |
|
double sum = 1.0, term = 1.0; |
|
|
|
for (int k = 1; k < 50; k++) |
|
{ |
|
term *= (x / (2.0 * k)) * (x / (2.0 * k)); |
|
sum += term; |
|
|
|
if (term < sum * 1e-12) { |
|
break; |
|
} |
|
} |
|
|
|
return sum; |
|
} |
|
|
|
// The filter for one output sample: a dot product of the input with a phase of the filter, 2 complex taps at a time |
|
struct DotBase |
|
{ |
|
#if defined(RESAMPLER_SSE2) |
|
static void dot(const float *x, const float *h, int taps, float *out) |
|
{ |
|
__m128 acc = _mm_setzero_ps(); |
|
|
|
for (int n = 0; n < 2 * taps; n += 4) { |
|
acc = _mm_add_ps(acc, _mm_mul_ps(_mm_loadu_ps(x + n), _mm_loadu_ps(h + n))); |
|
} |
|
|
|
// acc holds re, im of even taps and re, im of odd taps |
|
acc = _mm_add_ps(acc, _mm_movehl_ps(acc, acc)); |
|
_mm_storel_pi(reinterpret_cast<__m64 *>(out), acc); |
|
} |
|
#elif defined(RESAMPLER_NEON) |
|
static void dot(const float *x, const float *h, int taps, float *out) |
|
{ |
|
float32x4_t acc = vdupq_n_f32(0.0f); |
|
|
|
for (int n = 0; n < 2 * taps; n += 4) { |
|
acc = vmlaq_f32(acc, vld1q_f32(x + n), vld1q_f32(h + n)); |
|
} |
|
|
|
// acc holds re, im of even taps and re, im of odd taps |
|
vst1_f32(out, vadd_f32(vget_low_f32(acc), vget_high_f32(acc))); |
|
} |
|
#else |
|
static void dot(const float *x, const float *h, int taps, float *out) |
|
{ |
|
float re = 0.0f, im = 0.0f; |
|
|
|
for (int n = 0; n < 2 * taps; n += 2) |
|
{ |
|
re += x[n] * h[n]; |
|
im += x[n + 1] * h[n + 1]; |
|
} |
|
|
|
out[0] = re; |
|
out[1] = im; |
|
} |
|
#endif |
|
}; |
|
|
|
Resampler::Resampler() : |
|
m_taps(0), |
|
m_avx2(modemdvb::cpuHasAVX2()), |
|
m_step(1.0), |
|
m_nominalStep(1.0), |
|
m_correction(1.0), |
|
m_time(0.0) |
|
{ |
|
} |
|
|
|
void Resampler::configure(double inputRate, double outputRate, double passband) |
|
{ |
|
// Content above the stopband would alias onto the passband, at whichever rate is lower |
|
double stopband = std::min(inputRate, outputRate) - passband; |
|
double cutoff = (passband + stopband) / 2.0 / inputRate; // Cycles per input sample |
|
double transition = std::max(0.02, (stopband - passband) / inputRate); |
|
int length = (int) std::ceil((m_attenuation - 7.95) / (14.36 * transition)); // Kaiser estimate, in input samples |
|
|
|
m_taps = std::max(4, std::min(64, length + 1)); |
|
m_taps = (m_taps + 3) & ~3; // A multiple of 4, for SIMD |
|
m_filter.assign(m_phases * m_taps, 0.0f); |
|
// Phase p delays by p / m_phases of an input sample. Tap n multiplies input sample i0 - m_taps/2 + 1 + n |
|
for (int p = 0; p < m_phases; p++) |
|
{ |
|
double frac = (double) p / m_phases; |
|
double sum = 0.0; |
|
|
|
for (int n = 0; n < m_taps; n++) |
|
{ |
|
double x = (n - m_taps / 2 + 1) - frac; // Distance from the output time, in input samples |
|
double arg = 2.0 * x / m_taps; |
|
double window = std::abs(arg) < 1.0 ? besselI0(m_kaiserBeta * std::sqrt(1.0 - arg * arg)) / besselI0(m_kaiserBeta) : 0.0; |
|
double y = 2.0 * m_pi * cutoff * x; |
|
double sinc = std::abs(y) < 1e-12 ? 1.0 : std::sin(y) / y; |
|
double h = 2.0 * cutoff * sinc * window; |
|
m_filter[p * m_taps + n] = (float) h; |
|
sum += h; |
|
} |
|
|
|
for (int n = 0; n < m_taps; n++) { |
|
m_filter[p * m_taps + n] /= (float) sum; // Unity gain at DC for every phase |
|
} |
|
} |
|
|
|
m_filterPairs.resize(2 * m_filter.size()); |
|
|
|
for (size_t i = 0; i < m_filter.size(); i++) |
|
{ |
|
m_filterPairs[2 * i] = m_filter[i]; |
|
m_filterPairs[2 * i + 1] = m_filter[i]; |
|
} |
|
|
|
m_nominalStep = inputRate / outputRate; |
|
m_step = m_nominalStep * m_correction; |
|
reset(); |
|
} |
|
|
|
void Resampler::setRatioCorrection(double correction) |
|
{ |
|
// Clock offsets are a few ppm, so anything far beyond is an error, which mustn't stop the resampler working |
|
if (!std::isfinite(correction)) { |
|
correction = 1.0; |
|
} |
|
|
|
m_correction = std::max(0.99, std::min(1.01, correction)); |
|
m_step = m_nominalStep * m_correction; |
|
} |
|
|
|
void Resampler::reset() |
|
{ |
|
m_history.assign(m_taps, Cf(0.0f, 0.0f)); |
|
m_time = m_taps / 2 - 1; |
|
} |
|
|
|
void Resampler::prime(const std::vector<Cf>& previous) |
|
{ |
|
reset(); |
|
int n = std::min((int) previous.size(), m_taps); |
|
std::copy(previous.end() - n, previous.end(), m_history.end() - n); |
|
m_time = m_taps; // The first new sample |
|
} |
|
|
|
void Resampler::process(const Cf *in, int nbIn, std::vector<Cf>& out) |
|
{ |
|
const size_t n = out.size(); |
|
const int maxOut = maxOutputs(nbIn); |
|
out.resize(n + maxOut); |
|
const int made = process(in, nbIn, out.data() + n, maxOut); |
|
out.resize(n + made); |
|
} |
|
|
|
int Resampler::maxOutputs(int nbIn) const |
|
{ |
|
return resampleMaxOutputs((int) m_history.size() + nbIn, m_time, m_step); |
|
} |
|
|
|
void makePolyphaseFilter(int taps, int phases, const std::function<double(double)>& impulse, std::vector<float>& pairs) |
|
{ |
|
pairs.assign(2 * (size_t) phases * taps, 0.0f); |
|
std::vector<double> h(taps); |
|
|
|
// As configure(): phase p delays by p / phases of an input sample, tap n multiplies input sample i0 - taps/2 + 1 + n |
|
for (int p = 0; p < phases; p++) |
|
{ |
|
const double frac = (double) p / phases; |
|
double sum = 0.0; |
|
|
|
for (int n = 0; n < taps; n++) |
|
{ |
|
h[n] = impulse((n - taps / 2 + 1) - frac); |
|
sum += h[n]; |
|
} |
|
|
|
for (int n = 0; n < taps; n++) |
|
{ |
|
const float v = (float) (sum != 0.0 ? h[n] / sum : h[n]); |
|
pairs[2 * ((size_t) p * taps + n)] = v; |
|
pairs[2 * ((size_t) p * taps + n) + 1] = v; |
|
} |
|
} |
|
} |
|
|
|
int resampleFilter(const std::complex<float> *samples, int offset, int available, const float *pairs, int taps, |
|
int phases, double& time, double step, std::complex<float> *out, int maxOut) |
|
{ |
|
#if defined(MODEMDVB_X86) |
|
static const bool avx2 = modemdvb::cpuHasAVX2(); |
|
|
|
if (avx2) { |
|
return resampleAVX2(samples, offset, available, pairs, taps, phases, time, step, out, maxOut); |
|
} |
|
#endif |
|
|
|
return resampleRun<DotBase>(samples, offset, available, pairs, taps, phases, time, step, out, maxOut); |
|
} |
|
|
|
// Input sample j (from m_history[0], which m_time is from) at samples[j - offset], to available |
|
int Resampler::run(const Cf *samples, int offset, int available, Cf *out, int maxOut) |
|
{ |
|
#if defined(MODEMDVB_X86) |
|
if (m_avx2) { |
|
return resampleAVX2(samples, offset, available, m_filterPairs.data(), m_taps, m_phases, m_time, m_step, out, maxOut); |
|
} |
|
#endif |
|
|
|
return resampleRun<DotBase>(samples, offset, available, m_filterPairs.data(), m_taps, m_phases, m_time, m_step, out, maxOut); |
|
} |
|
|
|
// The input follows the history. The outputs whose taps include the history are made from a copy of it with the start |
|
// of the input, and the rest from the input itself, so it isn't copied. Either way, m_time is from m_history[0], so |
|
// the outputs are the same |
|
int Resampler::process(const Cf *in, int nbIn, Cf *out, int maxOut) |
|
{ |
|
const int h = (int) m_history.size(); |
|
const int half = m_taps / 2 - 1; |
|
const int available = h + nbIn; |
|
// Those that stop for want of the rest of the head have their first tap past the history |
|
const int head = std::min(nbIn, m_taps); |
|
m_staging.assign(m_history.begin(), m_history.end()); |
|
m_staging.insert(m_staging.end(), in, in + head); |
|
int made = run(m_staging.data(), 0, h + head, out, maxOut); |
|
|
|
if (head < nbIn) { |
|
made += run(in, h, available, out + made, maxOut - made); |
|
} |
|
|
|
// Keep what is still needed |
|
int consumed = std::max(0, (int) m_time - half); |
|
consumed = std::min(consumed, available); |
|
|
|
if (consumed >= h) |
|
{ |
|
m_history.assign(in + (consumed - h), in + nbIn); |
|
} |
|
else |
|
{ |
|
m_history.erase(m_history.begin(), m_history.begin() + consumed); |
|
m_history.insert(m_history.end(), in, in + nbIn); |
|
} |
|
|
|
m_time -= consumed; |
|
return made; |
|
} |
|
|
|
} // namespace dvbt
|
|
|