git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/TransformFunctions/arm_cfft_radix4_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        15. July 2011
   5 * $Revision:    V1.0.10
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:            arm_cfft_radix4_q31.c
   9 *
  10 * Description:  This file has function definition of Radix-4 FFT & IFFT function and
  11 *                               In-place bit reversal using bit reversal table
  12 *
  13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  14 *
  15 * Version 1.0.10 2011/7/15
  16 *    Big Endian support added and Merged M0 and M3/M4 Source code.
  17 *
  18 * Version 1.0.3 2010/11/29
  19 *    Re-organized the CMSIS folders and updated documentation.
  20 *
  21 * Version 1.0.2 2010/11/11
  22 *    Documentation updated.
  23 *
  24 * Version 1.0.1 2010/10/05
  25 *    Production release and review comments incorporated.
  26 *
  27 * Version 1.0.0 2010/09/20
  28 *    Production release and review comments incorporated.
  29 *
  30 * Version 0.0.5  2010/04/26
  31 *        incorporated review comments and updated with latest CMSIS layer
  32 *
  33 * Version 0.0.3  2010/03/10
  34 *    Initial version
  35 * -------------------------------------------------------------------- */
  36 #include "arm_math.h"
  37
  38
  39 /**
  40  * @ingroup groupTransforms
  41  */
  42
  43 /**
  44  * @addtogroup CFFT_CIFFT
  45  * @{
  46  */
  47
  48 /**
  49  * @details
  50  * @brief Processing function for the Q31 CFFT/CIFFT.
  51  * @param[in]      *S    points to an instance of the Q31 CFFT/CIFFT structure.
  52  * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
  53  * @return none.
  54  *
  55  * \par Input and output formats:
  56  * \par
  57  * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
  58  * Hence the output format is different for different FFT sizes.
  59  * The input and output formats for different FFT sizes and number of bits to upscale are mentioned in the tables below for CFFT and CIFFT:
  60  * \par
  61  * \image html CFFTQ31.gif "Input and Output Formats for Q31 CFFT"
  62  * \image html CIFFTQ31.gif "Input and Output Formats for Q31 CIFFT"
  63  *
  64  */
  65
  66 void arm_cfft_radix4_q31(
  67   const arm_cfft_radix4_instance_q31 * S,
  68   q31_t * pSrc)
  69 {
  70   if(S->ifftFlag == 1u)
  71   {
  72     /* Complex IFFT radix-4 */
  73     arm_radix4_butterfly_inverse_q31(pSrc, S->fftLen, S->pTwiddle,
  74                                      S->twidCoefModifier);
  75   }
  76   else
  77   {
  78     /* Complex FFT radix-4 */
  79     arm_radix4_butterfly_q31(pSrc, S->fftLen, S->pTwiddle,
  80                              S->twidCoefModifier);
  81   }
  82
  83
  84   if(S->bitReverseFlag == 1u)
  85   {
  86     /*  Bit Reversal */
  87     arm_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
  88   }
  89
  90 }
  91
  92 /**
  93  * @} end of CFFT_CIFFT group
  94  */
  95
  96 /*
  97 * Radix-4 FFT algorithm used is :
  98 *
  99 * Input real and imaginary data:
 100 * x(n) = xa + j * ya
 101 * x(n+N/4 ) = xb + j * yb
 102 * x(n+N/2 ) = xc + j * yc
 103 * x(n+3N 4) = xd + j * yd
 104 *
 105 *
 106 * Output real and imaginary data:
 107 * x(4r) = xa'+ j * ya'
 108 * x(4r+1) = xb'+ j * yb'
 109 * x(4r+2) = xc'+ j * yc'
 110 * x(4r+3) = xd'+ j * yd'
 111 *
 112 *
 113 * Twiddle factors for radix-4 FFT:
 114 * Wn = co1 + j * (- si1)
 115 * W2n = co2 + j * (- si2)
 116 * W3n = co3 + j * (- si3)
 117 *
 118 *  Butterfly implementation:
 119 * xa' = xa + xb + xc + xd
 120 * ya' = ya + yb + yc + yd
 121 * xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1)
 122 * yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1)
 123 * xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2)
 124 * yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2)
 125 * xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3)
 126 * yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3)
 127 *
 128 */
 129
 130 /**
 131  * @brief  Core function for the Q31 CFFT butterfly process.
 132  * @param[in, out] *pSrc            points to the in-place buffer of Q31 data type.
 133  * @param[in]      fftLen           length of the FFT.
 134  * @param[in]      *pCoef           points to twiddle coefficient buffer.
 135  * @param[in]      twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 136  * @return none.
 137  */
 138
 139 void arm_radix4_butterfly_q31(
 140   q31_t * pSrc,
 141   uint32_t fftLen,
 142   q31_t * pCoef,
 143   uint32_t twidCoefModifier)
 144 {
 145   uint32_t n1, n2, ia1, ia2, ia3, i0, i1, i2, i3, j, k;
 146   q31_t t1, t2, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3;
 147
 148
 149   /* Total process is divided into three stages */
 150
 151   /* process first stage, middle stages, & last stage */
 152
 153
 154   /* start of first stage process */
 155
 156   /*  Initializations for the first stage */
 157   n2 = fftLen;
 158   n1 = n2;
 159   /* n2 = fftLen/4 */
 160   n2 >>= 2u;
 161   i0 = 0u;
 162   ia1 = 0u;
 163
 164   j = n2;
 165
 166   /*  Calculation of first stage */
 167   do
 168   {
 169     /*  index calculation for the input as, */
 170     /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 171     i1 = i0 + n2;
 172     i2 = i1 + n2;
 173     i3 = i2 + n2;
 174
 175     /* input is in 1.31(q31) format and provide 4 guard bits for the input */
 176
 177     /*  Butterfly implementation */
 178     /* xa + xc */
 179     r1 = (pSrc[(2u * i0)] >> 4u) + (pSrc[(2u * i2)] >> 4u);
 180     /* xa - xc */
 181     r2 = (pSrc[2u * i0] >> 4u) - (pSrc[2u * i2] >> 4u);
 182
 183     /* ya + yc */
 184     s1 = (pSrc[(2u * i0) + 1u] >> 4u) + (pSrc[(2u * i2) + 1u] >> 4u);
 185     /* ya - yc */
 186     s2 = (pSrc[(2u * i0) + 1u] >> 4u) - (pSrc[(2u * i2) + 1u] >> 4u);
 187
 188     /* xb + xd */
 189     t1 = (pSrc[2u * i1] >> 4u) + (pSrc[2u * i3] >> 4u);
 190
 191     /* xa' = xa + xb + xc + xd */
 192     pSrc[2u * i0] = (r1 + t1);
 193     /* (xa + xc) - (xb + xd) */
 194     r1 = r1 - t1;
 195     /* yb + yd */
 196     t2 = (pSrc[(2u * i1) + 1u] >> 4u) + (pSrc[(2u * i3) + 1u] >> 4u);
 197     /* ya' = ya + yb + yc + yd */
 198     pSrc[(2u * i0) + 1u] = (s1 + t2);
 199
 200     /* (ya + yc) - (yb + yd) */
 201     s1 = s1 - t2;
 202
 203     /* yb - yd */
 204     t1 = (pSrc[(2u * i1) + 1u] >> 4u) - (pSrc[(2u * i3) + 1u] >> 4u);
 205     /* xb - xd */
 206     t2 = (pSrc[2u * i1] >> 4u) - (pSrc[2u * i3] >> 4u);
 207
 208     /*  index calculation for the coefficients */
 209     ia2 = 2u * ia1;
 210     co2 = pCoef[ia2 * 2u];
 211     si2 = pCoef[(ia2 * 2u) + 1u];
 212
 213     /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */
 214     pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) +
 215                      ((int32_t) (((q63_t) s1 * si2) >> 32))) << 1u;
 216
 217     /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */
 218     pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) -
 219                             ((int32_t) (((q63_t) r1 * si2) >> 32))) << 1u;
 220
 221     /* (xa - xc) + (yb - yd) */
 222     r1 = r2 + t1;
 223     /* (xa - xc) - (yb - yd) */
 224     r2 = r2 - t1;
 225
 226     /* (ya - yc) - (xb - xd) */
 227     s1 = s2 - t2;
 228     /* (ya - yc) + (xb - xd) */
 229     s2 = s2 + t2;
 230
 231     co1 = pCoef[ia1 * 2u];
 232     si1 = pCoef[(ia1 * 2u) + 1u];
 233
 234     /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */
 235     pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) +
 236                      ((int32_t) (((q63_t) s1 * si1) >> 32))) << 1u;
 237
 238     /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */
 239     pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) -
 240                             ((int32_t) (((q63_t) r1 * si1) >> 32))) << 1u;
 241
 242     /*  index calculation for the coefficients */
 243     ia3 = 3u * ia1;
 244     co3 = pCoef[ia3 * 2u];
 245     si3 = pCoef[(ia3 * 2u) + 1u];
 246
 247     /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */
 248     pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) +
 249                      ((int32_t) (((q63_t) s2 * si3) >> 32))) << 1u;
 250
 251     /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */
 252     pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) -
 253                             ((int32_t) (((q63_t) r2 * si3) >> 32))) << 1u;
 254
 255     /*  Twiddle coefficients index modifier */
 256     ia1 = ia1 + twidCoefModifier;
 257
 258     /*  Updating input index */
 259     i0 = i0 + 1u;
 260
 261   } while(--j);
 262
 263   /* end of first stage process */
 264
 265   /* data is in 5.27(q27) format */
 266
 267
 268   /* start of Middle stages process */
 269
 270
 271   /* each stage in middle stages provides two down scaling of the input */
 272
 273   twidCoefModifier <<= 2u;
 274
 275
 276   for (k = fftLen / 4u; k > 4u; k >>= 2u)
 277   {
 278     /*  Initializations for the first stage */
 279     n1 = n2;
 280     n2 >>= 2u;
 281     ia1 = 0u;
 282
 283     /*  Calculation of first stage */
 284     for (j = 0u; j <= (n2 - 1u); j++)
 285     {
 286       /*  index calculation for the coefficients */
 287       ia2 = ia1 + ia1;
 288       ia3 = ia2 + ia1;
 289       co1 = pCoef[ia1 * 2u];
 290       si1 = pCoef[(ia1 * 2u) + 1u];
 291       co2 = pCoef[ia2 * 2u];
 292       si2 = pCoef[(ia2 * 2u) + 1u];
 293       co3 = pCoef[ia3 * 2u];
 294       si3 = pCoef[(ia3 * 2u) + 1u];
 295       /*  Twiddle coefficients index modifier */
 296       ia1 = ia1 + twidCoefModifier;
 297
 298       for (i0 = j; i0 < fftLen; i0 += n1)
 299       {
 300         /*  index calculation for the input as, */
 301         /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 302         i1 = i0 + n2;
 303         i2 = i1 + n2;
 304         i3 = i2 + n2;
 305
 306         /*  Butterfly implementation */
 307         /* xa + xc */
 308         r1 = pSrc[2u * i0] + pSrc[2u * i2];
 309         /* xa - xc */
 310         r2 = pSrc[2u * i0] - pSrc[2u * i2];
 311
 312         /* ya + yc */
 313         s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
 314         /* ya - yc */
 315         s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
 316
 317         /* xb + xd */
 318         t1 = pSrc[2u * i1] + pSrc[2u * i3];
 319
 320         /* xa' = xa + xb + xc + xd */
 321         pSrc[2u * i0] = (r1 + t1) >> 2u;
 322         /* xa + xc -(xb + xd) */
 323         r1 = r1 - t1;
 324
 325         /* yb + yd */
 326         t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
 327         /* ya' = ya + yb + yc + yd */
 328         pSrc[(2u * i0) + 1u] = (s1 + t2) >> 2u;
 329
 330         /* (ya + yc) - (yb + yd) */
 331         s1 = s1 - t2;
 332
 333         /* (yb - yd) */
 334         t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
 335         /* (xb - xd) */
 336         t2 = pSrc[2u * i1] - pSrc[2u * i3];
 337
 338         /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */
 339         pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) +
 340                          ((int32_t) (((q63_t) s1 * si2) >> 32))) >> 1u;
 341
 342         /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */
 343         pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) -
 344                                 ((int32_t) (((q63_t) r1 * si2) >> 32))) >> 1u;
 345
 346         /* (xa - xc) + (yb - yd) */
 347         r1 = r2 + t1;
 348         /* (xa - xc) - (yb - yd) */
 349         r2 = r2 - t1;
 350
 351         /* (ya - yc) -  (xb - xd) */
 352         s1 = s2 - t2;
 353         /* (ya - yc) +  (xb - xd) */
 354         s2 = s2 + t2;
 355
 356         /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */
 357         pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) +
 358                          ((int32_t) (((q63_t) s1 * si1) >> 32))) >> 1u;
 359
 360         /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */
 361         pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) -
 362                                 ((int32_t) (((q63_t) r1 * si1) >> 32))) >> 1u;
 363
 364         /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */
 365         pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) +
 366                          ((int32_t) (((q63_t) s2 * si3) >> 32))) >> 1u;
 367
 368         /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */
 369         pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) -
 370                                 ((int32_t) (((q63_t) r2 * si3) >> 32))) >> 1u;
 371       }
 372     }
 373     twidCoefModifier <<= 2u;
 374   }
 375
 376   /* End of Middle stages process */
 377
 378   /* data is in 11.21(q21) format for the 1024 point as there are 3 middle stages */
 379   /* data is in 9.23(q23) format for the 256 point as there are 2 middle stages */
 380   /* data is in 7.25(q25) format for the 64 point as there are 1 middle stage */
 381   /* data is in 5.27(q27) format for the 16 point as there are no middle stages */
 382
 383
 384   /* start of Last stage process */
 385
 386   /*  Initializations of last stage */
 387   n1 = n2;
 388   n2 >>= 2u;
 389
 390   /*  Calculations of last stage */
 391   for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1)
 392   {
 393     /*  index calculation for the input as, */
 394     /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 395     i1 = i0 + n2;
 396     i2 = i1 + n2;
 397     i3 = i2 + n2;
 398
 399     /*  Butterfly implementation */
 400     /* xa + xb */
 401     r1 = pSrc[2u * i0] + pSrc[2u * i2];
 402     /* xa - xb */
 403     r2 = pSrc[2u * i0] - pSrc[2u * i2];
 404
 405     /* ya + yc */
 406     s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
 407     /* ya - yc */
 408     s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
 409
 410     /* xc + xd */
 411     t1 = pSrc[2u * i1] + pSrc[2u * i3];
 412     /* xa' = xa + xb + xc + xd */
 413     pSrc[2u * i0] = (r1 + t1);
 414     /* (xa + xb) - (xc + xd) */
 415     r1 = r1 - t1;
 416
 417     /* yb + yd */
 418     t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
 419     /* ya' = ya + yb + yc + yd */
 420     pSrc[(2u * i0) + 1u] = (s1 + t2);
 421     /* (ya + yc) - (yb + yd) */
 422     s1 = s1 - t2;
 423
 424     /* (yb-yd) */
 425     t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
 426     /* (xb-xd) */
 427     t2 = pSrc[2u * i1] - pSrc[2u * i3];
 428
 429     /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */
 430     pSrc[2u * i1] = r1;
 431     /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */
 432     pSrc[(2u * i1) + 1u] = s1;
 433
 434     /* (xa+yb-xc-yd) */
 435     r1 = r2 + t1;
 436     /* (xa-yb-xc+yd) */
 437     r2 = r2 - t1;
 438
 439     /* (ya-xb-yc+xd) */
 440     s1 = s2 - t2;
 441     /* (ya+xb-yc-xd) */
 442     s2 = s2 + t2;
 443
 444     /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */
 445     pSrc[2u * i2] = r1;
 446     /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */
 447     pSrc[(2u * i2) + 1u] = s1;
 448
 449     /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */
 450     pSrc[2u * i3] = r2;
 451     /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */
 452     pSrc[(2u * i3) + 1u] = s2;
 453
 454
 455   }
 456
 457   /* output is in 11.21(q21) format for the 1024 point */
 458   /* output is in 9.23(q23) format for the 256 point */
 459   /* output is in 7.25(q25) format for the 64 point */
 460   /* output is in 5.27(q27) format for the 16 point */
 461
 462   /* End of last stage process */
 463
 464 }
 465
 466
 467 /**
 468  * @brief  Core function for the Q31 CIFFT butterfly process.
 469  * @param[in, out] *pSrc            points to the in-place buffer of Q31 data type.
 470  * @param[in]      fftLen           length of the FFT.
 471  * @param[in]      *pCoef           points to twiddle coefficient buffer.
 472  * @param[in]      twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 473  * @return none.
 474  */
 475
 476
 477 /*
 478 * Radix-4 IFFT algorithm used is :
 479 *
 480 * CIFFT uses same twiddle coefficients as CFFT Function
 481 *  x[k] = x[n] + (j)k * x[n + fftLen/4] + (-1)k * x[n+fftLen/2] + (-j)k * x[n+3*fftLen/4]
 482 *
 483 *
 484 * IFFT is implemented with following changes in equations from FFT
 485 *
 486 * Input real and imaginary data:
 487 * x(n) = xa + j * ya
 488 * x(n+N/4 ) = xb + j * yb
 489 * x(n+N/2 ) = xc + j * yc
 490 * x(n+3N 4) = xd + j * yd
 491 *
 492 *
 493 * Output real and imaginary data:
 494 * x(4r) = xa'+ j * ya'
 495 * x(4r+1) = xb'+ j * yb'
 496 * x(4r+2) = xc'+ j * yc'
 497 * x(4r+3) = xd'+ j * yd'
 498 *
 499 *
 500 * Twiddle factors for radix-4 IFFT:
 501 * Wn = co1 + j * (si1)
 502 * W2n = co2 + j * (si2)
 503 * W3n = co3 + j * (si3)
 504
 505 * The real and imaginary output values for the radix-4 butterfly are
 506 * xa' = xa + xb + xc + xd
 507 * ya' = ya + yb + yc + yd
 508 * xb' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1)
 509 * yb' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1)
 510 * xc' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2)
 511 * yc' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2)
 512 * xd' = (xa+yb-xc-yd)* co3 - (ya-xb-yc+xd)* (si3)
 513 * yd' = (ya-xb-yc+xd)* co3 + (xa+yb-xc-yd)* (si3)
 514 *
 515 */
 516
 517 void arm_radix4_butterfly_inverse_q31(
 518   q31_t * pSrc,
 519   uint32_t fftLen,
 520   q31_t * pCoef,
 521   uint32_t twidCoefModifier)
 522 {
 523   uint32_t n1, n2, ia1, ia2, ia3, i0, i1, i2, i3, j, k;
 524   q31_t t1, t2, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3;
 525
 526   /* input is be 1.31(q31) format for all FFT sizes */
 527   /* Total process is divided into three stages */
 528   /* process first stage, middle stages, & last stage */
 529
 530   /* Start of first stage process */
 531
 532   /* Initializations for the first stage */
 533   n2 = fftLen;
 534   n1 = n2;
 535   /* n2 = fftLen/4 */
 536   n2 >>= 2u;
 537   i0 = 0u;
 538   ia1 = 0u;
 539
 540   j = n2;
 541
 542   do
 543   {
 544
 545     /* input is in 1.31(q31) format and provide 4 guard bits for the input */
 546
 547     /*  index calculation for the input as, */
 548     /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 549     i1 = i0 + n2;
 550     i2 = i1 + n2;
 551     i3 = i2 + n2;
 552
 553     /*  Butterfly implementation */
 554     /* xa + xc */
 555     r1 = (pSrc[2u * i0] >> 4u) + (pSrc[2u * i2] >> 4u);
 556     /* xa - xc */
 557     r2 = (pSrc[2u * i0] >> 4u) - (pSrc[2u * i2] >> 4u);
 558
 559     /* ya + yc */
 560     s1 = (pSrc[(2u * i0) + 1u] >> 4u) + (pSrc[(2u * i2) + 1u] >> 4u);
 561     /* ya - yc */
 562     s2 = (pSrc[(2u * i0) + 1u] >> 4u) - (pSrc[(2u * i2) + 1u] >> 4u);
 563
 564     /* xb + xd */
 565     t1 = (pSrc[2u * i1] >> 4u) + (pSrc[2u * i3] >> 4u);
 566
 567     /* xa' = xa + xb + xc + xd */
 568     pSrc[2u * i0] = (r1 + t1);
 569     /* (xa + xc) - (xb + xd) */
 570     r1 = r1 - t1;
 571     /* yb + yd */
 572     t2 = (pSrc[(2u * i1) + 1u] >> 4u) + (pSrc[(2u * i3) + 1u] >> 4u);
 573     /* ya' = ya + yb + yc + yd */
 574     pSrc[(2u * i0) + 1u] = (s1 + t2);
 575
 576     /* (ya + yc) - (yb + yd) */
 577     s1 = s1 - t2;
 578
 579     /* yb - yd */
 580     t1 = (pSrc[(2u * i1) + 1u] >> 4u) - (pSrc[(2u * i3) + 1u] >> 4u);
 581     /* xb - xd */
 582     t2 = (pSrc[2u * i1] >> 4u) - (pSrc[2u * i3] >> 4u);
 583
 584     /*  index calculation for the coefficients */
 585     ia2 = 2u * ia1;
 586     co2 = pCoef[ia2 * 2u];
 587     si2 = pCoef[(ia2 * 2u) + 1u];
 588
 589     /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */
 590     pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) -
 591                      ((int32_t) (((q63_t) s1 * si2) >> 32))) << 1u;
 592
 593     /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */
 594     pSrc[2u * i1 + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) +
 595                           ((int32_t) (((q63_t) r1 * si2) >> 32))) << 1u;
 596
 597     /* (xa - xc) - (yb - yd) */
 598     r1 = r2 - t1;
 599     /* (xa - xc) + (yb - yd) */
 600     r2 = r2 + t1;
 601
 602     /* (ya - yc) + (xb - xd) */
 603     s1 = s2 + t2;
 604     /* (ya - yc) - (xb - xd) */
 605     s2 = s2 - t2;
 606
 607     co1 = pCoef[ia1 * 2u];
 608     si1 = pCoef[(ia1 * 2u) + 1u];
 609
 610     /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */
 611     pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) -
 612                      ((int32_t) (((q63_t) s1 * si1) >> 32))) << 1u;
 613
 614     /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */
 615     pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) +
 616                             ((int32_t) (((q63_t) r1 * si1) >> 32))) << 1u;
 617
 618     /*  index calculation for the coefficients */
 619     ia3 = 3u * ia1;
 620     co3 = pCoef[ia3 * 2u];
 621     si3 = pCoef[(ia3 * 2u) + 1u];
 622
 623     /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */
 624     pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) -
 625                      ((int32_t) (((q63_t) s2 * si3) >> 32))) << 1u;
 626
 627     /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */
 628     pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) +
 629                             ((int32_t) (((q63_t) r2 * si3) >> 32))) << 1u;
 630
 631     /*  Twiddle coefficients index modifier */
 632     ia1 = ia1 + twidCoefModifier;
 633
 634     /*  Updating input index */
 635     i0 = i0 + 1u;
 636
 637   } while(--j);
 638
 639   /* data is in 5.27(q27) format */
 640   /* each stage provides two down scaling of the input */
 641
 642
 643   /* Start of Middle stages process */
 644
 645   twidCoefModifier <<= 2u;
 646
 647   /*  Calculation of second stage to excluding last stage */
 648   for (k = fftLen / 4u; k > 4u; k >>= 2u)
 649   {
 650     /*  Initializations for the first stage */
 651     n1 = n2;
 652     n2 >>= 2u;
 653     ia1 = 0u;
 654
 655     for (j = 0; j <= (n2 - 1u); j++)
 656     {
 657       /*  index calculation for the coefficients */
 658       ia2 = ia1 + ia1;
 659       ia3 = ia2 + ia1;
 660       co1 = pCoef[ia1 * 2u];
 661       si1 = pCoef[(ia1 * 2u) + 1u];
 662       co2 = pCoef[ia2 * 2u];
 663       si2 = pCoef[(ia2 * 2u) + 1u];
 664       co3 = pCoef[ia3 * 2u];
 665       si3 = pCoef[(ia3 * 2u) + 1u];
 666       /*  Twiddle coefficients index modifier */
 667       ia1 = ia1 + twidCoefModifier;
 668
 669       for (i0 = j; i0 < fftLen; i0 += n1)
 670       {
 671         /*  index calculation for the input as, */
 672         /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 673         i1 = i0 + n2;
 674         i2 = i1 + n2;
 675         i3 = i2 + n2;
 676
 677         /*  Butterfly implementation */
 678         /* xa + xc */
 679         r1 = pSrc[2u * i0] + pSrc[2u * i2];
 680         /* xa - xc */
 681         r2 = pSrc[2u * i0] - pSrc[2u * i2];
 682
 683         /* ya + yc */
 684         s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
 685         /* ya - yc */
 686         s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
 687
 688         /* xb + xd */
 689         t1 = pSrc[2u * i1] + pSrc[2u * i3];
 690
 691         /* xa' = xa + xb + xc + xd */
 692         pSrc[2u * i0] = (r1 + t1) >> 2u;
 693         /* xa + xc -(xb + xd) */
 694         r1 = r1 - t1;
 695         /* yb + yd */
 696         t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
 697         /* ya' = ya + yb + yc + yd */
 698         pSrc[(2u * i0) + 1u] = (s1 + t2) >> 2u;
 699
 700         /* (ya + yc) - (yb + yd) */
 701         s1 = s1 - t2;
 702
 703         /* (yb - yd) */
 704         t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
 705         /* (xb - xd) */
 706         t2 = pSrc[2u * i1] - pSrc[2u * i3];
 707
 708         /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */
 709         pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32u)) -
 710                          ((int32_t) (((q63_t) s1 * si2) >> 32u))) >> 1u;
 711
 712         /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */
 713         pSrc[(2u * i1) + 1u] =
 714           (((int32_t) (((q63_t) s1 * co2) >> 32u)) +
 715            ((int32_t) (((q63_t) r1 * si2) >> 32u))) >> 1u;
 716
 717         /* (xa - xc) - (yb - yd) */
 718         r1 = r2 - t1;
 719         /* (xa - xc) + (yb - yd) */
 720         r2 = r2 + t1;
 721
 722         /* (ya - yc) +  (xb - xd) */
 723         s1 = s2 + t2;
 724         /* (ya - yc) -  (xb - xd) */
 725         s2 = s2 - t2;
 726
 727         /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */
 728         pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) -
 729                          ((int32_t) (((q63_t) s1 * si1) >> 32))) >> 1u;
 730
 731         /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */
 732         pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) +
 733                                 ((int32_t) (((q63_t) r1 * si1) >> 32))) >> 1u;
 734
 735         /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */
 736         pSrc[(2u * i3)] = (((int32_t) (((q63_t) r2 * co3) >> 32)) -
 737                            ((int32_t) (((q63_t) s2 * si3) >> 32))) >> 1u;
 738
 739         /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */
 740         pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) +
 741                                 ((int32_t) (((q63_t) r2 * si3) >> 32))) >> 1u;
 742       }
 743     }
 744     twidCoefModifier <<= 2u;
 745   }
 746
 747   /* End of Middle stages process */
 748
 749   /* data is in 11.21(q21) format for the 1024 point as there are 3 middle stages */
 750   /* data is in 9.23(q23) format for the 256 point as there are 2 middle stages */
 751   /* data is in 7.25(q25) format for the 64 point as there are 1 middle stage */
 752   /* data is in 5.27(q27) format for the 16 point as there are no middle stages */
 753
 754
 755   /* Start of last stage process */
 756
 757
 758   /*  Initializations of last stage */
 759   n1 = n2;
 760   n2 >>= 2u;
 761
 762   /*  Calculations of last stage */
 763   for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1)
 764   {
 765     /*  index calculation for the input as, */
 766     /*  pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */
 767     i1 = i0 + n2;
 768     i2 = i1 + n2;
 769     i3 = i2 + n2;
 770
 771     /*  Butterfly implementation */
 772     /* xa + xc */
 773     r1 = pSrc[2u * i0] + pSrc[2u * i2];
 774     /* xa - xc */
 775     r2 = pSrc[2u * i0] - pSrc[2u * i2];
 776
 777     /* ya + yc */
 778     s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
 779     /* ya - yc */
 780     s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
 781
 782     /* xc + xd */
 783     t1 = pSrc[2u * i1] + pSrc[2u * i3];
 784     /* xa' = xa + xb + xc + xd */
 785     pSrc[2u * i0] = (r1 + t1);
 786     /* (xa + xb) - (xc + xd) */
 787     r1 = r1 - t1;
 788
 789     /* yb + yd */
 790     t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
 791     /* ya' = ya + yb + yc + yd */
 792     pSrc[(2u * i0) + 1u] = (s1 + t2);
 793     /* (ya + yc) - (yb + yd) */
 794     s1 = s1 - t2;
 795
 796     /* (yb-yd) */
 797     t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
 798     /* (xb-xd) */
 799     t2 = pSrc[2u * i1] - pSrc[2u * i3];
 800
 801     /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */
 802     pSrc[2u * i1] = r1;
 803     /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */
 804     pSrc[(2u * i1) + 1u] = s1;
 805
 806     /* (xa - xc) - (yb-yd) */
 807     r1 = r2 - t1;
 808
 809     /* (xa - xc) + (yb-yd) */
 810     r2 = r2 + t1;
 811
 812     /* (ya - yc) + (xb-xd) */
 813     s1 = s2 + t2;
 814
 815     /* (ya - yc) - (xb-xd) */
 816     s2 = s2 - t2;
 817
 818     /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */
 819     pSrc[2u * i2] = r1;
 820     /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */
 821     pSrc[(2u * i2) + 1u] = s1;
 822
 823     /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */
 824     pSrc[2u * i3] = r2;
 825     /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */
 826     pSrc[(2u * i3) + 1u] = s2;
 827
 828   }
 829
 830   /* output is in 11.21(q21) format for the 1024 point */
 831   /* output is in 9.23(q23) format for the 256 point */
 832   /* output is in 7.25(q25) format for the 64 point */
 833   /* output is in 5.27(q27) format for the 16 point */
 834
 835   /* End of last stage process */
 836 }
 837
 838
 839 /*
 840  * @brief  In-place bit reversal function.
 841  * @param[in, out] *pSrc        points to the in-place buffer of Q31 data type.
 842  * @param[in]      fftLen       length of the FFT.
 843  * @param[in]      bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table
 844  * @param[in]      *pBitRevTab  points to bit reversal table.
 845  * @return none.
 846  */
 847
 848 void arm_bitreversal_q31(
 849   q31_t * pSrc,
 850   uint32_t fftLen,
 851   uint16_t bitRevFactor,
 852   uint16_t * pBitRevTable)
 853 {
 854   uint32_t fftLenBy2, fftLenBy2p1, i, j;
 855   q31_t in;
 856
 857   /*  Initializations      */
 858   j = 0u;
 859   fftLenBy2 = fftLen / 2u;
 860   fftLenBy2p1 = (fftLen / 2u) + 1u;
 861
 862   /* Bit Reversal Implementation */
 863   for (i = 0u; i <= (fftLenBy2 - 2u); i += 2u)
 864   {
 865     if(i < j)
 866     {
 867       /*  pSrc[i] <-> pSrc[j]; */
 868       in = pSrc[2u * i];
 869       pSrc[2u * i] = pSrc[2u * j];
 870       pSrc[2u * j] = in;
 871
 872       /*  pSrc[i+1u] <-> pSrc[j+1u] */
 873       in = pSrc[(2u * i) + 1u];
 874       pSrc[(2u * i) + 1u] = pSrc[(2u * j) + 1u];
 875       pSrc[(2u * j) + 1u] = in;
 876
 877       /*  pSrc[i+fftLenBy2p1] <-> pSrc[j+fftLenBy2p1] */
 878       in = pSrc[2u * (i + fftLenBy2p1)];
 879       pSrc[2u * (i + fftLenBy2p1)] = pSrc[2u * (j + fftLenBy2p1)];
 880       pSrc[2u * (j + fftLenBy2p1)] = in;
 881
 882       /*  pSrc[i+fftLenBy2p1+1u] <-> pSrc[j+fftLenBy2p1+1u] */
 883       in = pSrc[(2u * (i + fftLenBy2p1)) + 1u];
 884       pSrc[(2u * (i + fftLenBy2p1)) + 1u] =
 885         pSrc[(2u * (j + fftLenBy2p1)) + 1u];
 886       pSrc[(2u * (j + fftLenBy2p1)) + 1u] = in;
 887
 888     }
 889
 890     /*  pSrc[i+1u] <-> pSrc[j+1u] */
 891     in = pSrc[2u * (i + 1u)];
 892     pSrc[2u * (i + 1u)] = pSrc[2u * (j + fftLenBy2)];
 893     pSrc[2u * (j + fftLenBy2)] = in;
 894
 895     /*  pSrc[i+2u] <-> pSrc[j+2u] */
 896     in = pSrc[(2u * (i + 1u)) + 1u];
 897     pSrc[(2u * (i + 1u)) + 1u] = pSrc[(2u * (j + fftLenBy2)) + 1u];
 898     pSrc[(2u * (j + fftLenBy2)) + 1u] = in;
 899
 900     /*  Reading the index for the bit reversal */
 901     j = *pBitRevTable;
 902
 903     /*  Updating the bit reversal index depending on the fft length */
 904     pBitRevTable += bitRevFactor;
 905   }
 906 }