aboutsummaryrefslogtreecommitdiff
path: root/rfft.c
diff options
context:
space:
mode:
Diffstat (limited to 'rfft.c')
-rw-r--r--rfft.c190
1 files changed, 190 insertions, 0 deletions
diff --git a/rfft.c b/rfft.c
new file mode 100644
index 0000000..0d20634
--- /dev/null
+++ b/rfft.c
@@ -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);
+}