arm_rfft_q15.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_rfft_q15.c
00004  * Description:  RFFT & RIFFT Q15 process function
00005  *
00006  * $Date:        27. January 2017
00007  * $Revision:    V.1.5.1
00008  *
00009  * Target Processor: Cortex-M cores
00010  * -------------------------------------------------------------------- */
00011 /*
00012  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
00013  *
00014  * SPDX-License-Identifier: Apache-2.0
00015  *
00016  * Licensed under the Apache License, Version 2.0 (the License); you may
00017  * not use this file except in compliance with the License.
00018  * You may obtain a copy of the License at
00019  *
00020  * www.apache.org/licenses/LICENSE-2.0
00021  *
00022  * Unless required by applicable law or agreed to in writing, software
00023  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00024  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00025  * See the License for the specific language governing permissions and
00026  * limitations under the License.
00027  */
00028 
00029 #include "arm_math.h"
00030 
00031 /* ----------------------------------------------------------------------
00032  * Internal functions prototypes
00033  * -------------------------------------------------------------------- */
00034 
00035 void arm_split_rfft_q15(
00036     q15_t * pSrc,
00037     uint32_t fftLen,
00038     q15_t * pATable,
00039     q15_t * pBTable,
00040     q15_t * pDst,
00041     uint32_t modifier);
00042 
00043 void arm_split_rifft_q15(
00044     q15_t * pSrc,
00045     uint32_t fftLen,
00046     q15_t * pATable,
00047     q15_t * pBTable,
00048     q15_t * pDst,
00049     uint32_t modifier);
00050 
00051 /**
00052 * @addtogroup RealFFT
00053 * @{
00054 */
00055 
00056 /**
00057 * @brief Processing function for the Q15 RFFT/RIFFT.
00058 * @param[in]  *S    points to an instance of the Q15 RFFT/RIFFT structure.
00059 * @param[in]  *pSrc points to the input buffer.
00060 * @param[out] *pDst points to the output buffer.
00061 * @return none.
00062 *
00063 * \par Input an output formats:
00064 * \par
00065 * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
00066 * Hence the output format is different for different RFFT sizes.
00067 * 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:
00068 * \par
00069 * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"
00070 * \par
00071 * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"
00072 */
00073 
00074 void arm_rfft_q15(
00075     const arm_rfft_instance_q15 * S,
00076     q15_t * pSrc,
00077     q15_t * pDst)
00078 {
00079     const arm_cfft_instance_q15 *S_CFFT = S->pCfft;
00080     uint32_t i;
00081     uint32_t L2 = S->fftLenReal >> 1;
00082 
00083     /* Calculation of RIFFT of input */
00084     if (S->ifftFlagR == 1U)
00085     {
00086         /*  Real IFFT core process */
00087         arm_split_rifft_q15(pSrc, L2, S->pTwiddleAReal,
00088                             S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00089 
00090         /* Complex IFFT process */
00091         arm_cfft_q15(S_CFFT, pDst, S->ifftFlagR, S->bitReverseFlagR);
00092 
00093         for(i=0;i<S->fftLenReal;i++)
00094         {
00095             pDst[i] = pDst[i] << 1;
00096         }
00097     }
00098     else
00099     {
00100         /* Calculation of RFFT of input */
00101 
00102         /* Complex FFT process */
00103         arm_cfft_q15(S_CFFT, pSrc, S->ifftFlagR, S->bitReverseFlagR);
00104 
00105         /*  Real FFT core process */
00106         arm_split_rfft_q15(pSrc, L2, S->pTwiddleAReal,
00107                             S->pTwiddleBReal, pDst, S->twidCoefRModifier);
00108     }
00109 }
00110 
00111 /**
00112 * @} end of RealFFT group
00113 */
00114 
00115 /**
00116 * @brief  Core Real FFT process
00117 * @param  *pSrc                 points to the input buffer.
00118 * @param  fftLen                length of FFT.
00119 * @param  *pATable          points to the A twiddle Coef buffer.
00120 * @param  *pBTable          points to the B twiddle Coef buffer.
00121 * @param  *pDst                 points to the output buffer.
00122 * @param  modifier          twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
00123 * @return none.
00124 * The function implements a Real FFT
00125 */
00126 
00127 void arm_split_rfft_q15(
00128     q15_t * pSrc,
00129     uint32_t fftLen,
00130     q15_t * pATable,
00131     q15_t * pBTable,
00132     q15_t * pDst,
00133     uint32_t modifier)
00134 {
00135     uint32_t i;                                    /* Loop Counter */
00136     q31_t outR, outI;                              /* Temporary variables for output */
00137     q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00138     q15_t *pSrc1, *pSrc2;
00139 #if defined (ARM_MATH_DSP)
00140     q15_t *pD1, *pD2;
00141 #endif
00142 
00143     //  pSrc[2U * fftLen] = pSrc[0];
00144     //  pSrc[(2U * fftLen) + 1U] = pSrc[1];
00145 
00146     pCoefA = &pATable[modifier * 2U];
00147     pCoefB = &pBTable[modifier * 2U];
00148 
00149     pSrc1 = &pSrc[2];
00150     pSrc2 = &pSrc[(2U * fftLen) - 2U];
00151 
00152 #if defined (ARM_MATH_DSP)
00153 
00154     /* Run the below code for Cortex-M4 and Cortex-M3 */
00155     i = 1U;
00156     pD1 = pDst + 2;
00157     pD2 = pDst + (4U * fftLen) - 2;
00158 
00159     for(i = fftLen - 1; i > 0; i--)
00160     {
00161         /*
00162         outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
00163         + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
00164         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
00165         */
00166 
00167         /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
00168         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
00169         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
00170 
00171 
00172 #ifndef ARM_MATH_BIG_ENDIAN
00173 
00174         /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
00175         outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
00176 
00177 #else
00178 
00179         /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
00180         outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
00181 
00182 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00183 
00184         /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +
00185         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
00186         outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 16U;
00187 
00188         /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
00189         pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00190 
00191 #ifndef ARM_MATH_BIG_ENDIAN
00192 
00193         outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00194 
00195 #else
00196 
00197         outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
00198 
00199 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00200 
00201         /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
00202         outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
00203 
00204         /* write output */
00205         *pD1++ = (q15_t) outR;
00206         *pD1++ = outI >> 16U;
00207 
00208         /* write complex conjugate output */
00209         pD2[0] = (q15_t) outR;
00210         pD2[1] = -(outI >> 16U);
00211         pD2 -= 2;
00212 
00213         /* update coefficient pointer */
00214         pCoefB = pCoefB + (2U * modifier);
00215         pCoefA = pCoefA + (2U * modifier);
00216     }
00217 
00218     pDst[2U * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
00219     pDst[(2U * fftLen) + 1U] = 0;
00220 
00221     pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
00222     pDst[1] = 0;
00223 
00224 #else
00225 
00226     /* Run the below code for Cortex-M0 */
00227     i = 1U;
00228 
00229     while (i < fftLen)
00230     {
00231         /*
00232         outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
00233         + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
00234         pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
00235         */
00236 
00237         outR = *pSrc1 * *pCoefA;
00238         outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
00239         outR = outR + (*pSrc2 * *pCoefB);
00240         outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 16;
00241 
00242 
00243         /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
00244         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
00245         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
00246         */
00247 
00248         outI = *pSrc2 * *(pCoefB + 1);
00249         outI = outI - (*(pSrc2 + 1) * *pCoefB);
00250         outI = outI + (*(pSrc1 + 1) * *pCoefA);
00251         outI = outI + (*pSrc1 * *(pCoefA + 1));
00252 
00253         /* update input pointers */
00254         pSrc1 += 2U;
00255         pSrc2 -= 2U;
00256 
00257         /* write output */
00258         pDst[2U * i] = (q15_t) outR;
00259         pDst[(2U * i) + 1U] = outI >> 16U;
00260 
00261         /* write complex conjugate output */
00262         pDst[(4U * fftLen) - (2U * i)] = (q15_t) outR;
00263         pDst[((4U * fftLen) - (2U * i)) + 1U] = -(outI >> 16U);
00264 
00265         /* update coefficient pointer */
00266         pCoefB = pCoefB + (2U * modifier);
00267         pCoefA = pCoefA + (2U * modifier);
00268 
00269         i++;
00270     }
00271 
00272     pDst[2U * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
00273     pDst[(2U * fftLen) + 1U] = 0;
00274 
00275     pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
00276     pDst[1] = 0;
00277 
00278 #endif /* #if defined (ARM_MATH_DSP) */
00279 }
00280 
00281 
00282 /**
00283 * @brief  Core Real IFFT process
00284 * @param[in]   *pSrc                points to the input buffer.
00285 * @param[in]   fftLen           length of FFT.
00286 * @param[in]   *pATable             points to the twiddle Coef A buffer.
00287 * @param[in]   *pBTable             points to the twiddle Coef B buffer.
00288 * @param[out]  *pDst                points to the output buffer.
00289 * @param[in]   modifier             twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
00290 * @return none.
00291 * The function implements a Real IFFT
00292 */
00293 void arm_split_rifft_q15(
00294     q15_t * pSrc,
00295     uint32_t fftLen,
00296     q15_t * pATable,
00297     q15_t * pBTable,
00298     q15_t * pDst,
00299     uint32_t modifier)
00300 {
00301     uint32_t i;                                    /* Loop Counter */
00302     q31_t outR, outI;                              /* Temporary variables for output */
00303     q15_t *pCoefA, *pCoefB;                        /* Temporary pointers for twiddle factors */
00304     q15_t *pSrc1, *pSrc2;
00305     q15_t *pDst1 = &pDst[0];
00306 
00307     pCoefA = &pATable[0];
00308     pCoefB = &pBTable[0];
00309 
00310     pSrc1 = &pSrc[0];
00311     pSrc2 = &pSrc[2U * fftLen];
00312 
00313 #if defined (ARM_MATH_DSP)
00314 
00315     /* Run the below code for Cortex-M4 and Cortex-M3 */
00316     i = fftLen;
00317 
00318     while (i > 0U)
00319     {
00320         /*
00321         outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
00322         pIn[2 * n - 2 * i] * pBTable[2 * i] -
00323         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
00324 
00325         outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
00326         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
00327         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
00328         */
00329 
00330 
00331 #ifndef ARM_MATH_BIG_ENDIAN
00332 
00333         /* pIn[2 * n - 2 * i] * pBTable[2 * i] -
00334         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
00335         outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB));
00336 
00337 #else
00338 
00339         /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +
00340         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
00341         outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
00342 
00343 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00344 
00345         /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
00346         pIn[2 * n - 2 * i] * pBTable[2 * i] */
00347         outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 16U;
00348 
00349         /*
00350         -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +
00351         pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
00352         outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
00353 
00354         /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
00355 
00356 #ifndef ARM_MATH_BIG_ENDIAN
00357 
00358         outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
00359 
00360 #else
00361 
00362         outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
00363 
00364 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00365         /* write output */
00366 
00367 #ifndef ARM_MATH_BIG_ENDIAN
00368 
00369         *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 16U), 16);
00370 
00371 #else
00372 
00373         *__SIMD32(pDst1)++ = __PKHBT((outI >> 16U), outR, 16);
00374 
00375 #endif /*      #ifndef ARM_MATH_BIG_ENDIAN     */
00376 
00377         /* update coefficient pointer */
00378         pCoefB = pCoefB + (2U * modifier);
00379         pCoefA = pCoefA + (2U * modifier);
00380 
00381         i--;
00382     }
00383 #else
00384     /* Run the below code for Cortex-M0 */
00385     i = fftLen;
00386 
00387     while (i > 0U)
00388     {
00389         /*
00390         outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
00391         pIn[2 * n - 2 * i] * pBTable[2 * i] -
00392         pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
00393         */
00394 
00395         outR = *pSrc2 * *pCoefB;
00396         outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
00397         outR = outR + (*pSrc1 * *pCoefA);
00398         outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 16;
00399 
00400         /*
00401         outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
00402         pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
00403         pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
00404         */
00405 
00406         outI = *(pSrc1 + 1) * *pCoefA;
00407         outI = outI - (*pSrc1 * *(pCoefA + 1));
00408         outI = outI - (*pSrc2 * *(pCoefB + 1));
00409         outI = outI - (*(pSrc2 + 1) * *(pCoefB));
00410 
00411         /* update input pointers */
00412         pSrc1 += 2U;
00413         pSrc2 -= 2U;
00414 
00415         /* write output */
00416         *pDst1++ = (q15_t) outR;
00417         *pDst1++ = (q15_t) (outI >> 16);
00418 
00419         /* update coefficient pointer */
00420         pCoefB = pCoefB + (2U * modifier);
00421         pCoefA = pCoefA + (2U * modifier);
00422 
00423         i--;
00424     }
00425 #endif /* #if defined (ARM_MATH_DSP) */
00426 }
00427