git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.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_q15.c
   9 *
  10 * Description:  RFFT & RIFFT Q15 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
  35 #include "arm_math.h"
  36
  37 /*--------------------------------------------------------------------
  38 *               Internal functions prototypes
  39 --------------------------------------------------------------------*/
  40
  41 void arm_split_rfft_q15(
  42   q15_t * pSrc,
  43   uint32_t fftLen,
  44   q15_t * pATable,
  45   q15_t * pBTable,
  46   q15_t * pDst,
  47   uint32_t modifier);
  48
  49 void arm_split_rifft_q15(
  50   q15_t * pSrc,
  51   uint32_t fftLen,
  52   q15_t * pATable,
  53   q15_t * pBTable,
  54   q15_t * pDst,
  55   uint32_t modifier);
  56
  57 /**
  58  * @addtogroup RFFT_RIFFT
  59  * @{
  60  */
  61
  62 /**
  63  * @brief Processing function for the Q15 RFFT/RIFFT.
  64  * @param[in]  *S    points to an instance of the Q15 RFFT/RIFFT structure.
  65  * @param[in]  *pSrc points to the input buffer.
  66  * @param[out] *pDst points to the output buffer.
  67  * @return none.
  68  *
  69  * \par Input an output formats:
  70  * \par
  71  * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
  72  * Hence the output format is different for different RFFT sizes.
  73  * 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:
  74  * \par
  75  * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"
  76  * \par
  77  * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"
  78  */
  79
  80 void arm_rfft_q15(
  81   const arm_rfft_instance_q15 * S,
  82   q15_t * pSrc,
  83   q15_t * pDst)
  84 {
  85   const arm_cfft_radix4_instance_q15 *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_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
  92                         S->pTwiddleBReal, pDst, S->twidCoefRModifier);
  93
  94     /* Complex readix-4 IFFT process */
  95     arm_radix4_butterfly_inverse_q15(pDst, S_CFFT->fftLen,
  96                                      S_CFFT->pTwiddle,
  97                                      S_CFFT->twidCoefModifier);
  98
  99     /* Bit reversal process */
 100     if(S->bitReverseFlagR == 1u)
 101     {
 102       arm_bitreversal_q15(pDst, S_CFFT->fftLen,
 103                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 104     }
 105   }
 106   else
 107   {
 108     /* Calculation of RFFT of input */
 109
 110     /* Complex readix-4 FFT process */
 111     arm_radix4_butterfly_q15(pSrc, S_CFFT->fftLen,
 112                              S_CFFT->pTwiddle, S_CFFT->twidCoefModifier);
 113
 114     /* Bit reversal process */
 115     if(S->bitReverseFlagR == 1u)
 116     {
 117       arm_bitreversal_q15(pSrc, S_CFFT->fftLen,
 118                           S_CFFT->bitRevFactor, S_CFFT->pBitRevTable);
 119     }
 120
 121     arm_split_rfft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal,
 122                        S->pTwiddleBReal, pDst, S->twidCoefRModifier);
 123   }
 124
 125 }
 126
 127   /**
 128    * @} end of RFFT_RIFFT group
 129    */
 130
 131 /**
 132  * @brief  Core Real FFT process
 133  * @param  *pSrc                                points to the input buffer.
 134  * @param  fftLen                               length of FFT.
 135  * @param  *pATable                     points to the A twiddle Coef buffer.
 136  * @param  *pBTable                     points to the B twiddle Coef buffer.
 137  * @param  *pDst                                points to the output buffer.
 138  * @param  modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 139  * @return none.
 140  * The function implements a Real FFT
 141  */
 142
 143 void arm_split_rfft_q15(
 144   q15_t * pSrc,
 145   uint32_t fftLen,
 146   q15_t * pATable,
 147   q15_t * pBTable,
 148   q15_t * pDst,
 149   uint32_t modifier)
 150 {
 151   uint32_t i;                                    /* Loop Counter */
 152   q31_t outR, outI;                              /* Temporary variables for output */
 153   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 154   q15_t *pSrc1, *pSrc2;
 155
 156
 157   pSrc[2u * fftLen] = pSrc[0];
 158   pSrc[(2u * fftLen) + 1u] = pSrc[1];
 159
 160   pCoefA = &pATable[modifier * 2u];
 161   pCoefB = &pBTable[modifier * 2u];
 162
 163   pSrc1 = &pSrc[2];
 164   pSrc2 = &pSrc[(2u * fftLen) - 2u];
 165
 166 #ifndef ARM_MATH_CM0
 167
 168   /* Run the below code for Cortex-M4 and Cortex-M3 */
 169
 170   i = 1u;
 171
 172   while(i < fftLen)
 173   {
 174     /*
 175        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
 176        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
 177        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 178      */
 179
 180     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
 181        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 182        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
 183
 184
 185 #ifndef ARM_MATH_BIG_ENDIAN
 186
 187     /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
 188     outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
 189
 190 #else
 191
 192     /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
 193     outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
 194
 195 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
 196
 197     /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +
 198        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
 199     outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 15u;
 200
 201     /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 202        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 203
 204 #ifndef ARM_MATH_BIG_ENDIAN
 205
 206     outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
 207
 208 #else
 209
 210     outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
 211
 212 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
 213
 214     /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
 215     outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
 216
 217     /* write output */
 218     pDst[2u * i] = (q15_t) outR;
 219     pDst[(2u * i) + 1u] = outI >> 15u;
 220
 221     /* write complex conjugate output */
 222     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
 223     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u);
 224
 225     /* update coefficient pointer */
 226     pCoefB = pCoefB + (2u * modifier);
 227     pCoefA = pCoefA + (2u * modifier);
 228
 229     i++;
 230
 231   }
 232
 233   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
 234   pDst[(2u * fftLen) + 1u] = 0;
 235
 236   pDst[0] = pSrc[0] + pSrc[1];
 237   pDst[1] = 0;
 238
 239
 240 #else
 241
 242   /* Run the below code for Cortex-M0 */
 243
 244   i = 1u;
 245
 246   while(i < fftLen)
 247   {
 248     /*
 249        outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
 250        + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
 251        pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 252      */
 253
 254     outR = *pSrc1 * *pCoefA;
 255     outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
 256     outR = outR + (*pSrc2 * *pCoefB);
 257     outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 15;
 258
 259
 260     /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
 261        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 262        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
 263      */
 264
 265     outI = *pSrc2 * *(pCoefB + 1);
 266     outI = outI - (*(pSrc2 + 1) * *pCoefB);
 267     outI = outI + (*(pSrc1 + 1) * *pCoefA);
 268     outI = outI + (*pSrc1 * *(pCoefA + 1));
 269
 270     /* update input pointers */
 271     pSrc1 += 2u;
 272     pSrc2 -= 2u;
 273
 274     /* write output */
 275     pDst[2u * i] = (q15_t) outR;
 276     pDst[(2u * i) + 1u] = outI >> 15u;
 277
 278     /* write complex conjugate output */
 279     pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
 280     pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u);
 281
 282     /* update coefficient pointer */
 283     pCoefB = pCoefB + (2u * modifier);
 284     pCoefA = pCoefA + (2u * modifier);
 285
 286     i++;
 287
 288   }
 289
 290   pDst[2u * fftLen] = pSrc[0] - pSrc[1];
 291   pDst[(2u * fftLen) + 1u] = 0;
 292
 293   pDst[0] = pSrc[0] + pSrc[1];
 294   pDst[1] = 0;
 295
 296 #endif /* #ifndef ARM_MATH_CM0 */
 297
 298 }
 299
 300
 301 /**
 302  * @brief  Core Real IFFT process
 303  * @param[in]   *pSrc                           points to the input buffer.
 304  * @param[in]   fftLen                      length of FFT.
 305  * @param[in]   *pATable                        points to the twiddle Coef A buffer.
 306  * @param[in]   *pBTable                        points to the twiddle Coef B buffer.
 307  * @param[out]  *pDst                           points to the output buffer.
 308  * @param[in]   modifier                twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
 309  * @return none.
 310  * The function implements a Real IFFT
 311  */
 312 void arm_split_rifft_q15(
 313   q15_t * pSrc,
 314   uint32_t fftLen,
 315   q15_t * pATable,
 316   q15_t * pBTable,
 317   q15_t * pDst,
 318   uint32_t modifier)
 319 {
 320   uint32_t i;                                    /* Loop Counter */
 321   q31_t outR, outI;                              /* Temporary variables for output */
 322   q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
 323   q15_t *pSrc1, *pSrc2;
 324   q15_t *pDst1 = &pDst[0];
 325
 326   pCoefA = &pATable[0];
 327   pCoefB = &pBTable[0];
 328
 329   pSrc1 = &pSrc[0];
 330   pSrc2 = &pSrc[2u * fftLen];
 331
 332 #ifndef ARM_MATH_CM0
 333
 334   /* Run the below code for Cortex-M4 and Cortex-M3 */
 335
 336   i = fftLen;
 337
 338   while(i > 0u)
 339   {
 340
 341     /*
 342        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
 343        pIn[2 * n - 2 * i] * pBTable[2 * i] -
 344        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 345
 346        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
 347        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 348        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
 349
 350      */
 351
 352
 353 #ifndef ARM_MATH_BIG_ENDIAN
 354
 355     /* pIn[2 * n - 2 * i] * pBTable[2 * i] -
 356        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
 357     outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB));
 358
 359 #else
 360
 361     /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +
 362        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
 363     outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
 364
 365 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
 366
 367     /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
 368        pIn[2 * n - 2 * i] * pBTable[2 * i] */
 369     outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 15u;
 370
 371     /*
 372        -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +
 373        pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
 374     outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
 375
 376     /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
 377
 378 #ifndef ARM_MATH_BIG_ENDIAN
 379
 380     outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
 381
 382 #else
 383
 384     outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
 385
 386 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
 387     /* write output */
 388
 389 #ifndef ARM_MATH_BIG_ENDIAN
 390
 391     *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 15u), 16);
 392
 393 #else
 394
 395     *__SIMD32(pDst1)++ = __PKHBT((outI >> 15u), outR, 16);
 396
 397 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
 398
 399     /* update coefficient pointer */
 400     pCoefB = pCoefB + (2u * modifier);
 401     pCoefA = pCoefA + (2u * modifier);
 402
 403     i--;
 404
 405   }
 406
 407
 408 #else
 409
 410   /* Run the below code for Cortex-M0 */
 411
 412   i = fftLen;
 413
 414   while(i > 0u)
 415   {
 416
 417     /*
 418        outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
 419        pIn[2 * n - 2 * i] * pBTable[2 * i] -
 420        pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
 421      */
 422
 423     outR = *pSrc2 * *pCoefB;
 424     outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
 425     outR = outR + (*pSrc1 * *pCoefA);
 426     outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 15;
 427
 428     /*
 429        outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
 430        pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
 431        pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
 432      */
 433
 434     outI = *(pSrc1 + 1) * *pCoefA;
 435     outI = outI - (*pSrc1 * *(pCoefA + 1));
 436     outI = outI - (*pSrc2 * *(pCoefB + 1));
 437     outI = outI - (*(pSrc2 + 1) * *(pCoefB));
 438
 439     /* update input pointers */
 440     pSrc1 += 2u;
 441     pSrc2 -= 2u;
 442
 443     /* write output */
 444     *pDst1++ = (q15_t) outR;
 445     *pDst1++ = (q15_t) (outI >> 15);
 446
 447     /* update coefficient pointer */
 448     pCoefB = pCoefB + (2u * modifier);
 449     pCoefA = pCoefA + (2u * modifier);
 450
 451     i--;
 452
 453   }
 454
 455 #endif /* #ifndef ARM_MATH_CM0 */
 456
 457 }