aboutsummaryrefslogtreecommitdiff
path: root/rfft.c
blob: 0d206345e876d62d3f049bcb7d9076dfb91f6040 (plain) (blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
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);
}