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
#ifndef FIXED_POINT
409
        st->scale = 1.f/nfft;
410
#endif
411
412
413
414
415
416
417
        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)
418
              goto fail;
419
        } else {
Jean-Marc Valin's avatar
Jean-Marc Valin committed
420
421
           st->twiddles = twiddles = (kiss_twiddle_cpx*)KISS_FFT_MALLOC(sizeof(kiss_twiddle_cpx)*nfft);
           compute_twiddles(twiddles, nfft);
422
           st->shift = -1;
423
        }
424
425
        if (!kf_factor(nfft,st->factors))
        {
426
           goto fail;
427
        }
Jean-Marc Valin's avatar
Jean-Marc Valin committed
428

429
        /* bitrev */
430
        st->bitrev = bitrev = (opus_int16*)KISS_FFT_MALLOC(sizeof(opus_int16)*nfft);
431
432
        if (st->bitrev==NULL)
            goto fail;
Jean-Marc Valin's avatar
Jean-Marc Valin committed
433
        compute_bitrev_table(0, bitrev, 1,1, st->factors,st);
434
435
    }
    return st;
436
fail:
437
    opus_fft_free(st);
438
    return NULL;
439
440
}

441
kiss_fft_state *opus_fft_alloc(int nfft,void * mem,size_t * lenmem )
442
{
443
   return opus_fft_alloc_twiddles(nfft, mem, lenmem, NULL);
444
}
445

446
void opus_fft_free(const kiss_fft_state *cfg)
447
{
448
449
   if (cfg)
   {
450
      opus_free((opus_int16*)cfg->bitrev);
451
      if (cfg->shift < 0)
452
453
         opus_free((kiss_twiddle_cpx*)cfg->twiddles);
      opus_free((kiss_fft_state*)cfg);
454
   }
455
456
}

457
#endif /* CUSTOM_MODES */
458

459
void opus_fft_impl(const kiss_fft_state *st,kiss_fft_cpx *fout)
460
{
461
462
463
464
    int m2, m;
    int p;
    int L;
    int fstride[MAXFACTORS];
Jean-Marc Valin's avatar
Jean-Marc Valin committed
465
    int i;
466
467
468
469
470
471
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
    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;
    }
505
506
}

507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
void opus_fft(const kiss_fft_state *st,const kiss_fft_cpx *fin,kiss_fft_cpx *fout)
{
   int i;
#ifdef FIXED_POINT
   /* FIXME: This should eventually just go in the state. */
   opus_val16 scale;
   int scale_shift;
   scale_shift = celt_ilog2(st->nfft);
   if (st->nfft == 1<<scale_shift)
      scale = Q15ONE;
   else
      scale = (1073741824+st->nfft/2)/st->nfft>>(15-scale_shift);
#endif

   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];
#ifdef FIXED_POINT
      fout[st->bitrev[i]].r = SHR32(MULT16_32_Q15(scale, x.r), scale_shift);
      fout[st->bitrev[i]].i = SHR32(MULT16_32_Q15(scale, x.i), scale_shift);
#else
      fout[st->bitrev[i]].r = st->scale*x.r;
      fout[st->bitrev[i]].i = st->scale*x.i;
#endif
   }
   opus_fft_impl(st, fout);
}

537

538
539
540
541
542
543
544
545
#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];
546
547
548
549
550
   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;
551
552
}
#endif