1 /******************************************************************************
2 *
3 * Copyright 2022 Google LLC
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 #include "mdct.h"
20 #include "tables.h"
21
22 #include "mdct_neon.h"
23
24
25 /* ----------------------------------------------------------------------------
26 * FFT processing
27 * -------------------------------------------------------------------------- */
28
29 /**
30 * FFT 5 Points
31 * x, y Input and output coefficients, of size 5xn
32 * n Number of interleaved transform to perform (n % 2 = 0)
33 */
34 #ifndef fft_5
fft_5(const struct lc3_complex * x,struct lc3_complex * y,int n)35 LC3_HOT static inline void fft_5(
36 const struct lc3_complex *x, struct lc3_complex *y, int n)
37 {
38 static const float cos1 = 0.3090169944; /* cos(-2Pi 1/5) */
39 static const float cos2 = -0.8090169944; /* cos(-2Pi 2/5) */
40
41 static const float sin1 = -0.9510565163; /* sin(-2Pi 1/5) */
42 static const float sin2 = -0.5877852523; /* sin(-2Pi 2/5) */
43
44 for (int i = 0; i < n; i++, x++, y+= 5) {
45
46 struct lc3_complex s14 =
47 { x[1*n].re + x[4*n].re, x[1*n].im + x[4*n].im };
48 struct lc3_complex d14 =
49 { x[1*n].re - x[4*n].re, x[1*n].im - x[4*n].im };
50
51 struct lc3_complex s23 =
52 { x[2*n].re + x[3*n].re, x[2*n].im + x[3*n].im };
53 struct lc3_complex d23 =
54 { x[2*n].re - x[3*n].re, x[2*n].im - x[3*n].im };
55
56 y[0].re = x[0].re + s14.re + s23.re;
57
58 y[0].im = x[0].im + s14.im + s23.im;
59
60 y[1].re = x[0].re + s14.re * cos1 - d14.im * sin1
61 + s23.re * cos2 - d23.im * sin2;
62
63 y[1].im = x[0].im + s14.im * cos1 + d14.re * sin1
64 + s23.im * cos2 + d23.re * sin2;
65
66 y[2].re = x[0].re + s14.re * cos2 - d14.im * sin2
67 + s23.re * cos1 + d23.im * sin1;
68
69 y[2].im = x[0].im + s14.im * cos2 + d14.re * sin2
70 + s23.im * cos1 - d23.re * sin1;
71
72 y[3].re = x[0].re + s14.re * cos2 + d14.im * sin2
73 + s23.re * cos1 - d23.im * sin1;
74
75 y[3].im = x[0].im + s14.im * cos2 - d14.re * sin2
76 + s23.im * cos1 + d23.re * sin1;
77
78 y[4].re = x[0].re + s14.re * cos1 + d14.im * sin1
79 + s23.re * cos2 + d23.im * sin2;
80
81 y[4].im = x[0].im + s14.im * cos1 - d14.re * sin1
82 + s23.im * cos2 - d23.re * sin2;
83 }
84 }
85 #endif /* fft_5 */
86
87 /**
88 * FFT Butterfly 3 Points
89 * x, y Input and output coefficients
90 * twiddles Twiddles factors, determine size of transform
91 * n Number of interleaved transforms
92 */
93 #ifndef fft_bf3
fft_bf3(const struct lc3_fft_bf3_twiddles * twiddles,const struct lc3_complex * x,struct lc3_complex * y,int n)94 LC3_HOT static inline void fft_bf3(
95 const struct lc3_fft_bf3_twiddles *twiddles,
96 const struct lc3_complex *x, struct lc3_complex *y, int n)
97 {
98 int n3 = twiddles->n3;
99 const struct lc3_complex (*w0)[2] = twiddles->t;
100 const struct lc3_complex (*w1)[2] = w0 + n3, (*w2)[2] = w1 + n3;
101
102 const struct lc3_complex *x0 = x, *x1 = x0 + n*n3, *x2 = x1 + n*n3;
103 struct lc3_complex *y0 = y, *y1 = y0 + n3, *y2 = y1 + n3;
104
105 for (int i = 0; i < n; i++, y0 += 3*n3, y1 += 3*n3, y2 += 3*n3)
106 for (int j = 0; j < n3; j++, x0++, x1++, x2++) {
107
108 y0[j].re = x0->re + x1->re * w0[j][0].re - x1->im * w0[j][0].im
109 + x2->re * w0[j][1].re - x2->im * w0[j][1].im;
110
111 y0[j].im = x0->im + x1->im * w0[j][0].re + x1->re * w0[j][0].im
112 + x2->im * w0[j][1].re + x2->re * w0[j][1].im;
113
114 y1[j].re = x0->re + x1->re * w1[j][0].re - x1->im * w1[j][0].im
115 + x2->re * w1[j][1].re - x2->im * w1[j][1].im;
116
117 y1[j].im = x0->im + x1->im * w1[j][0].re + x1->re * w1[j][0].im
118 + x2->im * w1[j][1].re + x2->re * w1[j][1].im;
119
120 y2[j].re = x0->re + x1->re * w2[j][0].re - x1->im * w2[j][0].im
121 + x2->re * w2[j][1].re - x2->im * w2[j][1].im;
122
123 y2[j].im = x0->im + x1->im * w2[j][0].re + x1->re * w2[j][0].im
124 + x2->im * w2[j][1].re + x2->re * w2[j][1].im;
125 }
126 }
127 #endif /* fft_bf3 */
128
129 /**
130 * FFT Butterfly 2 Points
131 * twiddles Twiddles factors, determine size of transform
132 * x, y Input and output coefficients
133 * n Number of interleaved transforms
134 */
135 #ifndef fft_bf2
fft_bf2(const struct lc3_fft_bf2_twiddles * twiddles,const struct lc3_complex * x,struct lc3_complex * y,int n)136 LC3_HOT static inline void fft_bf2(
137 const struct lc3_fft_bf2_twiddles *twiddles,
138 const struct lc3_complex *x, struct lc3_complex *y, int n)
139 {
140 int n2 = twiddles->n2;
141 const struct lc3_complex *w = twiddles->t;
142
143 const struct lc3_complex *x0 = x, *x1 = x0 + n*n2;
144 struct lc3_complex *y0 = y, *y1 = y0 + n2;
145
146 for (int i = 0; i < n; i++, y0 += 2*n2, y1 += 2*n2) {
147
148 for (int j = 0; j < n2; j++, x0++, x1++) {
149
150 y0[j].re = x0->re + x1->re * w[j].re - x1->im * w[j].im;
151 y0[j].im = x0->im + x1->im * w[j].re + x1->re * w[j].im;
152
153 y1[j].re = x0->re - x1->re * w[j].re + x1->im * w[j].im;
154 y1[j].im = x0->im - x1->im * w[j].re - x1->re * w[j].im;
155 }
156 }
157 }
158 #endif /* fft_bf2 */
159
160 /**
161 * Perform FFT
162 * x, y0, y1 Input, and 2 scratch buffers of size `n`
163 * n Number of points 30, 40, 60, 80, 90, 120, 160, 180, 240
164 * return The buffer `y0` or `y1` that hold the result
165 *
166 * Input `x` can be the same as the `y0` second scratch buffer
167 */
fft(const struct lc3_complex * x,int n,struct lc3_complex * y0,struct lc3_complex * y1)168 static struct lc3_complex *fft(const struct lc3_complex *x, int n,
169 struct lc3_complex *y0, struct lc3_complex *y1)
170 {
171 struct lc3_complex *y[2] = { y1, y0 };
172 int i2, i3, is = 0;
173
174 /* The number of points `n` can be decomposed as :
175 *
176 * n = 5^1 * 3^n3 * 2^n2
177 *
178 * for n = 40, 80, 160 n3 = 0, n2 = [3..5]
179 * n = 30, 60, 120, 240 n3 = 1, n2 = [1..4]
180 * n = 90, 180 n3 = 2, n2 = [1..2]
181 *
182 * Note that the expression `n & (n-1) == 0` is equivalent
183 * to the check that `n` is a power of 2. */
184
185 fft_5(x, y[is], n /= 5);
186
187 for (i3 = 0; n & (n-1); i3++, is ^= 1)
188 fft_bf3(lc3_fft_twiddles_bf3[i3], y[is], y[is ^ 1], n /= 3);
189
190 for (i2 = 0; n > 1; i2++, is ^= 1)
191 fft_bf2(lc3_fft_twiddles_bf2[i2][i3], y[is], y[is ^ 1], n >>= 1);
192
193 return y[is];
194 }
195
196
197 /* ----------------------------------------------------------------------------
198 * MDCT processing
199 * -------------------------------------------------------------------------- */
200
201 /**
202 * Windowing of samples before MDCT
203 * dt, sr Duration and samplerate (size of the transform)
204 * x, y Input current and delayed samples
205 * y, d Output windowed samples, and delayed ones
206 */
mdct_window(enum lc3_dt dt,enum lc3_srate sr,const float * x,float * d,float * y)207 LC3_HOT static void mdct_window(enum lc3_dt dt, enum lc3_srate sr,
208 const float *x, float *d, float *y)
209 {
210 int ns = LC3_NS(dt, sr), nd = LC3_ND(dt, sr);
211
212 const float *w0 = lc3_mdct_win[dt][sr], *w1 = w0 + ns;
213 const float *w2 = w1, *w3 = w2 + nd;
214
215 const float *x0 = x + ns-nd, *x1 = x0;
216 float *y0 = y + ns/2, *y1 = y0;
217 float *d0 = d, *d1 = d + nd;
218
219 while (x1 > x) {
220 *(--y0) = *d0 * *(w0++) - *(--x1) * *(--w1);
221 *(y1++) = (*(d0++) = *(x0++)) * *(w2++);
222
223 *(--y0) = *d0 * *(w0++) - *(--x1) * *(--w1);
224 *(y1++) = (*(d0++) = *(x0++)) * *(w2++);
225 }
226
227 for (x1 += ns; x0 < x1; ) {
228 *(--y0) = *d0 * *(w0++) - *(--d1) * *(--w1);
229 *(y1++) = (*(d0++) = *(x0++)) * *(w2++) + (*d1 = *(--x1)) * *(--w3);
230
231 *(--y0) = *d0 * *(w0++) - *(--d1) * *(--w1);
232 *(y1++) = (*(d0++) = *(x0++)) * *(w2++) + (*d1 = *(--x1)) * *(--w3);
233 }
234 }
235
236 /**
237 * Pre-rotate MDCT coefficients of N/2 points, before FFT N/4 points FFT
238 * def Size and twiddles factors
239 * x, y Input and output coefficients
240 *
241 * `x` and y` can be the same buffer
242 */
mdct_pre_fft(const struct lc3_mdct_rot_def * def,const float * x,struct lc3_complex * y)243 LC3_HOT static void mdct_pre_fft(const struct lc3_mdct_rot_def *def,
244 const float *x, struct lc3_complex *y)
245 {
246 int n4 = def->n4;
247
248 const float *x0 = x, *x1 = x0 + 2*n4;
249 const struct lc3_complex *w0 = def->w, *w1 = w0 + n4;
250 struct lc3_complex *y0 = y, *y1 = y0 + n4;
251
252 while (x0 < x1) {
253 struct lc3_complex u, uw = *(w0++);
254 u.re = - *(--x1) * uw.re + *x0 * uw.im;
255 u.im = *(x0++) * uw.re + *x1 * uw.im;
256
257 struct lc3_complex v, vw = *(--w1);
258 v.re = - *(--x1) * vw.im + *x0 * vw.re;
259 v.im = - *(x0++) * vw.im - *x1 * vw.re;
260
261 *(y0++) = u;
262 *(--y1) = v;
263 }
264 }
265
266 /**
267 * Post-rotate FFT N/4 points coefficients, resulting MDCT N points
268 * def Size and twiddles factors
269 * x, y Input and output coefficients
270 * scale Scale on output coefficients
271 *
272 * `x` and y` can be the same buffer
273 */
mdct_post_fft(const struct lc3_mdct_rot_def * def,const struct lc3_complex * x,float * y,float scale)274 LC3_HOT static void mdct_post_fft(const struct lc3_mdct_rot_def *def,
275 const struct lc3_complex *x, float *y, float scale)
276 {
277 int n4 = def->n4, n8 = n4 >> 1;
278
279 const struct lc3_complex *w0 = def->w + n8, *w1 = w0 - 1;
280 const struct lc3_complex *x0 = x + n8, *x1 = x0 - 1;
281
282 float *y0 = y + n4, *y1 = y0;
283
284 for ( ; y1 > y; x0++, x1--, w0++, w1--) {
285
286 float u0 = (x0->im * w0->im + x0->re * w0->re) * scale;
287 float u1 = (x1->re * w1->im - x1->im * w1->re) * scale;
288
289 float v0 = (x0->re * w0->im - x0->im * w0->re) * scale;
290 float v1 = (x1->im * w1->im + x1->re * w1->re) * scale;
291
292 *(y0++) = u0; *(y0++) = u1;
293 *(--y1) = v0; *(--y1) = v1;
294 }
295 }
296
297 /**
298 * Pre-rotate IMDCT coefficients of N points, before FFT N/4 points FFT
299 * def Size and twiddles factors
300 * x, y Input and output coefficients
301 *
302 * `x` and `y` can be the same buffer
303 * The real and imaginary parts of `y` are swapped,
304 * to operate on FFT instead of IFFT
305 */
imdct_pre_fft(const struct lc3_mdct_rot_def * def,const float * x,struct lc3_complex * y)306 LC3_HOT static void imdct_pre_fft(const struct lc3_mdct_rot_def *def,
307 const float *x, struct lc3_complex *y)
308 {
309 int n4 = def->n4;
310
311 const float *x0 = x, *x1 = x0 + 2*n4;
312
313 const struct lc3_complex *w0 = def->w, *w1 = w0 + n4;
314 struct lc3_complex *y0 = y, *y1 = y0 + n4;
315
316 while (x0 < x1) {
317 float u0 = *(x0++), u1 = *(--x1);
318 float v0 = *(x0++), v1 = *(--x1);
319 struct lc3_complex uw = *(w0++), vw = *(--w1);
320
321 (y0 )->re = - u0 * uw.re - u1 * uw.im;
322 (y0++)->im = - u1 * uw.re + u0 * uw.im;
323
324 (--y1)->re = - v1 * vw.re - v0 * vw.im;
325 ( y1)->im = - v0 * vw.re + v1 * vw.im;
326 }
327 }
328
329 /**
330 * Post-rotate FFT N/4 points coefficients, resulting IMDCT N points
331 * def Size and twiddles factors
332 * x, y Input and output coefficients
333 * scale Scale on output coefficients
334 *
335 * `x` and y` can be the same buffer
336 * The real and imaginary parts of `x` are swapped,
337 * to operate on FFT instead of IFFT
338 */
imdct_post_fft(const struct lc3_mdct_rot_def * def,const struct lc3_complex * x,float * y,float scale)339 LC3_HOT static void imdct_post_fft(const struct lc3_mdct_rot_def *def,
340 const struct lc3_complex *x, float *y, float scale)
341 {
342 int n4 = def->n4;
343
344 const struct lc3_complex *w0 = def->w, *w1 = w0 + n4;
345 const struct lc3_complex *x0 = x, *x1 = x0 + n4;
346
347 float *y0 = y, *y1 = y0 + 2*n4;
348
349 while (x0 < x1) {
350 struct lc3_complex uz = *(x0++), vz = *(--x1);
351 struct lc3_complex uw = *(w0++), vw = *(--w1);
352
353 *(y0++) = (uz.re * uw.im - uz.im * uw.re) * scale;
354 *(--y1) = (uz.re * uw.re + uz.im * uw.im) * scale;
355
356 *(--y1) = (vz.re * vw.im - vz.im * vw.re) * scale;
357 *(y0++) = (vz.re * vw.re + vz.im * vw.im) * scale;
358 }
359 }
360
361 /**
362 * Apply windowing of samples
363 * dt, sr Duration and samplerate
364 * x, d Middle half of IMDCT coefficients and delayed samples
365 * y, d Output samples and delayed ones
366 */
imdct_window(enum lc3_dt dt,enum lc3_srate sr,const float * x,float * d,float * y)367 LC3_HOT static void imdct_window(enum lc3_dt dt, enum lc3_srate sr,
368 const float *x, float *d, float *y)
369 {
370 /* The full MDCT coefficients is given by symmetry :
371 * T[ 0 .. n/4-1] = -half[n/4-1 .. 0 ]
372 * T[ n/4 .. n/2-1] = half[0 .. n/4-1]
373 * T[ n/2 .. 3n/4-1] = half[n/4 .. n/2-1]
374 * T[3n/4 .. n-1] = half[n/2-1 .. n/4 ] */
375
376 int n4 = LC3_NS(dt, sr) >> 1, nd = LC3_ND(dt, sr);
377 const float *w2 = lc3_mdct_win[dt][sr], *w0 = w2 + 3*n4, *w1 = w0;
378
379 const float *x0 = d + nd-n4, *x1 = x0;
380 float *y0 = y + nd-n4, *y1 = y0, *y2 = d + nd, *y3 = d;
381
382 while (y0 > y) {
383 *(--y0) = *(--x0) - *(x ) * *(w1++);
384 *(y1++) = *(x1++) + *(x++) * *(--w0);
385
386 *(--y0) = *(--x0) - *(x ) * *(w1++);
387 *(y1++) = *(x1++) + *(x++) * *(--w0);
388 }
389
390 while (y1 < y + nd) {
391 *(y1++) = *(x1++) + *(x++) * *(--w0);
392 *(y1++) = *(x1++) + *(x++) * *(--w0);
393 }
394
395 while (y1 < y + 2*n4) {
396 *(y1++) = *(x ) * *(--w0);
397 *(--y2) = *(x++) * *(w2++);
398
399 *(y1++) = *(x ) * *(--w0);
400 *(--y2) = *(x++) * *(w2++);
401 }
402
403 while (y2 > y3) {
404 *(y3++) = *(x ) * *(--w0);
405 *(--y2) = *(x++) * *(w2++);
406
407 *(y3++) = *(x ) * *(--w0);
408 *(--y2) = *(x++) * *(w2++);
409 }
410 }
411
412 /**
413 * Forward MDCT transformation
414 */
lc3_mdct_forward(enum lc3_dt dt,enum lc3_srate sr,enum lc3_srate sr_dst,const float * x,float * d,float * y)415 void lc3_mdct_forward(enum lc3_dt dt, enum lc3_srate sr,
416 enum lc3_srate sr_dst, const float *x, float *d, float *y)
417 {
418 const struct lc3_mdct_rot_def *rot = lc3_mdct_rot[dt][sr];
419 int nf = LC3_NS(dt, sr_dst);
420 int ns = LC3_NS(dt, sr);
421
422 struct lc3_complex buffer[ns/2];
423 struct lc3_complex *z = (struct lc3_complex *)y;
424 union { float *f; struct lc3_complex *z; } u = { .z = buffer };
425
426 mdct_window(dt, sr, x, d, u.f);
427
428 mdct_pre_fft(rot, u.f, u.z);
429 u.z = fft(u.z, ns/2, u.z, z);
430 mdct_post_fft(rot, u.z, y, sqrtf( (2.f*nf) / (ns*ns) ));
431 }
432
433 /**
434 * Inverse MDCT transformation
435 */
lc3_mdct_inverse(enum lc3_dt dt,enum lc3_srate sr,enum lc3_srate sr_src,const float * x,float * d,float * y)436 void lc3_mdct_inverse(enum lc3_dt dt, enum lc3_srate sr,
437 enum lc3_srate sr_src, const float *x, float *d, float *y)
438 {
439 const struct lc3_mdct_rot_def *rot = lc3_mdct_rot[dt][sr];
440 int nf = LC3_NS(dt, sr_src);
441 int ns = LC3_NS(dt, sr);
442
443 struct lc3_complex buffer[ns/2];
444 struct lc3_complex *z = (struct lc3_complex *)y;
445 union { float *f; struct lc3_complex *z; } u = { .z = buffer };
446
447 imdct_pre_fft(rot, x, z);
448 z = fft(z, ns/2, z, u.z);
449 imdct_post_fft(rot, z, u.f, sqrtf(2.f / nf));
450
451 imdct_window(dt, sr, u.f, d, y);
452 }
453
