tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/TransformFunctions/arm_rfft_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:            arm_rfft_q31.c
   9 *
  10 * Description:  RFFT & RIFFT Q31 process function
  11 *
  12 *
  13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  14 *
  15 * Redistribution and use in source and binary forms, with or without
  16 * modification, are permitted provided that the following conditions
  17 * are met:
  18 *   - Redistributions of source code must retain the above copyright
  19 *     notice, this list of conditions and the following disclaimer.
  20 *   - Redistributions in binary form must reproduce the above copyright
  21 *     notice, this list of conditions and the following disclaimer in
  22 *     the documentation and/or other materials provided with the
  23 *     distribution.
  24 *   - Neither the name of ARM LIMITED nor the names of its contributors
  25 *     may be used to endorse or promote products derived from this
  26 *     software without specific prior written permission.
  27 *
  28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39 * POSSIBILITY OF SUCH DAMAGE.
  40 * -------------------------------------------------------------------- */
  41
  42 #include "arm_math.h"
  43
  44 void arm_radix4_butterfly_inverse_q31(
  45 q31_t * pSrc,
  46 uint32_t fftLen,
  47 q31_t * pCoef,
  48 uint32_t twidCoefModifier);
  49
  50 void arm_radix4_butterfly_q31(
  51 q31_t * pSrc,
  52 uint32_t fftLen,
  53 q31_t * pCoef,
  54 uint32_t twidCoefModifier);
  55
  56 void arm_bitreversal_q31(
  57 q31_t * pSrc,
  58 uint32_t fftLen,
  59 uint16_t bitRevFactor,
  60 uint16_t * pBitRevTab);
  61
  62 /*--------------------------------------------------------------------
  63 *               Internal functions prototypes
  64 --------------------------------------------------------------------*/
  65
  66 void arm_split_rfft_q31(
  67   q31_t * pSrc,
  68   uint32_t fftLen,
  69   q31_t * pATable,
  70   q31_t * pBTable,
  71   q31_t * pDst,
  72   uint32_t modifier);
  73
  74 void arm_split_rifft_q31(
  75   q31_t * pSrc,
  76   uint32_t fftLen,
  77   q31_t * pATable,
  78   q31_t * pBTable,
  79   q31_t * pDst,
  80   uint32_t modifier);
  81
  82 /**
  83  * @addtogroup RealFFT
  84  * @{
  85  */
  86
  87 /**
  88  * @brief Processing function for the Q31 RFFT/RIFFT.
  89  * @param[in]  *S    points to an instance of the Q31 RFFT/RIFFT structure.
  90  * @param[in]  *pSrc points to the input buffer.
  91  * @param[out] *pDst points to the output buffer.
  92  * @return none.
  93  *
  94  * \par Input an output formats:
  95  * \par
  96  * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
  97  * Hence the output format is different for different RFFT sizes.
  98  * The input and output formats for different RFFT sizes and number of bits to upscale are mentioned in the tables below for RFFT and RIFFT:
  99  * \par
 100  * \image html RFFTQ31.gif "Input and Output Formats for Q31 RFFT"
 101  *
 102  * \par
 103  * \image html RIFFTQ31.gif "Input and Output Formats for Q31 RIFFT"
 104  */
 105
 106 void arm_rfft_q31(
 107   const arm_rfft_instance_q31 * S,
 108   q31_t * pSrc,
 109   q31_t * pDst)
 110 {
 111   const arm_cfft_radix4_instance_q31 *S_CFFT = S->pCfft;
 112
 113   /* Calculation of RIFFT of input */
 114   if(S->ifftFlagR == 1u)
 115   {
 116     /*  Real IFFT core process */
 117     arm_split_rifft_q31(pSrc, S->fftLenBy2, S->pTwiddleAReal,
 118                         S->pTwiddleBReal, pDst, S->twidCoefRModifier);
 119
 120     /* Complex readix-4 IFFT process */
 121     arm_radix4_butterfly_inverse_q31(pDst, S_CFFT->fftLen,
 122                                      S_CFFT->pTwiddle,
 123                                      S_CFFT->twidCoefModifier);
 124     /* Bit reversal process */
 125     if(S->bitReverseFlagR == 1u)
 126     {
 127       arm_bitreversal_q31(pDst, S_CFFT->fftLen,
 128                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 129     }
 130   }
 131   else
 132   {
 133     /* Calculation of RFFT of input */
 134
 135     /* Complex readix-4 FFT process */
 136     arm_radix4_butterfly_q31(pSrc, S_CFFT->fftLen,
 137                              S_CFFT->pTwiddle, S_CFFT->twidCoefModifier);
 138
 139     /* Bit reversal process */
 140     if(S->bitReverseFlagR == 1u)
 141     {
 142       arm_bitreversal_q31(pSrc, S_CFFT->fftLen,
 143                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 144     }
 145
 146     /*  Real FFT core process */
 147     arm_split_rfft_q31(pSrc, S->fftLenBy2, S->pTwiddleAReal,
 148                        S->pTwiddleBReal, pDst, S->twidCoefRModifier);
 149   }
 150
 151 }
 152
 153
 154   /**
 155    * @} end of RealFFT group
 156    */
 157
 158 /**
 159  * @brief  Core Real FFT process
 160  * @param[in]   *pSrc                           points to the input buffer.
 161  * @param[in]   fftLen                          length of FFT.
 162  * @param[in]   *pATable                        points to the twiddle Coef A buffer.
 163  * @param[in]   *pBTable                        points to the twiddle Coef B buffer.
 164  * @param[out]  *pDst                           points to the output buffer.
 165  * @param[in]   modifier                twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 166  * @return none.
 167  */
 168
 169 void arm_split_rfft_q31(
 170   q31_t * pSrc,
 171   uint32_t fftLen,
 172   q31_t * pATable,
 173   q31_t * pBTable,
 174   q31_t * pDst,
 175   uint32_t modifier)
 176 {
 177   uint32_t i;                                    /* Loop Counter */
 178   q31_t outR, outI;                              /* Temporary variables for output */
 179   q31_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 180   q31_t CoefA1, CoefA2, CoefB1;                  /* Temporary variables for twiddle coefficients */
 181   q31_t *pOut1 = &pDst[2], *pOut2 = &pDst[(4u * fftLen) - 1u];
 182   q31_t *pIn1 = &pSrc[2], *pIn2 = &pSrc[(2u * fftLen) - 1u];
 183
 184   /* Init coefficient pointers */
 185   pCoefA = &pATable[modifier * 2u];
 186   pCoefB = &pBTable[modifier * 2u];
 187
 188   i = fftLen - 1u;
 189
 190   while(i > 0u)
 191   {
 192     /*
 193        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
 194        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
 195        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 196      */
 197
 198     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
 199        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 200        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
 201
 202     CoefA1 = *pCoefA++;
 203     CoefA2 = *pCoefA;
 204
 205     /* outR = (pSrc[2 * i] * pATable[2 * i] */
 206     outR = ((int32_t) (((q63_t) * pIn1 * CoefA1) >> 32));
 207
 208     /* outI = pIn[2 * i] * pATable[2 * i + 1] */
 209     outI = ((int32_t) (((q63_t) * pIn1++ * CoefA2) >> 32));
 210
 211     /* - pSrc[2 * i + 1] * pATable[2 * i + 1] */
 212     outR =
 213       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn1 * (-CoefA2))) >> 32);
 214
 215     /* (pIn[2 * i + 1] * pATable[2 * i] */
 216     outI =
 217       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn1++ * (CoefA1))) >> 32);
 218
 219     /* pSrc[2 * n - 2 * i] * pBTable[2 * i]  */
 220     outR =
 221       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (-CoefA2))) >> 32);
 222     CoefB1 = *pCoefB;
 223
 224     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] */
 225     outI =
 226       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (-CoefB1))) >> 32);
 227
 228     /* pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1] */
 229     outR =
 230       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefB1))) >> 32);
 231
 232     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 233     outI =
 234       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (-CoefA2))) >> 32);
 235
 236     /* write output */
 237     *pOut1++ = (outR << 1u);
 238     *pOut1++ = (outI << 1u);
 239
 240     /* write complex conjugate output */
 241     *pOut2-- = -(outI << 1u);
 242     *pOut2-- = (outR << 1u);
 243
 244     /* update coefficient pointer */
 245     pCoefB = pCoefB + (modifier * 2u);
 246     pCoefA = pCoefA + ((modifier * 2u) - 1u);
 247
 248     i--;
 249
 250   }
 251
 252   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
 253   pDst[(2u * fftLen) + 1u] = 0;
 254
 255   pDst[0] = pSrc[0] + pSrc[1];
 256   pDst[1] = 0;
 257
 258 }
 259
 260
 261 /**
 262  * @brief  Core Real IFFT process
 263  * @param[in]   *pSrc                           points to the input buffer.
 264  * @param[in]   fftLen                          length of FFT.
 265  * @param[in]   *pATable                        points to the twiddle Coef A buffer.
 266  * @param[in]   *pBTable                        points to the twiddle Coef B buffer.
 267  * @param[out]  *pDst                           points to the output buffer.
 268  * @param[in]   modifier                twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 269  * @return none.
 270  */
 271
 272 void arm_split_rifft_q31(
 273   q31_t * pSrc,
 274   uint32_t fftLen,
 275   q31_t * pATable,
 276   q31_t * pBTable,
 277   q31_t * pDst,
 278   uint32_t modifier)
 279 {
 280   q31_t outR, outI;                              /* Temporary variables for output */
 281   q31_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 282   q31_t CoefA1, CoefA2, CoefB1;                  /* Temporary variables for twiddle coefficients */
 283   q31_t *pIn1 = &pSrc[0], *pIn2 = &pSrc[(2u * fftLen) + 1u];
 284
 285   pCoefA = &pATable[0];
 286   pCoefB = &pBTable[0];
 287
 288   while(fftLen > 0u)
 289   {
 290     /*
 291        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
 292        pIn[2 * n - 2 * i] * pBTable[2 * i] -
 293        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 294
 295        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
 296        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 297        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
 298
 299      */
 300     CoefA1 = *pCoefA++;
 301     CoefA2 = *pCoefA;
 302
 303     /* outR = (pIn[2 * i] * pATable[2 * i] */
 304     outR = ((int32_t) (((q63_t) * pIn1 * CoefA1) >> 32));
 305
 306     /* - pIn[2 * i] * pATable[2 * i + 1] */
 307     outI = -((int32_t) (((q63_t) * pIn1++ * CoefA2) >> 32));
 308
 309     /* pIn[2 * i + 1] * pATable[2 * i + 1] */
 310     outR =
 311       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn1 * (CoefA2))) >> 32);
 312
 313     /* pIn[2 * i + 1] * pATable[2 * i] */
 314     outI =
 315       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn1++ * (CoefA1))) >> 32);
 316
 317     /* pIn[2 * n - 2 * i] * pBTable[2 * i] */
 318     outR =
 319       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefA2))) >> 32);
 320
 321     CoefB1 = *pCoefB;
 322
 323     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] */
 324     outI =
 325       (q31_t) ((((q63_t) outI << 32) - ((q63_t) * pIn2-- * (CoefB1))) >> 32);
 326
 327     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1] */
 328     outR =
 329       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefB1))) >> 32);
 330
 331     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 332     outI =
 333       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (CoefA2))) >> 32);
 334
 335     /* write output */
 336     *pDst++ = (outR << 1u);
 337     *pDst++ = (outI << 1u);
 338
 339     /* update coefficient pointer */
 340     pCoefB = pCoefB + (modifier * 2u);
 341     pCoefA = pCoefA + ((modifier * 2u) - 1u);
 342
 343     /* Decrement loop count */
 344     fftLen--;
 345
 346   }
 347
 348
 349 }