kiss_fft.c 15 KB
Newer Older
1
2
3
4
/*Copyright (c) 2003-2004, Mark Borgerding
  Lots of modifications by Jean-Marc Valin
  Copyright (c) 2005-2007, Xiph.Org Foundation
  Copyright (c) 2008,      Xiph.Org Foundation, CSIRO
5

6
  All rights reserved.
7

8
9
  Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are met:
10

11
12
13
14
15
    * Redistributions of source code must retain the above copyright notice,
       this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
       this list of conditions and the following disclaimer in the
       documentation and/or other materials provided with the distribution.
16

17
18
19
20
21
22
23
24
25
26
27
  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  POSSIBILITY OF SUCH DAMAGE.*/
28
29
30
31

/* This code is originally from Mark Borgerding's KISS-FFT but has been
   heavily modified to better suit Opus */

32
33
34
35
#ifndef SKIP_CONFIG_H
#  ifdef HAVE_CONFIG_H
#    include "config.h"
#  endif
36
37
38
39
40
#endif

#include "_kiss_fft_guts.h"
#include "arch.h"
#include "os_support.h"
41
#include "mathops.h"
42
#include "stack_alloc.h"
43
44

/* The guts header contains all the multiplication and addition macros that are defined for
45
46
   complex numbers.  It also delares the kf_ internal functions.
*/
47
48

static void kf_bfly2(
49
50
                     kiss_fft_cpx * Fout,
                     const size_t fstride,
51
                     const kiss_fft_state *st,
52
53
54
55
                     int m,
                     int N,
                     int mm
                    )
56
{
57
   kiss_fft_cpx * Fout2;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
58
   const kiss_twiddle_cpx * tw1;
59
60
61
62
63
64
65
   int i,j;
   kiss_fft_cpx * Fout_beg = Fout;
   for (i=0;i<N;i++)
   {
      Fout = Fout_beg + i*mm;
      Fout2 = Fout + m;
      tw1 = st->twiddles;
66
      /* For non-custom modes, m is guaranteed to be a multiple of 4. */
67
68
      for(j=0;j<m;j++)
      {
69
         kiss_fft_cpx t;
70
71
72
         C_MUL (t,  *Fout2 , *tw1);
         tw1 += fstride;
         C_SUB( *Fout2 ,  *Fout , t );
73
         C_ADDTO( *Fout ,  t );
74
75
76
77
         ++Fout2;
         ++Fout;
      }
   }
78
79
}

80
81
82
static void kf_bfly4(
                     kiss_fft_cpx * Fout,
                     const size_t fstride,
83
                     const kiss_fft_state *st,
84
85
86
87
88
                     int m,
                     int N,
                     int mm
                    )
{
89
   int i;
90

91
   if (m==1)
92
   {
93
94
      /* Degenerate case where all the twiddles are 1. */
      for (i=0;i<N;i++)
95
      {
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
         kiss_fft_cpx scratch0, scratch1;

         C_SUB( scratch0 , *Fout, Fout[2] );
         C_ADDTO(*Fout, Fout[2]);
         C_ADD( scratch1 , Fout[1] , Fout[3] );
         C_SUB( Fout[2], *Fout, scratch1 );
         C_ADDTO( *Fout , scratch1 );
         C_SUB( scratch1 , Fout[1] , Fout[3] );

         Fout[1].r = scratch0.r + scratch1.i;
         Fout[1].i = scratch0.i - scratch1.r;
         Fout[3].r = scratch0.r - scratch1.i;
         Fout[3].i = scratch0.i + scratch1.r;
         Fout+=4;
      }
   } else {
      int j;
      kiss_fft_cpx scratch[6];
      const kiss_twiddle_cpx *tw1,*tw2,*tw3;
      const int m2=2*m;
      const int m3=3*m;
      kiss_fft_cpx * Fout_beg = Fout;
      for (i=0;i<N;i++)
      {
         Fout = Fout_beg + i*mm;
         tw3 = tw2 = tw1 = st->twiddles;
122
123
         /* For non-custom modes, m=4, otherwise m is guaranteed to be a
            multiple of 4. */
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
         for (j=0;j<m;j++)
         {
            C_MUL(scratch[0],Fout[m] , *tw1 );
            C_MUL(scratch[1],Fout[m2] , *tw2 );
            C_MUL(scratch[2],Fout[m3] , *tw3 );

            C_SUB( scratch[5] , *Fout, scratch[1] );
            C_ADDTO(*Fout, scratch[1]);
            C_ADD( scratch[3] , scratch[0] , scratch[2] );
            C_SUB( scratch[4] , scratch[0] , scratch[2] );
            C_SUB( Fout[m2], *Fout, scratch[3] );
            tw1 += fstride;
            tw2 += fstride*2;
            tw3 += fstride*3;
            C_ADDTO( *Fout , scratch[3] );

            Fout[m].r = scratch[5].r + scratch[4].i;
            Fout[m].i = scratch[5].i - scratch[4].r;
            Fout[m3].r = scratch[5].r - scratch[4].i;
            Fout[m3].i = scratch[5].i + scratch[4].r;
            ++Fout;
         }
146
147
148
149
      }
   }
}

