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