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