Making the IMDCT work on interleaved data
[opus.git] / libcelt / 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 FOUNDATION OR
20    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, N2;
62    kiss_twiddle_scalar *trig;
63    l->n = N;
64    N2 = N>>1;
65    N4 = N>>2;
66    l->maxshift = maxshift;
67    for (i=0;i<=maxshift;i++)
68    {
69       if (i==0)
70          l->kfft[i] = opus_fft_alloc(N>>2>>i, 0, 0);
71       else
72          l->kfft[i] = opus_fft_alloc_twiddles(N>>2>>i, 0, 0, l->kfft[0]);
73 #ifndef ENABLE_TI_DSPLIB55
74       if (l->kfft[i]==NULL)
75          return 0;
76 #endif
77    }
78    l->trig = trig = (kiss_twiddle_scalar*)celt_alloc((N4+1)*sizeof(kiss_twiddle_scalar));
79    if (l->trig==NULL)
80      return 0;
81    /* We have enough points that sine isn't necessary */
82 #if defined(FIXED_POINT)
83    for (i=0;i<=N4;i++)
84       trig[i] = TRIG_UPSCALE*celt_cos_norm(DIV32(ADD32(SHL32(EXTEND32(i),17),N2),N));
85 #else
86    for (i=0;i<=N4;i++)
87       trig[i] = (kiss_twiddle_scalar)cos(2*PI*i/N);
88 #endif
89    return 1;
90 }
91
92 void clt_mdct_clear(mdct_lookup *l)
93 {
94    int i;
95    for (i=0;i<=l->maxshift;i++)
96       opus_fft_free(l->kfft[i]);
97    celt_free((kiss_twiddle_scalar*)l->trig);
98 }
99
100 #endif /* CUSTOM_MODES */
101
102 void clt_mdct_forward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out, const opus_val16 *window, int overlap, int shift)
103 {
104    int i;
105    int N, N2, N4;
106    kiss_twiddle_scalar sine;
107    VARDECL(kiss_fft_scalar, f);
108    SAVE_STACK;
109    N = l->n;
110    N >>= shift;
111    N2 = N>>1;
112    N4 = N>>2;
113    ALLOC(f, N2, kiss_fft_scalar);
114    /* sin(x) ~= x here */
115 #ifdef FIXED_POINT
116    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
117 #else
118    sine = (kiss_twiddle_scalar)2*PI*(.125f)/N;
119 #endif
120
121    /* Consider the input to be composed of four blocks: [a, b, c, d] */
122    /* Window, shuffle, fold */
123    {
124       /* Temp pointers to make it really clear to the compiler what we're doing */
125       const kiss_fft_scalar * restrict xp1 = in+(overlap>>1);
126       const kiss_fft_scalar * restrict xp2 = in+N2-1+(overlap>>1);
127       kiss_fft_scalar * restrict yp = out;
128       const opus_val16 * restrict wp1 = window+(overlap>>1);
129       const opus_val16 * restrict wp2 = window+(overlap>>1)-1;
130       for(i=0;i<(overlap>>2);i++)
131       {
132          /* Real part arranged as -d-cR, Imag part arranged as -b+aR*/
133          *yp++ = MULT16_32_Q15(*wp2, xp1[N2]) + MULT16_32_Q15(*wp1,*xp2);
134          *yp++ = MULT16_32_Q15(*wp1, *xp1)    - MULT16_32_Q15(*wp2, xp2[-N2]);
135          xp1+=2;
136          xp2-=2;
137          wp1+=2;
138          wp2-=2;
139       }
140       wp1 = window;
141       wp2 = window+overlap-1;
142       for(;i<N4-(overlap>>2);i++)
143       {
144          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
145          *yp++ = *xp2;
146          *yp++ = *xp1;
147          xp1+=2;
148          xp2-=2;
149       }
150       for(;i<N4;i++)
151       {
152          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
153          *yp++ =  -MULT16_32_Q15(*wp1, xp1[-N2]) + MULT16_32_Q15(*wp2, *xp2);
154          *yp++ = MULT16_32_Q15(*wp2, *xp1)     + MULT16_32_Q15(*wp1, xp2[N2]);
155          xp1+=2;
156          xp2-=2;
157          wp1+=2;
158          wp2-=2;
159       }
160    }
161    /* Pre-rotation */
162    {
163       kiss_fft_scalar * restrict yp = out;
164       const kiss_twiddle_scalar *t = &l->trig[0];
165       for(i=0;i<N4;i++)
166       {
167          kiss_fft_scalar re, im, yr, yi;
168          re = yp[0];
169          im = yp[1];
170          yr = -S_MUL(re,t[i<<shift])  -  S_MUL(im,t[(N4-i)<<shift]);
171          yi = -S_MUL(im,t[i<<shift])  +  S_MUL(re,t[(N4-i)<<shift]);
172          /* works because the cos is nearly one */
173          *yp++ = yr + S_MUL(yi,sine);
174          *yp++ = yi - S_MUL(yr,sine);
175       }
176    }
177
178    /* N/4 complex FFT, down-scales by 4/N */
179    opus_fft(l->kfft[shift], (kiss_fft_cpx *)out, (kiss_fft_cpx *)f);
180
181    /* Post-rotate */
182    {
183       /* Temp pointers to make it really clear to the compiler what we're doing */
184       const kiss_fft_scalar * restrict fp = f;
185       kiss_fft_scalar * restrict yp1 = out;
186       kiss_fft_scalar * restrict yp2 = out+N2-1;
187       const kiss_twiddle_scalar *t = &l->trig[0];
188       /* Temp pointers to make it really clear to the compiler what we're doing */
189       for(i=0;i<N4;i++)
190       {
191          kiss_fft_scalar yr, yi;
192          yr = S_MUL(fp[1],t[(N4-i)<<shift]) + S_MUL(fp[0],t[i<<shift]);
193          yi = S_MUL(fp[0],t[(N4-i)<<shift]) - S_MUL(fp[1],t[i<<shift]);
194          /* works because the cos is nearly one */
195          *yp1 = yr - S_MUL(yi,sine);
196          *yp2 = yi + S_MUL(yr,sine);;
197          fp += 2;
198          yp1 += 2;
199          yp2 -= 2;
200       }
201    }
202    RESTORE_STACK;
203 }
204
205 void clt_mdct_backward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out,
206       const opus_val16 * restrict window, int overlap, int shift, int stride)
207 {
208    int i;
209    int N, N2, N4;
210    kiss_twiddle_scalar sine;
211    VARDECL(kiss_fft_scalar, f);
212    VARDECL(kiss_fft_scalar, f2);
213    SAVE_STACK;
214    N = l->n;
215    N >>= shift;
216    N2 = N>>1;
217    N4 = N>>2;
218    ALLOC(f, N2, kiss_fft_scalar);
219    ALLOC(f2, N2, kiss_fft_scalar);
220    /* sin(x) ~= x here */
221 #ifdef FIXED_POINT
222    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
223 #else
224    sine = (kiss_twiddle_scalar)2*PI*(.125f)/N;
225 #endif
226
227    /* Pre-rotate */
228    {
229       /* Temp pointers to make it really clear to the compiler what we're doing */
230       const kiss_fft_scalar * restrict xp1 = in;
231       const kiss_fft_scalar * restrict xp2 = in+stride*(N2-1);
232       kiss_fft_scalar * restrict yp = f2;
233       const kiss_twiddle_scalar *t = &l->trig[0];
234       for(i=0;i<N4;i++)
235       {
236          kiss_fft_scalar yr, yi;
237          yr = -S_MUL(*xp2, t[i<<shift]) + S_MUL(*xp1,t[(N4-i)<<shift]);
238          yi =  -S_MUL(*xp2, t[(N4-i)<<shift]) - S_MUL(*xp1,t[i<<shift]);
239          /* works because the cos is nearly one */
240          *yp++ = yr - S_MUL(yi,sine);
241          *yp++ = yi + S_MUL(yr,sine);
242          xp1+=2*stride;
243          xp2-=2*stride;
244       }
245    }
246
247    /* Inverse N/4 complex FFT. This one should *not* downscale even in fixed-point */
248    opus_ifft(l->kfft[shift], (kiss_fft_cpx *)f2, (kiss_fft_cpx *)f);
249
250    /* Post-rotate */
251    {
252       kiss_fft_scalar * restrict fp = f;
253       const kiss_twiddle_scalar *t = &l->trig[0];
254
255       for(i=0;i<N4;i++)
256       {
257          kiss_fft_scalar re, im, yr, yi;
258          re = fp[0];
259          im = fp[1];
260          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
261          yr = S_MUL(re,t[i<<shift]) - S_MUL(im,t[(N4-i)<<shift]);
262          yi = S_MUL(im,t[i<<shift]) + S_MUL(re,t[(N4-i)<<shift]);
263          /* works because the cos is nearly one */
264          *fp++ = yr - S_MUL(yi,sine);
265          *fp++ = yi + S_MUL(yr,sine);
266       }
267    }
268    /* De-shuffle the components for the middle of the window only */
269    {
270       const kiss_fft_scalar * restrict fp1 = f;
271       const kiss_fft_scalar * restrict fp2 = f+N2-1;
272       kiss_fft_scalar * restrict yp = f2;
273       for(i = 0; i < N4; i++)
274       {
275          *yp++ =-*fp1;
276          *yp++ = *fp2;
277          fp1 += 2;
278          fp2 -= 2;
279       }
280    }
281    out -= (N2-overlap)>>1;
282    /* Mirror on both sides for TDAC */
283    {
284       kiss_fft_scalar * restrict fp1 = f2+N4-1;
285       kiss_fft_scalar * restrict xp1 = out+N2-1;
286       kiss_fft_scalar * restrict yp1 = out+N4-overlap/2;
287       const opus_val16 * restrict wp1 = window;
288       const opus_val16 * restrict wp2 = window+overlap-1;
289       for(i = 0; i< N4-overlap/2; i++)
290       {
291          *xp1 = *fp1;
292          xp1--;
293          fp1--;
294       }
295       for(; i < N4; i++)
296       {
297          kiss_fft_scalar x1;
298          x1 = *fp1--;
299          *yp1++ +=-MULT16_32_Q15(*wp1, x1);
300          *xp1-- += MULT16_32_Q15(*wp2, x1);
301          wp1++;
302          wp2--;
303       }
304    }
305    {
306       kiss_fft_scalar * restrict fp2 = f2+N4;
307       kiss_fft_scalar * restrict xp2 = out+N2;
308       kiss_fft_scalar * restrict yp2 = out+N-1-(N4-overlap/2);
309       const opus_val16 * restrict wp1 = window;
310       const opus_val16 * restrict wp2 = window+overlap-1;
311       for(i = 0; i< N4-overlap/2; i++)
312       {
313          *xp2 = *fp2;
314          xp2++;
315          fp2++;
316       }
317       for(; i < N4; i++)
318       {
319          kiss_fft_scalar x2;
320          x2 = *fp2++;
321          *yp2--  = MULT16_32_Q15(*wp1, x2);
322          *xp2++  = MULT16_32_Q15(*wp2, x2);
323          wp1++;
324          wp2--;
325       }
326    }
327    RESTORE_STACK;
328 }