diff --git a/varsamp.c b/varsamp.c index bbfcf63..aeef36e 100644 --- a/varsamp.c +++ b/varsamp.c @@ -59,10 +59,23 @@ void calc_varsamp (VARSAMP a) fc_norm_low = a->fc_low / norm_rate; a->rsize = (int)(140.0 * norm_rate / min_rate); a->ncoef = a->rsize + 1; - a->ncoef += (a->R - 1) * (a->ncoef - 1); - a->h = fir_bandpass(a->ncoef, fc_norm_low, fc_norm_high, (double)a->R, 1, 0, (double)a->R * a->gain); - // print_impulse ("imp.txt", a->ncoef, a->h, 0, 0); - a->ring = (double *)malloc0(a->rsize * sizeof(complex)); + a->ncoef += (a->R - 1) * (a->ncoef - 1); // = R * rsize + 1 + { + /* Store the coefficients transposed into phases. hshift() walks + h[hidx + m*R] for m = 0..rsize-1, which strides by R doubles -- 8 KB + at R = 1024 -- over a 1.1 MB table, so every tap is its own cache + line. Transposing makes each phase contiguous; hshift() interpolates + between phases hidx and hidx+1, hence R+1 of them. */ + int p, m; + double* h = fir_bandpass(a->ncoef, fc_norm_low, fc_norm_high, (double)a->R, 1, 0, (double)a->R * a->gain); + a->hp = (double *)malloc0 ((size_t)(a->R + 1) * a->rsize * sizeof (double)); + for (p = 0; p <= a->R; p++) + for (m = 0; m < a->rsize; m++) + a->hp[(size_t)p * a->rsize + m] = h[p + (size_t)m * a->R]; + _aligned_free (h); + } + a->ringI = (double *)malloc0(a->rsize * sizeof(double)); + a->ringQ = (double *)malloc0(a->rsize * sizeof(double)); a->idx_in = a->rsize - 1; a->h_offset = 0.0; a->hs = (double *)malloc0 (a->rsize * sizeof (double)); @@ -72,8 +85,9 @@ void calc_varsamp (VARSAMP a) void decalc_varsamp (VARSAMP a) { _aligned_free (a->hs); - _aligned_free (a->ring); - _aligned_free (a->h); + _aligned_free (a->ringQ); + _aligned_free (a->ringI); + _aligned_free (a->hp); } VARSAMP create_varsamp ( int run, int size, double* in, double* out, @@ -105,22 +119,60 @@ void destroy_varsamp (VARSAMP a) void flush_varsamp (VARSAMP a) { - memset (a->ring, 0, a->rsize * sizeof (complex)); + memset (a->ringI, 0, a->rsize * sizeof (double)); + memset (a->ringQ, 0, a->rsize * sizeof (double)); a->idx_in = a->rsize - 1; a->h_offset = 0.0; a->isamps = 0.0; } +/* Accumulate n taps of a unit-stride complex dot product into *pI / *pQ. + + Four independent accumulator pairs keep the FMAs off a single dependency + chain and let the vectorizer in: an 'I += h[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 varsamp_dot (const double* WDSP_RESTRICT hp, + const double* WDSP_RESTRICT xI, const double* WDSP_RESTRICT xQ, + int n, double* pI, double* pQ) +{ + double i0 = 0.0, i1 = 0.0, i2 = 0.0, i3 = 0.0; + double q0 = 0.0, q1 = 0.0, q2 = 0.0, q3 = 0.0; + int j = 0; + for (; j <= n - 4; j += 4) + { + i0 += hp[j + 0] * xI[j + 0]; q0 += hp[j + 0] * xQ[j + 0]; + i1 += hp[j + 1] * xI[j + 1]; q1 += hp[j + 1] * xQ[j + 1]; + i2 += hp[j + 2] * xI[j + 2]; q2 += hp[j + 2] * xQ[j + 2]; + i3 += hp[j + 3] * xI[j + 3]; q3 += hp[j + 3] * xQ[j + 3]; + } + for (; j < n; j++) + { + i0 += hp[j] * xI[j]; + q0 += hp[j] * xQ[j]; + } + *pI += (i0 + i1) + (i2 + i3); + *pQ += (q0 + q1) + (q2 + q3); +} + void hshift (VARSAMP a) { - int i, j, k; + int m; int hidx; double frac, pos; + const int rsize = a->rsize; + const double* WDSP_RESTRICT h0; + const double* WDSP_RESTRICT h1; + double* WDSP_RESTRICT hs = a->hs; + /* h_offset is normalized to [0,1) by the caller, so hidx is in [0, R-1] + and phase hidx+1 <= R exists. */ pos = (double)a->R * a->h_offset; hidx = (int)(pos); frac = pos - (double)hidx; - for (i = a->rsize - 1, j = hidx, k = hidx + 1; i >= 0; i--, j += a->R, k += a->R) - a->hs[i] = a->h[j] + frac * (a->h[k] - a->h[j]); + h0 = a->hp + (size_t)hidx * rsize; + h1 = h0 + rsize; + for (m = 0; m < rsize; m++) + hs[rsize - 1 - m] = h0[m] + frac * (h1[m] - h0[m]); } int xvarsamp (VARSAMP a, double var) @@ -140,13 +192,21 @@ int xvarsamp (VARSAMP a, double var) else a->dicvar = 0.0; if (a->run) { - int i, j; - int idx_out; + int i, n1; double I, Q; + const int rsize = a->rsize; + /* a->hs is rewritten by hshift() on every output sample, so it must not + be hoisted behind a restrict pointer here; varsamp_dot() re-reads it. */ + const double* in = a->in; + double* out = a->out; + double* WDSP_RESTRICT ringI = a->ringI; + double* WDSP_RESTRICT ringQ = a->ringQ; + int idx_in = a->idx_in; + for (i = 0; i < a->size; i++) { - a->ring[2 * a->idx_in + 0] = a->in[2 * i + 0]; - a->ring[2 * a->idx_in + 1] = a->in[2 * i + 1]; + ringI[idx_in] = in[2 * i + 0]; + ringQ[idx_in] = in[2 * i + 1]; a->inv_cvar += a->dicvar; picvar = (uint64_t*)(&a->inv_cvar); N = *picvar & 0xffffffffffff0000; @@ -160,20 +220,21 @@ int xvarsamp (VARSAMP a, double var) a->h_offset += a->delta; while (a->h_offset >= 1.0) a->h_offset -= 1.0; while (a->h_offset < 0.0) a->h_offset += 1.0; - for (j = 0; j < a->rsize; j++) - { - if ((idx_out = a->idx_in + j) >= a->rsize) idx_out -= a->rsize; - I += a->hs[j] * a->ring[2 * idx_out + 0]; - Q += a->hs[j] * a->ring[2 * idx_out + 1]; - } - a->out[2 * outsamps + 0] = I; - a->out[2 * outsamps + 1] = Q; + /* the ring wraps at most once over rsize taps; split it so both + halves are unit-stride */ + n1 = rsize - idx_in; + varsamp_dot (a->hs, ringI + idx_in, ringQ + idx_in, n1, &I, &Q); + if (n1 < rsize) + varsamp_dot (a->hs + n1, ringI, ringQ, rsize - n1, &I, &Q); + out[2 * outsamps + 0] = I; + out[2 * outsamps + 1] = Q; outsamps++; a->isamps += a->inv_cvar; } a->isamps -= 1.0; - if (--a->idx_in < 0) a->idx_in = a->rsize - 1; + if (--idx_in < 0) idx_in = rsize - 1; } + a->idx_in = idx_in; } else if (a->in != a->out) memcpy (a->out, a->in, a->size * sizeof (complex)); diff --git a/varsamp.h b/varsamp.h index 63f55db..daef291 100644 --- a/varsamp.h +++ b/varsamp.h @@ -41,9 +41,13 @@ typedef struct _varsamp double gain; int idx_in; int ncoef; - double* h; + double* hp; // coefficients, polyphase: hp[p * rsize + m] = h[p + m * R], + // p = 0..R. hshift() then reads two adjacent phases + // contiguously instead of striding by R. int rsize; - double* ring; + double* ringI; // ring buffer, in-phase + double* ringQ; // ring buffer, quadrature (split from I so the tap loop + // reads unit-stride and vectorizes) double var; int varmode; double cvar;