150

151
#ifndef RADIX_TWO_ONLY
152

153
static void kf_bfly3(
154
155
                     kiss_fft_cpx * Fout,
                     const size_t fstride,
156
                     const kiss_fft_state *st,
157
158
159
                     int m,
                     int N,
                     int mm
160
                    )
161
{
162
163
   int i;
   size_t k;
164
   const size_t m2 = 2*m;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
165
   const kiss_twiddle_cpx *tw1,*tw2;
166
   kiss_fft_cpx scratch[5];
167
   kiss_twiddle_cpx epi3;
168

169
170
171
172
173
174
   kiss_fft_cpx * Fout_beg = Fout;
   epi3 = st->twiddles[fstride*m];
   for (i=0;i<N;i++)
   {
      Fout = Fout_beg + i*mm;
      tw1=tw2=st->twiddles;
175
      /* For non-custom modes, m is guaranteed to be a multiple of 4. */
176
177
      k=m;
      do {
178

179
180
         C_MUL(scratch[1],Fout[m] , *tw1);
         C_MUL(scratch[2],Fout[m2] , *tw2);
181

182
183
184
185
         C_ADD(scratch[3],scratch[1],scratch[2]);
         C_SUB(scratch[0],scratch[1],scratch[2]);
         tw1 += fstride;
         tw2 += fstride*2;
186

187
188
         Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
         Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
189

190
         C_MULBYSCALAR( scratch[0] , epi3.i );
191

192
         C_ADDTO(*Fout,scratch[3]);
193

194
195
         Fout[m2].r = Fout[m].r + scratch[0].i;
         Fout[m2].i = Fout[m].i - scratch[0].r;
196

197
198
         Fout[m].r -= scratch[0].i;
         Fout[m].i += scratch[0].r;
199

200
201
202
         ++Fout;
      } while(--k);
   }
203
}
204

205

206
207
208
static void kf_bfly5(
                     kiss_fft_cpx * Fout,
                     const size_t fstride,
209
                     const kiss_fft_state *st,
210
211
212
                     int m,
                     int N,
                     int mm
213
214
215
                    )
{
   kiss_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
216
   int i, u;
217
   kiss_fft_cpx scratch[13];
Jean-Marc Valin's avatar
Jean-Marc Valin committed
218
219
   const kiss_twiddle_cpx * twiddles = st->twiddles;
   const kiss_twiddle_cpx *tw;
220
   kiss_twiddle_cpx ya,yb;
221
222
   kiss_fft_cpx * Fout_beg = Fout;

223
224
   ya = twiddles[fstride*m];
   yb = twiddles[fstride*2*m];
225
   tw=st->twiddles;
226

227
228
229
230
231
232
233
234
   for (i=0;i<N;i++)
   {
      Fout = Fout_beg + i*mm;
      Fout0=Fout;
      Fout1=Fout0+m;
      Fout2=Fout0+2*m;
      Fout3=Fout0+3*m;
      Fout4=Fout0+4*m;
235

236
      /* For non-custom modes, m is guaranteed to be a multiple of 4. */
237
238
      for ( u=0; u<m; ++u ) {
         scratch[0] = *Fout0;
239

240
241
242
243
         C_MUL(scratch[1] ,*Fout1, tw[u*fstride]);
         C_MUL(scratch[2] ,*Fout2, tw[2*u*fstride]);
         C_MUL(scratch[3] ,*Fout3, tw[3*u*fstride]);
         C_MUL(scratch[4] ,*Fout4, tw[4*u*fstride]);
244

245
246
247
248
         C_ADD( scratch[7],scratch[1],scratch[4]);
         C_SUB( scratch[10],scratch[1],scratch[4]);
         C_ADD( scratch[8],scratch[2],scratch[3]);
         C_SUB( scratch[9],scratch[2],scratch[3]);
249

250
251
         Fout0->r += scratch[7].r + scratch[8].r;
         Fout0->i += scratch[7].i + scratch[8].i;
252

253
254
         scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
         scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
255

256
257
         scratch[6].r =  S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
         scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
258

259
260
         C_SUB(*Fout1,scratch[5],scratch[6]);
         C_ADD(*Fout4,scratch[5],scratch[6]);
261

262
263
264
265
         scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
         scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
         scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
         scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
266

267
268
         C_ADD(*Fout2,scratch[11],scratch[12]);
         C_SUB(*Fout3,scratch[11],scratch[12]);
269

270
271
         ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
      }
272
273
   }
}
274

