anr, anf: vectorize the LMS tap and update loops
Both loops indexed the delay line as (in_idx + j + delay) & mask, one
masked index per tap, which made the address non-affine and stopped the
vectorizer. The window wraps at most once, so split it at the wrap and
walk two contiguous runs instead.
As in resample.c, the y/sigma reduction cannot be reassociated without
-ffast-math (which this library must not enable, see linux_port.h), so
carry four independent accumulator pairs to break the FMA dependency
chain and let the vectorizer in.
in_buff and out_buff alias in RXA -- both are midbuff -- so only the
private d/w arrays are marked restrict.
Measured in situ on an Apple M1 Pro, 512-sample buffers, cost of turning
the block on, best of 5:
anr 59303 ns -> 20253 ns 2.93x
anf 55073 ns -> 20511 ns 2.68x
Summation order changes, so output is not bit-identical: over 300 buffers
of the full RX chain the worst deviation is 2.4e-07, an SNR of 153 dB.
An LMS filter is an adaptive feedback loop, so its trajectory is
chaotic. As a control, perturbing a single input sample of the unmodified
code by one ulp diverges it from itself by 6.6e-07, an SNR of 144.6 dB --
i.e. this change disturbs the filter less than the last bit of the input
does.
Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
Uladzimir Karpenka
@@ -26,6 +26,45 @@ warren@wpratt.com
|
||||
|
||||
#include "comm.h"
|
||||
|
||||
/* Filter output and tap-window energy over a unit-stride run of the delay line.
|
||||
|
||||
The delay line is indexed (in_idx + j + delay) & mask, which wraps at most
|
||||
once across the tap window; xanf() splits the window at the wrap so both
|
||||
halves are contiguous here. Four independent accumulator pairs keep the FMAs
|
||||
off a single dependency chain and let the vectorizer in -- a 'y += w[j]*x[j]'
|
||||
reduction cannot be reassociated without -ffast-math, which this library must
|
||||
not enable (it relies on IEEE semantics for 0/0 = NaN and x/0 = Inf). */
|
||||
static inline void anf_dot (const double* WDSP_RESTRICT w,
|
||||
const double* WDSP_RESTRICT x, int n, double* py, double* psigma)
|
||||
{
|
||||
double y0 = 0.0, y1 = 0.0, y2 = 0.0, y3 = 0.0;
|
||||
double s0 = 0.0, s1 = 0.0, s2 = 0.0, s3 = 0.0;
|
||||
int j = 0;
|
||||
for (; j <= n - 4; j += 4)
|
||||
{
|
||||
y0 += w[j + 0] * x[j + 0]; s0 += x[j + 0] * x[j + 0];
|
||||
y1 += w[j + 1] * x[j + 1]; s1 += x[j + 1] * x[j + 1];
|
||||
y2 += w[j + 2] * x[j + 2]; s2 += x[j + 2] * x[j + 2];
|
||||
y3 += w[j + 3] * x[j + 3]; s3 += x[j + 3] * x[j + 3];
|
||||
}
|
||||
for (; j < n; j++)
|
||||
{
|
||||
y0 += w[j] * x[j];
|
||||
s0 += x[j] * x[j];
|
||||
}
|
||||
*py += (y0 + y1) + (y2 + y3);
|
||||
*psigma += (s0 + s1) + (s2 + s3);
|
||||
}
|
||||
|
||||
/* Leaky-LMS tap update over the same unit-stride run. */
|
||||
static inline void anf_update (double* WDSP_RESTRICT w,
|
||||
const double* WDSP_RESTRICT x, int n, double c0, double c1)
|
||||
{
|
||||
int j;
|
||||
for (j = 0; j < n; j++)
|
||||
w[j] = c0 * w[j] + c1 * x[j];
|
||||
}
|
||||
|
||||
ANF create_anf (
|
||||
int run,
|
||||
int position,
|
||||
@@ -81,53 +120,81 @@ void destroy_anf (ANF a)
|
||||
|
||||
void xanf(ANF a, int position)
|
||||
{
|
||||
int i, j, idx;
|
||||
int i;
|
||||
double c0, c1;
|
||||
double y, error, sigma, inv_sigp;
|
||||
double nel, nev;
|
||||
if (a->run && (a->position == position))
|
||||
{
|
||||
for (i = 0; i < a->buff_size; i++)
|
||||
const int n_taps = a->n_taps;
|
||||
const int dline_size = a->dline_size;
|
||||
const int mask = a->mask;
|
||||
const int delay = a->delay;
|
||||
const int buff_size = a->buff_size;
|
||||
const double two_mu = a->two_mu;
|
||||
const double gamma = a->gamma;
|
||||
const double den_mult = a->den_mult;
|
||||
const double lincr = a->lincr;
|
||||
const double ldecr = a->ldecr;
|
||||
const double lidx_min = a->lidx_min;
|
||||
const double lidx_max = a->lidx_max;
|
||||
/* in_buff and out_buff are the same buffer in RXA, so neither may be
|
||||
marked restrict; d and w are private to the struct. */
|
||||
const double* in_buff = a->in_buff;
|
||||
double* out_buff = a->out_buff;
|
||||
double* WDSP_RESTRICT d = a->d;
|
||||
double* WDSP_RESTRICT w = a->w;
|
||||
int in_idx = a->in_idx;
|
||||
double lidx = a->lidx;
|
||||
double ngamma = a->ngamma;
|
||||
|
||||
for (i = 0; i < buff_size; i++)
|
||||
{
|
||||
a->d[a->in_idx] = a->in_buff[2 * i + 0];
|
||||
double dsamp;
|
||||
int base, n1;
|
||||
|
||||
y = 0;
|
||||
sigma = 0;
|
||||
dsamp = in_buff[2 * i + 0];
|
||||
d[in_idx] = dsamp;
|
||||
|
||||
base = (in_idx + delay) & mask;
|
||||
if ((n1 = dline_size - base) > n_taps) n1 = n_taps;
|
||||
|
||||
y = 0.0;
|
||||
sigma = 0.0;
|
||||
anf_dot (w, d + base, n1, &y, &sigma);
|
||||
if (n1 < n_taps)
|
||||
anf_dot (w + n1, d, n_taps - n1, &y, &sigma);
|
||||
|
||||
for (j = 0; j < a->n_taps; j++)
|
||||
{
|
||||
idx = (a->in_idx + j + a->delay) & a->mask;
|
||||
y += a->w[j] * a->d[idx];
|
||||
sigma += a->d[idx] * a->d[idx];
|
||||
}
|
||||
inv_sigp = 1.0 / (sigma + 1e-10);
|
||||
error = a->d[a->in_idx] - y;
|
||||
error = dsamp - y;
|
||||
|
||||
a->out_buff[2 * i + 0] = error;
|
||||
a->out_buff[2 * i + 1] = 0.0;
|
||||
out_buff[2 * i + 0] = error;
|
||||
out_buff[2 * i + 1] = 0.0;
|
||||
|
||||
if((nel = error * (1.0 - a->two_mu * sigma * inv_sigp)) < 0.0) nel = -nel;
|
||||
if((nev = a->d[a->in_idx] - (1.0 - a->two_mu * a->ngamma) * y - a->two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev;
|
||||
if((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0) nel = -nel;
|
||||
if((nev = dsamp - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev;
|
||||
if (nev < nel)
|
||||
{
|
||||
if ((a->lidx += a->lincr) > a->lidx_max) a->lidx = a->lidx_max;
|
||||
if ((lidx += lincr) > lidx_max) lidx = lidx_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((a->lidx -= a->ldecr) < a->lidx_min) a->lidx = a->lidx_min;
|
||||
if ((lidx -= ldecr) < lidx_min) lidx = lidx_min;
|
||||
}
|
||||
a->ngamma = a->gamma * (a->lidx * a->lidx) * (a->lidx * a->lidx) * a->den_mult;
|
||||
ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult;
|
||||
|
||||
c0 = 1.0 - a->two_mu * a->ngamma;
|
||||
c1 = a->two_mu * error * inv_sigp;
|
||||
c0 = 1.0 - two_mu * ngamma;
|
||||
c1 = two_mu * error * inv_sigp;
|
||||
|
||||
for (j = 0; j < a->n_taps; j++)
|
||||
{
|
||||
idx = (a->in_idx + j + a->delay) & a->mask;
|
||||
a->w[j] = c0 * a->w[j] + c1 * a->d[idx];
|
||||
}
|
||||
a->in_idx = (a->in_idx + a->mask) & a->mask;
|
||||
anf_update (w, d + base, n1, c0, c1);
|
||||
if (n1 < n_taps)
|
||||
anf_update (w + n1, d, n_taps - n1, c0, c1);
|
||||
|
||||
in_idx = (in_idx + mask) & mask;
|
||||
}
|
||||
a->in_idx = in_idx;
|
||||
a->lidx = lidx;
|
||||
a->ngamma = ngamma;
|
||||
}
|
||||
else if (a->in_buff != a->out_buff)
|
||||
memcpy (a->out_buff, a->in_buff, a->buff_size * sizeof (complex));
|
||||
|
||||
@@ -26,6 +26,45 @@ warren@wpratt.com
|
||||
|
||||
#include "comm.h"
|
||||
|
||||
/* Filter output and tap-window energy over a unit-stride run of the delay line.
|
||||
|
||||
The delay line is indexed (in_idx + j + delay) & mask, which wraps at most
|
||||
once across the tap window; xanr() splits the window at the wrap so both
|
||||
halves are contiguous here. Four independent accumulator pairs keep the FMAs
|
||||
off a single dependency chain and let the vectorizer in -- an 'y += w[j]*x[j]'
|
||||
reduction cannot be reassociated without -ffast-math, which this library must
|
||||
not enable (it relies on IEEE semantics for 0/0 = NaN and x/0 = Inf). */
|
||||
static inline void anr_dot (const double* WDSP_RESTRICT w,
|
||||
const double* WDSP_RESTRICT x, int n, double* py, double* psigma)
|
||||
{
|
||||
double y0 = 0.0, y1 = 0.0, y2 = 0.0, y3 = 0.0;
|
||||
double s0 = 0.0, s1 = 0.0, s2 = 0.0, s3 = 0.0;
|
||||
int j = 0;
|
||||
for (; j <= n - 4; j += 4)
|
||||
{
|
||||
y0 += w[j + 0] * x[j + 0]; s0 += x[j + 0] * x[j + 0];
|
||||
y1 += w[j + 1] * x[j + 1]; s1 += x[j + 1] * x[j + 1];
|
||||
y2 += w[j + 2] * x[j + 2]; s2 += x[j + 2] * x[j + 2];
|
||||
y3 += w[j + 3] * x[j + 3]; s3 += x[j + 3] * x[j + 3];
|
||||
}
|
||||
for (; j < n; j++)
|
||||
{
|
||||
y0 += w[j] * x[j];
|
||||
s0 += x[j] * x[j];
|
||||
}
|
||||
*py += (y0 + y1) + (y2 + y3);
|
||||
*psigma += (s0 + s1) + (s2 + s3);
|
||||
}
|
||||
|
||||
/* Leaky-LMS tap update over the same unit-stride run. */
|
||||
static inline void anr_update (double* WDSP_RESTRICT w,
|
||||
const double* WDSP_RESTRICT x, int n, double c0, double c1)
|
||||
{
|
||||
int j;
|
||||
for (j = 0; j < n; j++)
|
||||
w[j] = c0 * w[j] + c1 * x[j];
|
||||
}
|
||||
|
||||
ANR create_anr (
|
||||
int run,
|
||||
int position,
|
||||
@@ -81,53 +120,81 @@ void destroy_anr (ANR a)
|
||||
|
||||
void xanr (ANR a, int position)
|
||||
{
|
||||
int i, j, idx;
|
||||
int i;
|
||||
double c0, c1;
|
||||
double y, error, sigma, inv_sigp;
|
||||
double nel, nev;
|
||||
if (a->run && (a->position == position))
|
||||
{
|
||||
for (i = 0; i < a->buff_size; i++)
|
||||
const int n_taps = a->n_taps;
|
||||
const int dline_size = a->dline_size;
|
||||
const int mask = a->mask;
|
||||
const int delay = a->delay;
|
||||
const int buff_size = a->buff_size;
|
||||
const double two_mu = a->two_mu;
|
||||
const double gamma = a->gamma;
|
||||
const double den_mult = a->den_mult;
|
||||
const double lincr = a->lincr;
|
||||
const double ldecr = a->ldecr;
|
||||
const double lidx_min = a->lidx_min;
|
||||
const double lidx_max = a->lidx_max;
|
||||
/* in_buff and out_buff are the same buffer in RXA, so neither may be
|
||||
marked restrict; d and w are private to the struct. */
|
||||
const double* in_buff = a->in_buff;
|
||||
double* out_buff = a->out_buff;
|
||||
double* WDSP_RESTRICT d = a->d;
|
||||
double* WDSP_RESTRICT w = a->w;
|
||||
int in_idx = a->in_idx;
|
||||
double lidx = a->lidx;
|
||||
double ngamma = a->ngamma;
|
||||
|
||||
for (i = 0; i < buff_size; i++)
|
||||
{
|
||||
a->d[a->in_idx] = a->in_buff[2 * i + 0];
|
||||
double dsamp;
|
||||
int base, n1;
|
||||
|
||||
y = 0;
|
||||
sigma = 0;
|
||||
dsamp = in_buff[2 * i + 0];
|
||||
d[in_idx] = dsamp;
|
||||
|
||||
base = (in_idx + delay) & mask;
|
||||
if ((n1 = dline_size - base) > n_taps) n1 = n_taps;
|
||||
|
||||
y = 0.0;
|
||||
sigma = 0.0;
|
||||
anr_dot (w, d + base, n1, &y, &sigma);
|
||||
if (n1 < n_taps)
|
||||
anr_dot (w + n1, d, n_taps - n1, &y, &sigma);
|
||||
|
||||
for (j = 0; j < a->n_taps; j++)
|
||||
{
|
||||
idx = (a->in_idx + j + a->delay) & a->mask;
|
||||
y += a->w[j] * a->d[idx];
|
||||
sigma += a->d[idx] * a->d[idx];
|
||||
}
|
||||
inv_sigp = 1.0 / (sigma + 1e-10);
|
||||
error = a->d[a->in_idx] - y;
|
||||
error = dsamp - y;
|
||||
|
||||
a->out_buff[2 * i + 0] = y;
|
||||
a->out_buff[2 * i + 1] = 0.0;
|
||||
out_buff[2 * i + 0] = y;
|
||||
out_buff[2 * i + 1] = 0.0;
|
||||
|
||||
if((nel = error * (1.0 - a->two_mu * sigma * inv_sigp)) < 0.0) nel = -nel;
|
||||
if((nev = a->d[a->in_idx] - (1.0 - a->two_mu * a->ngamma) * y - a->two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev;
|
||||
if((nel = error * (1.0 - two_mu * sigma * inv_sigp)) < 0.0) nel = -nel;
|
||||
if((nev = dsamp - (1.0 - two_mu * ngamma) * y - two_mu * error * sigma * inv_sigp) < 0.0) nev = -nev;
|
||||
if (nev < nel)
|
||||
{
|
||||
if ((a->lidx += a->lincr) > a->lidx_max) a->lidx = a->lidx_max;
|
||||
if ((lidx += lincr) > lidx_max) lidx = lidx_max;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((a->lidx -= a->ldecr) < a->lidx_min) a->lidx = a->lidx_min;
|
||||
if ((lidx -= ldecr) < lidx_min) lidx = lidx_min;
|
||||
}
|
||||
a->ngamma = a->gamma * (a->lidx * a->lidx) * (a->lidx * a->lidx) * a->den_mult;
|
||||
ngamma = gamma * (lidx * lidx) * (lidx * lidx) * den_mult;
|
||||
|
||||
c0 = 1.0 - a->two_mu * a->ngamma;
|
||||
c1 = a->two_mu * error * inv_sigp;
|
||||
c0 = 1.0 - two_mu * ngamma;
|
||||
c1 = two_mu * error * inv_sigp;
|
||||
|
||||
for (j = 0; j < a->n_taps; j++)
|
||||
{
|
||||
idx = (a->in_idx + j + a->delay) & a->mask;
|
||||
a->w[j] = c0 * a->w[j] + c1 * a->d[idx];
|
||||
}
|
||||
a->in_idx = (a->in_idx + a->mask) & a->mask;
|
||||
anr_update (w, d + base, n1, c0, c1);
|
||||
if (n1 < n_taps)
|
||||
anr_update (w + n1, d, n_taps - n1, c0, c1);
|
||||
|
||||
in_idx = (in_idx + mask) & mask;
|
||||
}
|
||||
a->in_idx = in_idx;
|
||||
a->lidx = lidx;
|
||||
a->ngamma = ngamma;
|
||||
}
|
||||
else if (a->in_buff != a->out_buff)
|
||||
memcpy (a->out_buff, a->in_buff, a->buff_size * sizeof (complex));
|
||||
|
||||
Reference in New Issue
Block a user