Merges the FFT scaling with the MDCT pre-rotate
[opus.git] / celt / mdct.c
1 /* Copyright (c) 2007-2008 CSIRO
2    Copyright (c) 2007-2008 Xiph.Org Foundation
3    Written by Jean-Marc Valin */
4 /*
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15
16    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
20    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
21    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
22    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28
29 /* This is a simple MDCT implementation that uses a N/4 complex FFT
30    to do most of the work. It should be relatively straightforward to
31    plug in pretty much and FFT here.
32
33    This replaces the Vorbis FFT (and uses the exact same API), which
34    was a bit too messy and that was ending up duplicating code
35    (might as well use the same FFT everywhere).
36
37    The algorithm is similar to (and inspired from) Fabrice Bellard's
38    MDCT implementation in FFMPEG, but has differences in signs, ordering
39    and scaling in many places.
40 */
41
42 #ifndef SKIP_CONFIG_H
43 #ifdef HAVE_CONFIG_H
44 #include "config.h"
45 #endif
46 #endif
47
48 #include "mdct.h"
49 #include "kiss_fft.h"
50 #include "_kiss_fft_guts.h"
51 #include <math.h>
52 #include "os_support.h"
53 #include "mathops.h"
54 #include "stack_alloc.h"
55
56 #ifdef CUSTOM_MODES
57
58 int clt_mdct_init(mdct_lookup *l,int N, int maxshift)
59 {
60    int i;
61    int N4;
62    kiss_twiddle_scalar *trig;
63 #if defined(FIXED_POINT)
64    int N2=N>>1;
65 #endif
66    l->n = N;
67    N4 = N>>2;
68    l->maxshift = maxshift;
69    for (i=0;i<=maxshift;i++)
70    {
71       if (i==0)
72          l->kfft[i] = opus_fft_alloc(N>>2>>i, 0, 0);
73       else
74          l->kfft[i] = opus_fft_alloc_twiddles(N>>2>>i, 0, 0, l->kfft[0]);
75 #ifndef ENABLE_TI_DSPLIB55
76       if (l->kfft[i]==NULL)
77          return 0;
78 #endif
79    }
80    l->trig = trig = (kiss_twiddle_scalar*)opus_alloc((N4+1)*sizeof(kiss_twiddle_scalar));
81    if (l->trig==NULL)
82      return 0;
83    /* We have enough points that sine isn't necessary */
84 #if defined(FIXED_POINT)
85    for (i=0;i<=N4;i++)
86       trig[i] = TRIG_UPSCALE*celt_cos_norm(DIV32(ADD32(SHL32(EXTEND32(i),17),N2),N));
87 #else
88    for (i=0;i<=N4;i++)
89       trig[i] = (kiss_twiddle_scalar)cos(2*PI*i/N);
90 #endif
91    return 1;
92 }
93
94 void clt_mdct_clear(mdct_lookup *l)
95 {
96    int i;
97    for (i=0;i<=l->maxshift;i++)
98       opus_fft_free(l->kfft[i]);
99    opus_free((kiss_twiddle_scalar*)l->trig);
100 }
101
102 #endif /* CUSTOM_MODES */
103
104 /* Forward MDCT trashes the input array */
105 void clt_mdct_forward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * OPUS_RESTRICT out,
106       const opus_val16 *window, int overlap, int shift, int stride)
107 {
108    int i;
109    int N, N2, N4;
110    kiss_twiddle_scalar sine;
111    VARDECL(kiss_fft_scalar, f);
112    VARDECL(kiss_fft_cpx, f2);
113    const kiss_fft_state *st = l->kfft[shift];
114 #ifdef FIXED_POINT
115    /* FIXME: This should eventually just go in the state. */
116    opus_val16 scale;
117    int scale_shift;
118    scale_shift = celt_ilog2(st->nfft);
119    if (st->nfft == 1<<scale_shift)
120       scale = Q15ONE;
121    else
122       scale = (1073741824+st->nfft/2)/st->nfft>>(15-scale_shift);
123 #endif
124    SAVE_STACK;
125    N = l->n;
126    N >>= shift;
127    N2 = N>>1;
128    N4 = N>>2;
129    ALLOC(f, N2, kiss_fft_scalar);
130    ALLOC(f2, N2, kiss_fft_cpx);
131    /* sin(x) ~= x here */
132 #ifdef FIXED_POINT
133    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
134 #else
135    sine = (kiss_twiddle_scalar)2*PI*(.125f)/N;
136 #endif
137
138    /* Consider the input to be composed of four blocks: [a, b, c, d] */
139    /* Window, shuffle, fold */
140    {
141       /* Temp pointers to make it really clear to the compiler what we're doing */
142       const kiss_fft_scalar * OPUS_RESTRICT xp1 = in+(overlap>>1);
143       const kiss_fft_scalar * OPUS_RESTRICT xp2 = in+N2-1+(overlap>>1);
144       kiss_fft_scalar * OPUS_RESTRICT yp = f;
145       const opus_val16 * OPUS_RESTRICT wp1 = window+(overlap>>1);
146       const opus_val16 * OPUS_RESTRICT wp2 = window+(overlap>>1)-1;
147       for(i=0;i<((overlap+3)>>2);i++)
148       {
149          /* Real part arranged as -d-cR, Imag part arranged as -b+aR*/
150          *yp++ = MULT16_32_Q15(*wp2, xp1[N2]) + MULT16_32_Q15(*wp1,*xp2);
151          *yp++ = MULT16_32_Q15(*wp1, *xp1)    - MULT16_32_Q15(*wp2, xp2[-N2]);
152          xp1+=2;
153          xp2-=2;
154          wp1+=2;
155          wp2-=2;
156       }
157       wp1 = window;
158       wp2 = window+overlap-1;
159       for(;i<N4-((overlap+3)>>2);i++)
160       {
161          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
162          *yp++ = *xp2;
163          *yp++ = *xp1;
164          xp1+=2;
165          xp2-=2;
166       }
167       for(;i<N4;i++)
168       {
169          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
170          *yp++ =  -MULT16_32_Q15(*wp1, xp1[-N2]) + MULT16_32_Q15(*wp2, *xp2);
171          *yp++ = MULT16_32_Q15(*wp2, *xp1)     + MULT16_32_Q15(*wp1, xp2[N2]);
172          xp1+=2;
173          xp2-=2;
174          wp1+=2;
175          wp2-=2;
176       }
177    }
178    /* Pre-rotation */
179    {
180       kiss_fft_scalar * OPUS_RESTRICT yp = f;
181       const kiss_twiddle_scalar *t = &l->trig[0];
182       for(i=0;i<N4;i++)
183       {
184          kiss_fft_cpx yc;
185          kiss_twiddle_scalar t0, t1;
186          kiss_fft_scalar re, im, yr, yi;
187          t0 = t[i<<shift];
188          t1 = t[(N4-i)<<shift];
189 #ifdef FIXED_POINT
190          t0 = MULT16_16_P15(t0, scale);
191          t1 = MULT16_16_P15(t1, scale);
192 #else
193          t0 *= st->scale;
194          t1 *= st->scale;
195 #endif
196          re = *yp++;
197          im = *yp++;
198          yr = -S_MUL(re,t0)  -  S_MUL(im,t1);
199          yi = -S_MUL(im,t0)  +  S_MUL(re,t1);
200          /* works because the cos is nearly one */
201          yc.r = yr + S_MUL(yi,sine);
202          yc.i = yi - S_MUL(yr,sine);
203 #ifdef FIXED_POINT
204          yc.r = SHR32(yc.r, scale_shift);
205          yc.i = SHR32(yc.i, scale_shift);
206 #endif
207          f2[st->bitrev[i]] = yc;
208       }
209    }
210
211    /* N/4 complex FFT, down-scales by 4/N */
212    opus_fft_impl(st, f2);
213
214    /* Post-rotate */
215    {
216       /* Temp pointers to make it really clear to the compiler what we're doing */
217       const kiss_fft_cpx * OPUS_RESTRICT fp = f2;
218       kiss_fft_scalar * OPUS_RESTRICT yp1 = out;
219       kiss_fft_scalar * OPUS_RESTRICT yp2 = out+stride*(N2-1);
220       const kiss_twiddle_scalar *t = &l->trig[0];
221       /* Temp pointers to make it really clear to the compiler what we're doing */
222       for(i=0;i<N4;i++)
223       {
224          kiss_fft_scalar yr, yi;
225          yr = S_MUL(fp->i,t[(N4-i)<<shift]) + S_MUL(fp->r,t[i<<shift]);
226          yi = S_MUL(fp->r,t[(N4-i)<<shift]) - S_MUL(fp->i,t[i<<shift]);
227          /* works because the cos is nearly one */
228          *yp1 = yr - S_MUL(yi,sine);
229          *yp2 = yi + S_MUL(yr,sine);;
230          fp++;
231          yp1 += 2*stride;
232          yp2 -= 2*stride;
233       }
234    }
235    RESTORE_STACK;
236 }
237
238 void clt_mdct_backward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * OPUS_RESTRICT out,
239       const opus_val16 * OPUS_RESTRICT window, int overlap, int shift, int stride)
240 {
241    int i;
242    int N, N2, N4;
243    kiss_twiddle_scalar sine;
244    VARDECL(kiss_fft_cpx, f2);
245    SAVE_STACK;
246    N = l->n;
247    N >>= shift;
248    N2 = N>>1;
249    N4 = N>>2;
250    ALLOC(f2, N4, kiss_fft_cpx);
251    /* sin(x) ~= x here */
252 #ifdef FIXED_POINT
253    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
254 #else
255    sine = (kiss_twiddle_scalar)2*PI*(.125f)/N;
256 #endif
257
258    /* Pre-rotate */
259    {
260       /* Temp pointers to make it really clear to the compiler what we're doing */
261       const kiss_fft_scalar * OPUS_RESTRICT xp1 = in;
262       const kiss_fft_scalar * OPUS_RESTRICT xp2 = in+stride*(N2-1);
263       kiss_fft_cpx * OPUS_RESTRICT yp = f2;
264       const kiss_twiddle_scalar * OPUS_RESTRICT t = &l->trig[0];
265       const opus_int16 * OPUS_RESTRICT bitrev = l->kfft[shift]->bitrev;
266       for(i=0;i<N4;i++)
267       {
268          kiss_fft_scalar yr, yi;
269          kiss_fft_cpx yc;
270          yr = -S_MUL(*xp2, t[i<<shift]) + S_MUL(*xp1,t[(N4-i)<<shift]);
271          yi =  -S_MUL(*xp2, t[(N4-i)<<shift]) - S_MUL(*xp1,t[i<<shift]);
272          /* Works because the cos is nearly one. We swap real and imag because we
273             use an FFT instead of an IFFT. */
274          yc.i = yr - S_MUL(yi,sine);
275          yc.r = yi + S_MUL(yr,sine);
276          /* Storing the pre-rotation directly in the bitrev order. */
277          yp[*bitrev++] = yc;
278          xp1+=2*stride;
279          xp2-=2*stride;
280       }
281    }
282
283    opus_fft_impl(l->kfft[shift], f2);
284
285    /* Post-rotate and de-shuffle from both ends of the buffer at once to make
286       it in-place. */
287    {
288       kiss_fft_scalar * OPUS_RESTRICT yp0 = out+(overlap>>1);
289       kiss_fft_scalar * OPUS_RESTRICT yp1 = out+(overlap>>1)+N2-2;
290       const kiss_twiddle_scalar *t = &l->trig[0];
291       /* Loop to (N4+1)>>1 to handle odd N4. When N4 is odd, the
292          middle pair will be computed twice. */
293       for(i=0;i<(N4+1)>>1;i++)
294       {
295          kiss_fft_scalar re, im, yr, yi;
296          kiss_twiddle_scalar t0, t1;
297          /* We swap real and imag because we're using an FFT instead of an IFFT. */
298          re = f2[i].i;
299          im = f2[i].r;
300          t0 = t[i<<shift];
301          t1 = t[(N4-i)<<shift];
302          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
303          yr = S_MUL(re,t0) - S_MUL(im,t1);
304          yi = S_MUL(im,t0) + S_MUL(re,t1);
305          /* We swap real and imag because we're using an FFT instead of an IFFT. */
306          re = f2[N4-i-1].i;
307          im = f2[N4-i-1].r;
308          /* works because the cos is nearly one */
309          yp0[0] = -(yr - S_MUL(yi,sine));
310          yp1[1] = yi + S_MUL(yr,sine);
311
312          t0 = t[(N4-i-1)<<shift];
313          t1 = t[(i+1)<<shift];
314          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
315          yr = S_MUL(re,t0) - S_MUL(im,t1);
316          yi = S_MUL(im,t0) + S_MUL(re,t1);
317          /* works because the cos is nearly one */
318          yp1[0] = -(yr - S_MUL(yi,sine));
319          yp0[1] = yi + S_MUL(yr,sine);
320          yp0 += 2;
321          yp1 -= 2;
322       }
323    }
324
325    /* Mirror on both sides for TDAC */
326    {
327       kiss_fft_scalar * OPUS_RESTRICT xp1 = out+overlap-1;
328       kiss_fft_scalar * OPUS_RESTRICT yp1 = out;
329       const opus_val16 * OPUS_RESTRICT wp1 = window;
330       const opus_val16 * OPUS_RESTRICT wp2 = window+overlap-1;
331
332       for(i = 0; i < overlap/2; i++)
333       {
334          kiss_fft_scalar x1, x2;
335          x1 = *xp1;
336          x2 = *yp1;
337          *yp1++ = MULT16_32_Q15(*wp2, x2) - MULT16_32_Q15(*wp1, x1);
338          *xp1-- = MULT16_32_Q15(*wp1, x2) + MULT16_32_Q15(*wp2, x1);
339          wp1++;
340          wp2--;
341       }
342    }
343    RESTORE_STACK;
344 }