diff --git a/anf.c b/anf.c index 192b9d6..17c28d9 100644 --- a/anf.c +++ b/anf.c @@ -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)); diff --git a/anr.c b/anr.c index 538fa2c..a0d8133 100644 --- a/anr.c +++ b/anr.c @@ -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));