Converting allocation table to 1/32 bit/sample resolution.
[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    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 /* This is a simple MDCT implementation that uses a N/4 complex FFT
34    to do most of the work. It should be relatively straightforward to
35    plug in pretty much and FFT here.
36    
37    This replaces the Vorbis FFT (and uses the exact same API), which 
38    was a bit too messy and that was ending up duplicating code 
39    (might as well use the same FFT everywhere).
40    
41    The algorithm is similar to (and inspired from) Fabrice Bellard's
42    MDCT implementation in FFMPEG, but has differences in signs, ordering
43    and scaling in many places. 
44 */
45
46 #ifndef SKIP_CONFIG_H
47 #ifdef HAVE_CONFIG_H
48 #include "config.h"
49 #endif
50 #endif
51
52 #include "mdct.h"
53 #include "kiss_fft.h"
54 #include "_kiss_fft_guts.h"
55 #include <math.h>
56 #include "os_support.h"
57 #include "mathops.h"
58 #include "stack_alloc.h"
59
60 #ifndef M_PI
61 #define M_PI 3.141592653
62 #endif
63
64 #ifndef STATIC_MODES
65
66 void clt_mdct_init(mdct_lookup *l,int N, int maxshift)
67 {
68    int i;
69    int N2, N4;
70    kiss_twiddle_scalar *trig;
71    l->n = N;
72    N2 = N>>1;
73    N4 = N>>2;
74    l->maxshift = maxshift;
75    for (i=0;i<=maxshift;i++)
76    {
77       if (i==0)
78          l->kfft[i] = kiss_fft_alloc(N>>2>>i, 0, 0);
79       else
80          l->kfft[i] = kiss_fft_alloc_twiddles(N>>2>>i, 0, 0, l->kfft[0]);
81 #ifndef ENABLE_TI_DSPLIB55
82       if (l->kfft[i]==NULL)
83          return;
84 #endif
85    }
86    l->trig = trig = (kiss_twiddle_scalar*)celt_alloc((N4+1)*sizeof(kiss_twiddle_scalar));
87    if (l->trig==NULL)
88      return;
89    /* We have enough points that sine isn't necessary */
90 #if defined(FIXED_POINT)
91    for (i=0;i<=N4;i++)
92       trig[i] = TRIG_UPSCALE*celt_cos_norm(DIV32(ADD32(SHL32(EXTEND32(i),17),N2),N));
93 #else
94    for (i=0;i<=N4;i++)
95       trig[i] = (kiss_twiddle_scalar)cos(2*M_PI*i/N);
96 #endif
97 }
98
99 void clt_mdct_clear(mdct_lookup *l)
100 {
101    int i;
102    for (i=0;i<=l->maxshift;i++)
103       kiss_fft_free(l->kfft[i]);
104    celt_free((kiss_twiddle_scalar*)l->trig);
105 }
106
107 #endif /* STATIC_MODES */
108
109 void clt_mdct_forward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out, const celt_word16 *window, int overlap, int shift)
110 {
111    int i;
112    int N, N2, N4;
113    kiss_twiddle_scalar sine;
114    VARDECL(kiss_fft_scalar, f);
115    SAVE_STACK;
116    N = l->n;
117    N >>= shift;
118    N2 = N>>1;
119    N4 = N>>2;
120    ALLOC(f, N2, kiss_fft_scalar);
121    /* sin(x) ~= x here */
122 #ifdef FIXED_POINT
123    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
124 #else
125    sine = (kiss_twiddle_scalar)2*M_PI*(.125f)/N;
126 #endif
127
128    /* Consider the input to be composed of four blocks: [a, b, c, d] */
129    /* Window, shuffle, fold */
130    {
131       /* Temp pointers to make it really clear to the compiler what we're doing */
132       const kiss_fft_scalar * restrict xp1 = in+(overlap>>1);
133       const kiss_fft_scalar * restrict xp2 = in+N2-1+(overlap>>1);
134       kiss_fft_scalar * restrict yp = out;
135       const celt_word16 * restrict wp1 = window+(overlap>>1);
136       const celt_word16 * restrict wp2 = window+(overlap>>1)-1;
137       for(i=0;i<(overlap>>2);i++)
138       {
139          /* Real part arranged as -d-cR, Imag part arranged as -b+aR*/
140          *yp++ = MULT16_32_Q15(*wp2, xp1[N2]) + MULT16_32_Q15(*wp1,*xp2);
141          *yp++ = MULT16_32_Q15(*wp1, *xp1)    - MULT16_32_Q15(*wp2, xp2[-N2]);
142          xp1+=2;
143          xp2-=2;
144          wp1+=2;
145          wp2-=2;
146       }
147       wp1 = window;
148       wp2 = window+overlap-1;
149       for(;i<N4-(overlap>>2);i++)
150       {
151          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
152          *yp++ = *xp2;
153          *yp++ = *xp1;
154          xp1+=2;
155          xp2-=2;
156       }
157       for(;i<N4;i++)
158       {
159          /* Real part arranged as a-bR, Imag part arranged as -c-dR */
160          *yp++ =  -MULT16_32_Q15(*wp1, xp1[-N2]) + MULT16_32_Q15(*wp2, *xp2);
161          *yp++ = MULT16_32_Q15(*wp2, *xp1)     + MULT16_32_Q15(*wp1, xp2[N2]);
162          xp1+=2;
163          xp2-=2;
164          wp1+=2;
165          wp2-=2;
166       }
167    }
168    /* Pre-rotation */
169    {
170       kiss_fft_scalar * restrict yp = out;
171       const kiss_twiddle_scalar *t = &l->trig[0];
172       for(i=0;i<N4;i++)
173       {
174          kiss_fft_scalar re, im, yr, yi;
175          re = yp[0];
176          im = yp[1];
177          yr = -S_MUL(re,t[i<<shift])  -  S_MUL(im,t[(N4-i)<<shift]);
178          yi = -S_MUL(im,t[i<<shift])  +  S_MUL(re,t[(N4-i)<<shift]);
179          /* works because the cos is nearly one */
180          *yp++ = yr + S_MUL(yi,sine);
181          *yp++ = yi - S_MUL(yr,sine);
182       }
183    }
184
185    /* N/4 complex FFT, down-scales by 4/N */
186    kiss_fft(l->kfft[shift], (kiss_fft_cpx *)out, (kiss_fft_cpx *)f);
187
188    /* Post-rotate */
189    {
190       /* Temp pointers to make it really clear to the compiler what we're doing */
191       const kiss_fft_scalar * restrict fp = f;
192       kiss_fft_scalar * restrict yp1 = out;
193       kiss_fft_scalar * restrict yp2 = out+N2-1;
194       const kiss_twiddle_scalar *t = &l->trig[0];
195       /* Temp pointers to make it really clear to the compiler what we're doing */
196       for(i=0;i<N4;i++)
197       {
198          kiss_fft_scalar yr, yi;
199          yr = S_MUL(fp[1],t[(N4-i)<<shift]) + S_MUL(fp[0],t[i<<shift]);
200          yi = S_MUL(fp[0],t[(N4-i)<<shift]) - S_MUL(fp[1],t[i<<shift]);
201          /* works because the cos is nearly one */
202          *yp1 = yr - S_MUL(yi,sine);
203          *yp2 = yi + S_MUL(yr,sine);;
204          fp += 2;
205          yp1 += 2;
206          yp2 -= 2;
207       }
208    }
209    RESTORE_STACK;
210 }
211
212
213 void clt_mdct_backward(const mdct_lookup *l, kiss_fft_scalar *in, kiss_fft_scalar * restrict out, const celt_word16 * restrict window, int overlap, int shift)
214 {
215    int i;
216    int N, N2, N4;
217    kiss_twiddle_scalar sine;
218    VARDECL(kiss_fft_scalar, f);
219    VARDECL(kiss_fft_scalar, f2);
220    SAVE_STACK;
221    N = l->n;
222    N >>= shift;
223    N2 = N>>1;
224    N4 = N>>2;
225    ALLOC(f, N2, kiss_fft_scalar);
226    ALLOC(f2, N2, kiss_fft_scalar);
227    /* sin(x) ~= x here */
228 #ifdef FIXED_POINT
229    sine = TRIG_UPSCALE*(QCONST16(0.7853981f, 15)+N2)/N;
230 #else
231    sine = (kiss_twiddle_scalar)2*M_PI*(.125f)/N;
232 #endif
233    
234    /* Pre-rotate */
235    {
236       /* Temp pointers to make it really clear to the compiler what we're doing */
237       const kiss_fft_scalar * restrict xp1 = in;
238       const kiss_fft_scalar * restrict xp2 = in+N2-1;
239       kiss_fft_scalar * restrict yp = f2;
240       const kiss_twiddle_scalar *t = &l->trig[0];
241       for(i=0;i<N4;i++) 
242       {
243          kiss_fft_scalar yr, yi;
244          yr = -S_MUL(*xp2, t[i<<shift]) + S_MUL(*xp1,t[(N4-i)<<shift]);
245          yi =  -S_MUL(*xp2, t[(N4-i)<<shift]) - S_MUL(*xp1,t[i<<shift]);
246          /* works because the cos is nearly one */
247          *yp++ = yr - S_MUL(yi,sine);
248          *yp++ = yi + S_MUL(yr,sine);
249          xp1+=2;
250          xp2-=2;
251       }
252    }
253
254    /* Inverse N/4 complex FFT. This one should *not* downscale even in fixed-point */
255    kiss_ifft(l->kfft[shift], (kiss_fft_cpx *)f2, (kiss_fft_cpx *)f);
256    
257    /* Post-rotate */
258    {
259       kiss_fft_scalar * restrict fp = f;
260       const kiss_twiddle_scalar *t = &l->trig[0];
261
262       for(i=0;i<N4;i++)
263       {
264          kiss_fft_scalar re, im, yr, yi;
265          re = fp[0];
266          im = fp[1];
267          /* We'd scale up by 2 here, but instead it's done when mixing the windows */
268          yr = S_MUL(re,t[i<<shift]) - S_MUL(im,t[(N4-i)<<shift]);
269          yi = S_MUL(im,t[i<<shift]) + S_MUL(re,t[(N4-i)<<shift]);
270          /* works because the cos is nearly one */
271          *fp++ = yr - S_MUL(yi,sine);
272          *fp++ = yi + S_MUL(yr,sine);
273       }
274    }
275    /* De-shuffle the components for the middle of the window only */
276    {
277       const kiss_fft_scalar * restrict fp1 = f;
278       const kiss_fft_scalar * restrict fp2 = f+N2-1;
279       kiss_fft_scalar * restrict yp = f2;
280       for(i = 0; i < N4; i++)
281       {
282          *yp++ =-*fp1;
283          *yp++ = *fp2;
284          fp1 += 2;
285          fp2 -= 2;
286       }
287    }
288    out -= (N2-overlap)>>1;
289    /* Mirror on both sides for TDAC */
290    {
291       kiss_fft_scalar * restrict fp1 = f2+N4-1;
292       kiss_fft_scalar * restrict xp1 = out+N2-1;
293       kiss_fft_scalar * restrict yp1 = out+N4-overlap/2;
294       const celt_word16 * restrict wp1 = window;
295       const celt_word16 * restrict wp2 = window+overlap-1;
296       for(i = 0; i< N4-overlap/2; i++)
297       {
298          *xp1 = *fp1;
299          xp1--;
300          fp1--;
301       }
302       for(; i < N4; i++)
303       {
304          kiss_fft_scalar x1;
305          x1 = *fp1--;
306          *yp1++ +=-MULT16_32_Q15(*wp1, x1);
307          *xp1-- += MULT16_32_Q15(*wp2, x1);
308          wp1++;
309          wp2--;
310       }
311    }
312    {
313       kiss_fft_scalar * restrict fp2 = f2+N4;
314       kiss_fft_scalar * restrict xp2 = out+N2;
315       kiss_fft_scalar * restrict yp2 = out+N-1-(N4-overlap/2);
316       const celt_word16 * restrict wp1 = window;
317       const celt_word16 * restrict wp2 = window+overlap-1;
318       for(i = 0; i< N4-overlap/2; i++)
319       {
320          *xp2 = *fp2;
321          xp2++;
322          fp2++;
323       }
324       for(; i < N4; i++)
325       {
326          kiss_fft_scalar x2;
327          x2 = *fp2++;
328          *yp2--  = MULT16_32_Q15(*wp1, x2);
329          *xp2++  = MULT16_32_Q15(*wp2, x2);
330          wp1++;
331          wp2--;
332       }
333    }
334    RESTORE_STACK;
335 }
336
337