git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_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_rfft_q31.c
   9 *
  10 * Description:  RFFT & RIFFT Q31 process function
  11 *
  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.7  2010/06/10
  31 *    Misra-C changes done
  32 * -------------------------------------------------------------------- */
  33
  34 #include "arm_math.h"
  35
  36 /*--------------------------------------------------------------------
  37 *               Internal functions prototypes
  38 --------------------------------------------------------------------*/
  39
  40 void arm_split_rfft_q31(
  41   q31_t * pSrc,
  42   uint32_t fftLen,
  43   q31_t * pATable,
  44   q31_t * pBTable,
  45   q31_t * pDst,
  46   uint32_t modifier);
  47
  48 void arm_split_rifft_q31(
  49   q31_t * pSrc,
  50   uint32_t fftLen,
  51   q31_t * pATable,
  52   q31_t * pBTable,
  53   q31_t * pDst,
  54   uint32_t modifier);
  55
  56 /**
  57  * @addtogroup RFFT_RIFFT
  58  * @{
  59  */
  60
  61 /**
  62  * @brief Processing function for the Q31 RFFT/RIFFT.
  63  * @param[in]  *S    points to an instance of the Q31 RFFT/RIFFT structure.
  64  * @param[in]  *pSrc points to the input buffer.
  65  * @param[out] *pDst points to the output buffer.
  66  * @return none.
  67  *
  68  * \par Input an output formats:
  69  * \par
  70  * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
  71  * Hence the output format is different for different RFFT sizes.
  72  * 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:
  73  * \par
  74  * \image html RFFTQ31.gif "Input and Output Formats for Q31 RFFT"
  75  *
  76  * \par
  77  * \image html RIFFTQ31.gif "Input and Output Formats for Q31 RIFFT"
  78  */
  79
  80 void arm_rfft_q31(
  81   const arm_rfft_instance_q31 * S,
  82   q31_t * pSrc,
  83   q31_t * pDst)
  84 {
  85   const arm_cfft_radix4_instance_q31 *S_CFFT = S->pCfft;
  86
  87   /* Calculation of RIFFT of input */
  88   if(S->ifftFlagR == 1u)
  89   {
  90     /*  Real IFFT core process */
  91     arm_split_rifft_q31(pSrc, S->fftLenBy2, S->pTwiddleAReal,
  92                         S->pTwiddleBReal, pDst, S->twidCoefRModifier);
  93
  94     /* Complex readix-4 IFFT process */
  95     arm_radix4_butterfly_inverse_q31(pDst, S_CFFT->fftLen,
  96                                      S_CFFT->pTwiddle,
  97                                      S_CFFT->twidCoefModifier);
  98     /* Bit reversal process */
  99     if(S->bitReverseFlagR == 1u)
 100     {
 101       arm_bitreversal_q31(pDst, S_CFFT->fftLen,
 102                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 103     }
 104   }
 105   else
 106   {
 107     /* Calculation of RFFT of input */
 108
 109     /* Complex readix-4 FFT process */
 110     arm_radix4_butterfly_q31(pSrc, S_CFFT->fftLen,
 111                              S_CFFT->pTwiddle, S_CFFT->twidCoefModifier);
 112
 113     /* Bit reversal process */
 114     if(S->bitReverseFlagR == 1u)
 115     {
 116       arm_bitreversal_q31(pSrc, S_CFFT->fftLen,
 117                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 118     }
 119
 120     /*  Real FFT core process */
 121     arm_split_rfft_q31(pSrc, S->fftLenBy2, S->pTwiddleAReal,
 122                        S->pTwiddleBReal, pDst, S->twidCoefRModifier);
 123   }
 124
 125 }
 126
 127
 128   /**
 129    * @} end of RFFT_RIFFT group
 130    */
 131
 132 /**
 133  * @brief  Core Real FFT process
 134  * @param[in]   *pSrc                           points to the input buffer.
 135  * @param[in]   fftLen                          length of FFT.
 136  * @param[in]   *pATable                        points to the twiddle Coef A buffer.
 137  * @param[in]   *pBTable                        points to the twiddle Coef B buffer.
 138  * @param[out]  *pDst                           points to the output buffer.
 139  * @param[in]   modifier                twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 140  * @return none.
 141  */
 142
 143 void arm_split_rfft_q31(
 144   q31_t * pSrc,
 145   uint32_t fftLen,
 146   q31_t * pATable,
 147   q31_t * pBTable,
 148   q31_t * pDst,
 149   uint32_t modifier)
 150 {
 151   uint32_t i;                                    /* Loop Counter */
 152   q31_t outR, outI;                              /* Temporary variables for output */
 153   q31_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 154   q31_t CoefA1, CoefA2, CoefB1;                  /* Temporary variables for twiddle coefficients */
 155   q31_t *pOut1 = &pDst[2], *pOut2 = &pDst[(4u * fftLen) - 1u];
 156   q31_t *pIn1 = &pSrc[2], *pIn2 = &pSrc[(2u * fftLen) - 1u];
 157
 158   pSrc[2u * fftLen] = pSrc[0];
 159   pSrc[(2u * fftLen) + 1u] = pSrc[1];
 160
 161   /* Init coefficient pointers */
 162   pCoefA = &pATable[modifier * 2u];
 163   pCoefB = &pBTable[modifier * 2u];
 164
 165   i = fftLen - 1u;
 166
 167   while(i > 0u)
 168   {
 169     /*
 170        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
 171        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
 172        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 173      */
 174
 175     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
 176        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 177        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
 178
 179     CoefA1 = *pCoefA++;
 180     CoefA2 = *pCoefA;
 181
 182     /* outR = (pSrc[2 * i] * pATable[2 * i] */
 183     outR = ((int32_t) (((q63_t) * pIn1 * CoefA1) >> 32));
 184
 185     /* outI = pIn[2 * i] * pATable[2 * i + 1] */
 186     outI = ((int32_t) (((q63_t) * pIn1++ * CoefA2) >> 32));
 187
 188     /* - pSrc[2 * i + 1] * pATable[2 * i + 1] */
 189     outR =
 190       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn1 * (-CoefA2))) >> 32);
 191
 192     /* (pIn[2 * i + 1] * pATable[2 * i] */
 193     outI =
 194       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn1++ * (CoefA1))) >> 32);
 195
 196     /* pSrc[2 * n - 2 * i] * pBTable[2 * i]  */
 197     outR =
 198       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (-CoefA2))) >> 32);
 199     CoefB1 = *pCoefB;
 200
 201     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] */
 202     outI =
 203       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (-CoefB1))) >> 32);
 204
 205     /* pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1] */
 206     outR =
 207       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefB1))) >> 32);
 208
 209     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 210     outI =
 211       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (-CoefA2))) >> 32);
 212
 213     /* write output */
 214     *pOut1++ = (outR << 1u);
 215     *pOut1++ = (outI << 1u);
 216
 217     /* write complex conjugate output */
 218     *pOut2-- = -(outI << 1u);
 219     *pOut2-- = (outR << 1u);
 220
 221     /* update coefficient pointer */
 222     pCoefB = pCoefB + (modifier * 2u);
 223     pCoefA = pCoefA + ((modifier * 2u) - 1u);
 224
 225     i--;
 226
 227   }
 228
 229   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
 230   pDst[(2u * fftLen) + 1u] = 0;
 231
 232   pDst[0] = pSrc[0] + pSrc[1];
 233   pDst[1] = 0;
 234
 235 }
 236
 237
 238 /**
 239  * @brief  Core Real IFFT process
 240  * @param[in]   *pSrc                           points to the input buffer.
 241  * @param[in]   fftLen                          length of FFT.
 242  * @param[in]   *pATable                        points to the twiddle Coef A buffer.
 243  * @param[in]   *pBTable                        points to the twiddle Coef B buffer.
 244  * @param[out]  *pDst                           points to the output buffer.
 245  * @param[in]   modifier                twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 246  * @return none.
 247  */
 248
 249 void arm_split_rifft_q31(
 250   q31_t * pSrc,
 251   uint32_t fftLen,
 252   q31_t * pATable,
 253   q31_t * pBTable,
 254   q31_t * pDst,
 255   uint32_t modifier)
 256 {
 257   q31_t outR, outI;                              /* Temporary variables for output */
 258   q31_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 259   q31_t CoefA1, CoefA2, CoefB1;                  /* Temporary variables for twiddle coefficients */
 260   q31_t *pIn1 = &pSrc[0], *pIn2 = &pSrc[(2u * fftLen) + 1u];
 261
 262   pCoefA = &pATable[0];
 263   pCoefB = &pBTable[0];
 264
 265   while(fftLen > 0u)
 266   {
 267     /*
 268        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
 269        pIn[2 * n - 2 * i] * pBTable[2 * i] -
 270        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 271
 272        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
 273        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 274        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
 275
 276      */
 277     CoefA1 = *pCoefA++;
 278     CoefA2 = *pCoefA;
 279
 280     /* outR = (pIn[2 * i] * pATable[2 * i] */
 281     outR = ((int32_t) (((q63_t) * pIn1 * CoefA1) >> 32));
 282
 283     /* - pIn[2 * i] * pATable[2 * i + 1] */
 284     outI = -((int32_t) (((q63_t) * pIn1++ * CoefA2) >> 32));
 285
 286     /* pIn[2 * i + 1] * pATable[2 * i + 1] */
 287     outR =
 288       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn1 * (CoefA2))) >> 32);
 289
 290     /* pIn[2 * i + 1] * pATable[2 * i] */
 291     outI =
 292       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn1++ * (CoefA1))) >> 32);
 293
 294     /* pIn[2 * n - 2 * i] * pBTable[2 * i] */
 295     outR =
 296       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefA2))) >> 32);
 297
 298     CoefB1 = *pCoefB;
 299
 300     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] */
 301     outI =
 302       (q31_t) ((((q63_t) outI << 32) - ((q63_t) * pIn2-- * (CoefB1))) >> 32);
 303
 304     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1] */
 305     outR =
 306       (q31_t) ((((q63_t) outR << 32) + ((q63_t) * pIn2 * (CoefB1))) >> 32);
 307
 308     /* pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 309     outI =
 310       (q31_t) ((((q63_t) outI << 32) + ((q63_t) * pIn2-- * (CoefA2))) >> 32);
 311
 312     /* write output */
 313     *pDst++ = (outR << 1u);
 314     *pDst++ = (outI << 1u);
 315
 316     /* update coefficient pointer */
 317     pCoefB = pCoefB + (modifier * 2u);
 318     pCoefA = pCoefA + ((modifier * 2u) - 1u);
 319
 320     /* Decrement loop count */
 321     fftLen--;
 322
 323   }
 324
 325
 326 }