275

276
#endif
277

278

279
#ifdef CUSTOM_MODES
280
281
282
283

static
void compute_bitrev_table(
         int Fout,
284
         opus_int16 *f,
285
286
         const size_t fstride,
         int in_stride,
287
         opus_int16 * factors,
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
         const kiss_fft_state *st
            )
{
   const int p=*factors++; /* the radix  */
   const int m=*factors++; /* stage's fft length/p */

    /*printf ("fft %d %d %d %d %d %d\n", p*m, m, p, s2, fstride*in_stride, N);*/
   if (m==1)
   {
      int j;
      for (j=0;j<p;j++)
      {
         *f = Fout+j;
         f += fstride*in_stride;
      }
   } else {
      int j;
      for (j=0;j<p;j++)
      {
         compute_bitrev_table( Fout , f, fstride*p, in_stride, factors,st);
         f += fstride*in_stride;
         Fout += m;
      }
   }
}

314
/*  facbuf is populated by p1,m1,p2,m2, ...
315
    where
316
317
    p[i] * m[i] = m[i-1]
    m0 = n                  */
318
static
319
int kf_factor(int n,opus_int16 * facbuf)
320
321
{
    int p=4;
322
323
324
    int i;
    int stages=0;
    int nbak = n;
325
326
327
328
329
330
331
332
333

    /*factor out powers of 4, powers of 2, then any remaining primes */
    do {
        while (n % p) {
            switch (p) {
                case 4: p = 2; break;
                case 2: p = 3; break;
                default: p += 2; break;
            }
334
            if (p>32000 || (opus_int32)p*(opus_int32)p > n)
335
336
337
                p = n;          /* no more factors, skip to end */
        }
        n /= p;
338
339
340
#ifdef RADIX_TWO_ONLY
        if (p!=2 && p != 4)
#else
341
        if (p>5)
342
#endif
343
344
345
        {
           return 0;
        }
346
347
        facbuf[2*stages] = p;
        stages++;
348
    } while (n > 1);
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
    n = nbak;
    /* Reverse the order to get the radix 4 at the end, so we can use the
       fast degenerate case. It turns out that reversing the order also
       improves the noise behaviour. */
    for (i=0;i<stages/2;i++)
    {
       int tmp;
       tmp = facbuf[2*i];
       facbuf[2*i] = facbuf[2*(stages-i-1)];
       facbuf[2*(stages-i-1)] = tmp;
    }
    for (i=0;i<stages;i++)
    {
        n /= facbuf[2*i];
        facbuf[2*i+1] = n;
    }
365
    return 1;
366
}
367
368
369
370

static void compute_twiddles(kiss_twiddle_cpx *twiddles, int nfft)
{
   int i;
371
#ifdef FIXED_POINT
372
   for (i=0;i<nfft;++i) {
373
      opus_val32 phase = -i;
374
375
376
377
378
379
380
381
382
383
384
      kf_cexp2(twiddles+i, DIV32(SHL32(phase,17),nfft));
   }
#else
   for (i=0;i<nfft;++i) {
      const double pi=3.14159265358979323846264338327;
      double phase = ( -2*pi /nfft ) * i;
      kf_cexp(twiddles+i, phase );
   }
#endif
}

385
386
/*
 *
387
388
389
 * Allocates all necessary storage space for the fft and ifft.
 * The return value is a contiguous block of memory.  As such,
 * It can be freed with free().
390
 * */
391
kiss_fft_state *opus_fft_alloc_twiddles(int nfft,void * mem,size_t * lenmem,  const kiss_fft_state *base)
392
{
393
    kiss_fft_state *st=NULL;
394
    size_t memneeded = sizeof(struct kiss_fft_state); /* twiddle factors*/
395
396

    if ( lenmem==NULL ) {
397
        st = ( kiss_fft_state*)KISS_FFT_MALLOC( memneeded );
398
399
    }else{
        if (mem != NULL && *lenmem >= memneeded)
400
            st = (kiss_fft_state*)mem;
401
402
403
        *lenmem = memneeded;
    }
    if (st) {
404
        opus_int16 *bitrev;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
405
406
        kiss_twiddle_cpx *twiddles;

407
        st->nfft=nfft;
408
409
410
411
412
413
414
#ifdef FIXED_POINT
        st->scale_shift = celt_ilog2(st->nfft);
        if (st->nfft == 1<<st->scale_shift)
           st->scale = Q15ONE;
        else
           st->scale = (1073741824+st->nfft/2)/st->nfft>>(15-st->scale_shift);
#else
415
        st->scale = 1.f/nfft;
416
#endif
417
418
419
420
421
422
423
        if (base != NULL)
        {
           st->twiddles = base->twiddles;
           st->shift = 0;
           while (nfft<<st->shift != base->nfft && st->shift < 32)
              st->shift++;
           if (st->shift>=32)
424
              goto fail;
425
        } else {
Jean-Marc Valin's avatar
Jean-Marc Valin committed
426
427
           st->twiddles = twiddles = (kiss_twiddle_cpx*)KISS_FFT_MALLOC(sizeof(kiss_twiddle_cpx)*nfft);
           compute_twiddles(twiddles, nfft);
428
           st->shift = -1;
429
        }
430
431
        if (!kf_factor(nfft,st->factors))
        {
432
           goto fail;
433
        }
Jean-Marc Valin's avatar
Jean-Marc Valin committed
434

435
        /* bitrev */
436
        st->bitrev = bitrev = (opus_int16*)KISS_FFT_MALLOC(sizeof(opus_int16)*nfft);
437
438
        if (st->bitrev==NULL)
            goto fail;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
439
        compute_bitrev_table(0, bitrev, 1,1, st->factors,st);
440
441
    }
    return st;
442
fail:
443
    opus_fft_free(st);
444
    return NULL;
445
446
}

