diff options
Diffstat (limited to 'rfft.c')
| -rw-r--r-- | rfft.c | 190 |
1 files changed, 190 insertions, 0 deletions
@@ -0,0 +1,190 @@ +#include "rfft.h" + +#include <math.h> +#include <stddef.h> + +#ifndef M_PI +# define M_PI 3.14159265358979323846 +#endif + +void svad_rfft_init(svad_rfft *f, int n_fft, int simd) { + const int n = n_fft / 2; + f->n = n; + f->simd = simd; + + int bits = 0; + while ((1 << bits) < n) + ++bits; + for (int i = 0; i < n; ++i) { + unsigned r = 0; + for (int b = 0; b < bits; ++b) + r |= ((unsigned)i >> b & 1u) << (bits - 1 - b); + f->rev[i] = (uint16_t)r; + } + + /* Stage twiddles, e^(-i*pi*j/half); the stage with half-size `half` + * occupies 2*(half-1) onwards, since 1 + 2 + ... + n/2 == n - 1. */ + for (int half = 1; half < n; half <<= 1) { + float *w = f->tw + 2 * (half - 1); + for (int j = 0; j < half; ++j) { + const double a = -M_PI * (double)j / (double)half; + w[2 * j + 0] = (float)cos(a); + w[2 * j + 1] = (float)sin(a); + } + } + + /* Untangle twiddles, e^(-i*pi*k/n) for k in 0..n. */ + for (int k = 0; k <= n; ++k) { + const double a = -M_PI * (double)k / (double)n; + f->post[2 * k + 0] = (float)cos(a); + f->post[2 * k + 1] = (float)sin(a); + } + + /* Twiddles are computed in double and rounded once, so they carry no more + * error than the stored basis they replace. */ +} + +/* ---- portable C ------------------------------------------------------ */ + +static void rfft_mag_scalar(const svad_rfft *f, + const float *x, + int hop, + int frames, + const float *window, + float *out, + int out_stride, + float *z) { + const int n = f->n; + const int mask = n - 1; + + for (int t = 0; t < frames; ++t) { + const float *src = x + (size_t)t * hop; + for (int k = 0; k < n; ++k) { + const int r = f->rev[k]; + z[2 * r + 0] = src[2 * k + 0] * window[2 * k + 0]; + z[2 * r + 1] = src[2 * k + 1] * window[2 * k + 1]; + } + + for (int half = 1; half < n; half <<= 1) { + const float *w = f->tw + 2 * (half - 1); + const int len = half << 1; + for (int i = 0; i < n; i += len) { + for (int j = 0; j < half; ++j) { + const float wr = w[2 * j + 0], wi = w[2 * j + 1]; + float *a = z + 2 * (i + j); + float *b = z + 2 * (i + j + half); + const float tr = b[0] * wr - b[1] * wi; + const float ti = b[0] * wi + b[1] * wr; + b[0] = a[0] - tr; + b[1] = a[1] - ti; + a[0] = a[0] + tr; + a[1] = a[1] + ti; + } + } + } + + float *dst = out + (size_t)t * out_stride; + for (int k = 0; k <= n; ++k) { + const int ka = k & mask, kb = (n - k) & mask; + const float ar = z[2 * ka + 0], ai = z[2 * ka + 1]; + const float br = z[2 * kb + 0], bi = z[2 * kb + 1]; + /* even part (Z[k] + conj(Z[n-k]))/2, odd part (Z[k] - conj(Z[n-k]))/2i */ + const float er = 0.5f * (ar + br), ei = 0.5f * (ai - bi); + const float orr = 0.5f * (ai + bi), oi = -0.5f * (ar - br); + const float wr = f->post[2 * k + 0], wi = f->post[2 * k + 1]; + const float xr = er + (wr * orr - wi * oi); + const float xi = ei + (wr * oi + wi * orr); + dst[k] = sqrtf(xr * xr + xi * xi); + } + } +} + +/* ---- AArch64 NEON, four frames in the vector lanes -------------------- */ + +#if defined(__aarch64__) && defined(__ARM_NEON) +# define SVAD_RFFT_HAVE_SIMD 1 +# include <arm_neon.h> + +/* Frames are independent transforms of identical shape, so they map onto the + * vector lanes directly: every butterfly becomes the scalar butterfly on four + * frames at once, with twiddles broadcast and no shuffles anywhere. Only the + * load and the final store cross lanes. */ +static void rfft_mag_neon4(const svad_rfft *f, + const float *x, + int hop, + const float *window, + float *out, + int out_stride, + float *scratch) { + const int n = f->n; + const int mask = n - 1; + float32x4_t *zr = (float32x4_t *)scratch; + float32x4_t *zi = zr + n; + + for (int k = 0; k < n; ++k) { + const int r = f->rev[k]; + const int e = 2 * k, o = 2 * k + 1; + const float32x4_t re = {x[e], x[hop + e], x[2 * hop + e], x[3 * hop + e]}; + const float32x4_t im = {x[o], x[hop + o], x[2 * hop + o], x[3 * hop + o]}; + zr[r] = vmulq_n_f32(re, window[e]); + zi[r] = vmulq_n_f32(im, window[o]); + } + + for (int half = 1; half < n; half <<= 1) { + const float *w = f->tw + 2 * (half - 1); + const int len = half << 1; + for (int i = 0; i < n; i += len) { + for (int j = 0; j < half; ++j) { + const float32x4_t wr = vdupq_n_f32(w[2 * j + 0]); + const float32x4_t wi = vdupq_n_f32(w[2 * j + 1]); + const int ia = i + j, ib = ia + half; + const float32x4_t br = zr[ib], bi = zi[ib]; + const float32x4_t tr = vfmsq_f32(vmulq_f32(br, wr), bi, wi); + const float32x4_t ti = vfmaq_f32(vmulq_f32(br, wi), bi, wr); + const float32x4_t ar = zr[ia], ai = zi[ia]; + zr[ib] = vsubq_f32(ar, tr); + zi[ib] = vsubq_f32(ai, ti); + zr[ia] = vaddq_f32(ar, tr); + zi[ia] = vaddq_f32(ai, ti); + } + } + } + + const float32x4_t half_v = vdupq_n_f32(0.5f); + for (int k = 0; k <= n; ++k) { + const int ka = k & mask, kb = (n - k) & mask; + const float32x4_t ar = zr[ka], ai = zi[ka]; + const float32x4_t br = zr[kb], bi = zi[kb]; + const float32x4_t er = vmulq_f32(vaddq_f32(ar, br), half_v); + const float32x4_t ei = vmulq_f32(vsubq_f32(ai, bi), half_v); + const float32x4_t orr = vmulq_f32(vaddq_f32(ai, bi), half_v); + const float32x4_t oi = vmulq_f32(vsubq_f32(br, ar), half_v); + const float32x4_t wr = vdupq_n_f32(f->post[2 * k + 0]); + const float32x4_t wi = vdupq_n_f32(f->post[2 * k + 1]); + const float32x4_t xr = vaddq_f32(er, vfmsq_f32(vmulq_f32(wr, orr), wi, oi)); + const float32x4_t xi = vaddq_f32(ei, vfmaq_f32(vmulq_f32(wr, oi), wi, orr)); + const float32x4_t mag = vsqrtq_f32(vfmaq_f32(vmulq_f32(xr, xr), xi, xi)); + vst1q_lane_f32(out + 0 * out_stride + k, mag, 0); + vst1q_lane_f32(out + 1 * out_stride + k, mag, 1); + vst1q_lane_f32(out + 2 * out_stride + k, mag, 2); + vst1q_lane_f32(out + 3 * out_stride + k, mag, 3); + } +} +#endif /* aarch64 NEON */ + +void svad_rfft_mag(const svad_rfft *f, + const float *x, + int hop, + int frames, + const float *window, + float *out, + int out_stride, + float *scratch) { +#if defined(SVAD_RFFT_HAVE_SIMD) + if (f->simd && frames == 4) { + rfft_mag_neon4(f, x, hop, window, out, out_stride, scratch); + return; + } +#endif + rfft_mag_scalar(f, x, hop, frames, window, out, out_stride, scratch); +} |
