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