#include "rfft.h" #include #include #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 /* 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); }