CELT_SET_END_BAND_REQUEST in the decoder was performing the wrong bounds check and...
[opus.git] / libcelt / c64_fft.c
1 /* (c) Copyright 2008/2009 Xiph.Org Foundation */
2 /*
3    Redistribution and use in source and binary forms, with or without
4    modification, are permitted provided that the following conditions
5    are met:
6    
7    - Redistributions of source code must retain the above copyright
8    notice, this list of conditions and the following disclaimer.
9    
10    - Redistributions in binary form must reproduce the above copyright
11    notice, this list of conditions and the following disclaimer in the
12    documentation and/or other materials provided with the distribution.
13    
14    - Neither the name of the Xiph.org Foundation nor the names of its
15    contributors may be used to endorse or promote products derived from
16    this software without specific prior written permission.
17    
18    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
21    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
22    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
23    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
24    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
25    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
27    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
28    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
29 */
30
31 #include "c64_fft.h"
32
33 #include "dsp_fft16x16t.h"
34 #include "dsp_fft32x32s.h"
35 #include "dsp_ifft32x32.h"
36
37 #ifndef PI
38 # ifdef M_PI
39 #  define PI M_PI
40 # else
41 #  define PI 3.14159265358979323846
42 # endif
43 #endif
44
45
46 /* ======================================================================== */
47 /*  D2S -- Truncate a 'double' to a 'short', with clamping.                 */
48 /* ======================================================================== */
49 static short d2s(double d)
50 {
51   if (d >=  32767.0) return  32767;
52   if (d <= -32768.0) return -32768;
53   return (short)d;
54 }
55
56
57 /* ======================================================================== */
58 /*  D2S -- Truncate a 'double' to a 'int',   with clamping.                 */
59 /* ======================================================================== */
60 static int d2i(double d)
61 {
62   if (d >=  2147483647.0) return (int)0x7FFFFFFF;
63   if (d <= -2147483648.0) return (int)0x80000000;
64   return (int)d;
65 }
66
67
68 /* ======================================================================== */
69 /*  GEN_TWIDDLE -- Generate twiddle factors for TI's custom FFTs.           */
70 /*                                                                          */
71 /*  USAGE                                                                   */
72 /*      This routine is called as follows:                                  */
73 /*                                                                          */
74 /*          int gen_twiddle(short *w, int n, double scale)                  */
75 /*                                                                          */
76 /*          short  *w     Pointer to twiddle-factor array                   */
77 /*          int    n      Size of FFT                                       */
78 /*          double scale  Scale factor to apply to values.                  */
79 /*                                                                          */
80 /*      The routine will generate the twiddle-factors directly into the     */
81 /*      array you specify.  The array needs to be approximately 2*N         */
82 /*      elements long.  (The actual size, which is slightly smaller, is     */
83 /*      returned by the function.)                                          */
84 /* ======================================================================== */
85 int gen_twiddle16(short *w, int n, double scale)
86 {
87   int i, j, k;
88   
89   for (j = 1, k = 0; j < n >> 2; j = j << 2)
90     {
91       for (i = 0; i < n >> 2; i += j << 1)
92         {
93           w[k + 11] = d2s(scale * cos(6.0 * PI * (i + j) / n));
94           w[k + 10] = d2s(scale * sin(6.0 * PI * (i + j) / n));
95           w[k +  9] = d2s(scale * cos(6.0 * PI * (i    ) / n));
96           w[k +  8] = d2s(scale * sin(6.0 * PI * (i    ) / n));
97           
98           w[k +  7] = d2s(scale * cos(4.0 * PI * (i + j) / n));
99           w[k +  6] = d2s(scale * sin(4.0 * PI * (i + j) / n));
100           w[k +  5] = d2s(scale * cos(4.0 * PI * (i    ) / n));
101           w[k +  4] = d2s(scale * sin(4.0 * PI * (i    ) / n));
102           
103           w[k +  3] = d2s(scale * cos(2.0 * PI * (i + j) / n));
104           w[k +  2] = d2s(scale * sin(2.0 * PI * (i + j) / n));
105           w[k +  1] = d2s(scale * cos(2.0 * PI * (i    ) / n));
106           w[k +  0] = d2s(scale * sin(2.0 * PI * (i    ) / n));
107           
108           k += 12;
109         }
110     }
111   
112   return k;
113 }
114
115
116 /* ======================================================================== */
117 /*  GEN_TWIDDLE -- Generate twiddle factors for TI's custom FFTs.           */
118 /*                                                                          */
119 /*  USAGE                                                                   */
120 /*      This routine is called as follows:                                  */
121 /*                                                                          */
122 /*          int gen_twiddle(int *w, int n, double scale)                    */
123 /*                                                                          */
124 /*          int    *w     Pointer to twiddle-factor array                   */
125 /*          int    n      Size of FFT                                       */
126 /*          double scale  Scale factor to apply to values.                  */
127 /*                                                                          */
128 /*      The routine will generate the twiddle-factors directly into the     */
129 /*      array you specify.  The array needs to be approximately 2*N         */
130 /*      elements long.  (The actual size, which is slightly smaller, is     */
131 /*      returned by the function.)                                          */
132 /* ======================================================================== */
133 int gen_twiddle32(int *w, int n, double scale)
134 {
135   int i, j, k, s=0, t;
136   
137   for (j = 1, k = 0; j < n >> 2; j = j << 2, s++)
138     {
139       for (i = t=0; i < n >> 2; i += j, t++)
140         {
141           w[k +  5] = d2i(scale * cos(6.0 * PI * i / n));
142           w[k +  4] = d2i(scale * sin(6.0 * PI * i / n));
143           
144           w[k +  3] = d2i(scale * cos(4.0 * PI * i / n));
145           w[k +  2] = d2i(scale * sin(4.0 * PI * i / n));
146           
147           w[k +  1] = d2i(scale * cos(2.0 * PI * i / n));
148           w[k +  0] = d2i(scale * sin(2.0 * PI * i / n));
149           
150           k += 6;
151         }
152     }
153   
154   return k;
155 }
156
157 #define NBCACHE 3
158 static c64_fft_t *cache16[NBCACHE] = {NULL,};
159 static c64_fft_t *cache32[NBCACHE] = {NULL,};
160
161 c64_fft_t *c64_fft16_alloc(int length, int x, int y)
162 {
163   c64_fft_t *state;
164   celt_int16 *w, *iw;
165
166   int i, c;
167
168   for (c = 0; c < NBCACHE; c++) {
169     if (cache16[c] == NULL)
170       break;
171     if (cache16[c]->nfft == length)
172       return cache16[c];
173   }
174
175   state = (c64_fft_t *)celt_alloc(sizeof(c64_fft_t));
176   state->shift = log(length)/log(2) - ceil(log(length)/log(4)-1);
177   state->nfft = length;
178   state->twiddle = celt_alloc(length*2*sizeof(celt_int16));
179   state->itwiddle = celt_alloc(length*2*sizeof(celt_int16));
180
181   gen_twiddle16((celt_int16 *)state->twiddle, length, 32767.0);
182
183   w = (celt_int16 *)state->twiddle;
184   iw = (celt_int16 *)state->itwiddle;
185
186   for (i = 0; i < length; i++) {
187     iw[2*i+0] = w[2*i+0];
188     iw[2*i+1] = - w[2*i+1];
189   }
190
191   if (c < NBCACHE)
192     cache16[c++] = state;
193   if (c < NBCACHE)
194     cache16[c] = NULL;
195
196   return state;
197 }
198
199
200 c64_fft_t *c64_fft32_alloc(int length, int x, int y) 
201 {
202   c64_fft_t *state;
203   int i, c;
204
205   for (c = 0; c < NBCACHE; c++) {
206     if (cache32[c] == NULL)
207       break;
208     if (cache32[c]->nfft == length)
209       return cache32[c];
210   }
211
212   state = (c64_fft_t *)celt_alloc(sizeof(c64_fft_t));
213   state->shift = log(length)/log(2) - ceil(log(length)/log(4)-1);
214   state->nfft = length;
215   state->twiddle = celt_alloc(length*2*sizeof(celt_int32));
216   state->itwiddle = celt_alloc(length*2*sizeof(celt_int32));
217
218   // Generate the inverse twiddle first because it does not need scaling
219   gen_twiddle32(state->itwiddle, length, 2147483647.000000000);
220
221   for (i = 0; i < length; i++) {
222     state->twiddle[2*i+0] = state->itwiddle[2*i+0] >> 1;
223     state->twiddle[2*i+1] = state->itwiddle[2*i+1] >> 1;
224   }
225
226   if (c < NBCACHE)
227     cache32[c++] = state;
228   if (c < NBCACHE)
229     cache32[c] = NULL;
230
231   return state;
232 }
233
234
235 void c64_fft16_free(c64_fft_t *state) 
236 {
237   c64_fft32_free(state);
238 }
239
240
241 void c64_fft32_free(c64_fft_t *state)
242 {
243 }
244
245
246 void c64_fft16_inplace(c64_fft_t * restrict state, celt_int16 *X)
247 {
248   int i;
249   VARDECL(celt_int16, cin);
250   VARDECL(celt_int16, cout);
251   SAVE_STACK;
252
253   ALLOC(cin,  state->nfft*2, celt_int16);
254   ALLOC(cout, state->nfft*2, celt_int16);
255
256   for (i = 0; i < state->nfft; i++) {
257     cin[2*i+0] = X[2*i+0];
258     cin[2*i+1] = X[2*i+1];
259   }
260
261   DSP_fft16x16t((celt_int16 *)state->twiddle, state->nfft, cin, cout);
262
263   for (i = 0; i < state->nfft; i++) {
264     X[2*i+0] = cout[2*i+0];
265     X[2*i+1] = cout[2*i+1];
266   }
267   
268   RESTORE_STACK;
269 }
270
271
272
273 void c64_fft32(c64_fft_t * restrict state, const celt_int32 *X, celt_int32 *Y)
274 {
275   int i;
276   VARDECL(celt_int32, cin);
277   SAVE_STACK;
278   ALLOC(cin, state->nfft*2, celt_int32);
279
280   for (i = 0; i < state->nfft; i++) {
281     cin[2*i+0] = X[2*i+0] >> state->shift;
282     cin[2*i+1] = X[2*i+1] >> state->shift;
283   }
284
285   DSP_fft32x32s(state->twiddle, state->nfft, cin, Y);
286
287   RESTORE_STACK;
288 }
289
290
291 void c64_ifft16(c64_fft_t * restrict state, const celt_int16 *X, celt_int16 *Y)
292 {
293   int i;
294   VARDECL(celt_int16, cin);
295   VARDECL(celt_int16, cout);
296   SAVE_STACK;
297
298   ALLOC(cin, state->nfft*2, celt_int16);
299   if ((celt_int32)Y & 7) 
300     ALLOC(cout, state->nfft*2, celt_int16);
301   else
302     cout = Y;
303
304   for (i = 0; i < state->nfft; i++) {
305     // No need to scale for this one but still need to save the input
306     // because the fft is going to change it!
307     cin[2*i+0] = X[2*i+0];
308     cin[2*i+1] = X[2*i+1];
309   }
310
311   DSP_fft16x16t((celt_int16 *)state->itwiddle, state->nfft, cin, cout);
312
313   if ((celt_int32)Y & 7)
314     for (i = 0; i < state->nfft; i++) {
315       Y[2*i+0] = cout[2*i+0];
316       Y[2*i+1] = cout[2*i+1];
317     }
318   
319   RESTORE_STACK;
320 }
321
322
323 void c64_ifft32(c64_fft_t * restrict state, const celt_int32 *X, celt_int32 *Y)
324 {
325   int i;
326   VARDECL(celt_int32, cin);
327   SAVE_STACK;
328   ALLOC(cin, state->nfft*2, celt_int32);
329
330   celt_assert(Y & 7 == 0);
331
332   for (i = 0; i < state->nfft; i++) {
333     // No need to scale for this one but still need to save the input
334     // because the fft is going to change it!
335     cin[2*i+0] = X[2*i+0]; 
336     cin[2*i+1] = X[2*i+1];
337   }
338
339   DSP_ifft32x32(state->itwiddle, state->nfft, cin, Y);
340
341   RESTORE_STACK;
342 }
343
344