447
kiss_fft_state *opus_fft_alloc(int nfft,void * mem,size_t * lenmem )
448
{
449
   return opus_fft_alloc_twiddles(nfft, mem, lenmem, NULL);
450
}
451

452
void opus_fft_free(const kiss_fft_state *cfg)
453
{
454
455
   if (cfg)
   {
456
      opus_free((opus_int16*)cfg->bitrev);
457
      if (cfg->shift < 0)
458
459
         opus_free((kiss_twiddle_cpx*)cfg->twiddles);
      opus_free((kiss_fft_state*)cfg);
460
   }
461
462
}

463
#endif /* CUSTOM_MODES */
464

465
void opus_fft_impl(const kiss_fft_state *st,kiss_fft_cpx *fout)
466
{
467
468
469
470
    int m2, m;
    int p;
    int L;
    int fstride[MAXFACTORS];
Jean-Marc Valin's avatar
Jean-Marc Valin committed
471
    int i;
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
    int shift;

    /* st->shift can be -1 */
    shift = st->shift>0 ? st->shift : 0;

    fstride[0] = 1;
    L=0;
    do {
       p = st->factors[2*L];
       m = st->factors[2*L+1];
       fstride[L+1] = fstride[L]*p;
       L++;
    } while(m!=1);
    m = st->factors[2*L-1];
    for (i=L-1;i>=0;i--)
    {
       if (i!=0)
          m2 = st->factors[2*i-1];
       else
          m2 = 1;
       switch (st->factors[2*i])
       {
       case 2:
          kf_bfly2(fout,fstride[i]<<shift,st,m, fstride[i], m2);
          break;
       case 4:
          kf_bfly4(fout,fstride[i]<<shift,st,m, fstride[i], m2);
          break;
 #ifndef RADIX_TWO_ONLY
       case 3:
          kf_bfly3(fout,fstride[i]<<shift,st,m, fstride[i], m2);
          break;
       case 5:
          kf_bfly5(fout,fstride[i]<<shift,st,m, fstride[i], m2);
          break;
 #endif
       }
       m = m2;
    }
511
512
}

513
514
515
516
void opus_fft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
{
   int i;
   opus_val16 scale;
517
518
519
520
#ifdef FIXED_POINT
   /* Allows us to scale with MULT16_32_Q16(), which is faster than
      MULT16_32_Q15() on ARM. */
   int scale_shift = st->scale_shift-1;
521
#endif
522
   scale = st->scale;
523
524
525
526
527
528

   celt_assert2 (fin != fout, "In-place FFT not supported");
   /* Bit-reverse the input */
   for (i=0;i<st->nfft;i++)
   {
      kiss_fft_cpx x = fin[i];
529
530
      fout[st->bitrev[i]].r = SHR32(MULT16_32_Q16(scale, x.r), scale_shift);
      fout[st->bitrev[i]].i = SHR32(MULT16_32_Q16(scale, x.i), scale_shift);
531
532
533
534
   }
   opus_fft_impl(st, fout);
}

535

536
537
538
539
540
541
542
543
#ifdef TEST_UNIT_DFT_C
void opus_ifft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
{
   int i;
   celt_assert2 (fin != fout, "In-place FFT not supported");
   /* Bit-reverse the input */
   for (i=0;i<st->nfft;i++)
      fout[st->bitrev[i]] = fin[i];
544
545
546
547
548
   for (i=0;i<st->nfft;i++)
      fout[i].i = -fout[i].i;
   opus_fft_impl(st, fout);
   for (i=0;i<st->nfft;i++)
      fout[i].i = -fout[i].i;
549
550
}
#endif