CMSIS DSP library
Dependents: KL25Z_FFT_Demo Hat_Board_v5_1 KL25Z_FFT_Demo_tony KL25Z_FFT_Demo_tony ... more
Fork of mbed-dsp by
arm_rfft_q15.c
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 17. January 2013 00005 * $Revision: V1.4.1 00006 * 00007 * Project: CMSIS DSP Library 00008 * Title: arm_rfft_q15.c 00009 * 00010 * Description: RFFT & RIFFT Q15 process function 00011 * 00012 * 00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 00014 * 00015 * Redistribution and use in source and binary forms, with or without 00016 * modification, are permitted provided that the following conditions 00017 * are met: 00018 * - Redistributions of source code must retain the above copyright 00019 * notice, this list of conditions and the following disclaimer. 00020 * - Redistributions in binary form must reproduce the above copyright 00021 * notice, this list of conditions and the following disclaimer in 00022 * the documentation and/or other materials provided with the 00023 * distribution. 00024 * - Neither the name of ARM LIMITED nor the names of its contributors 00025 * may be used to endorse or promote products derived from this 00026 * software without specific prior written permission. 00027 * 00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00039 * POSSIBILITY OF SUCH DAMAGE. 00040 * -------------------------------------------------------------------- */ 00041 00042 #include "arm_math.h" 00043 00044 void arm_radix4_butterfly_q15( 00045 q15_t * pSrc16, 00046 uint32_t fftLen, 00047 q15_t * pCoef16, 00048 uint32_t twidCoefModifier); 00049 00050 void arm_radix4_butterfly_inverse_q15( 00051 q15_t * pSrc16, 00052 uint32_t fftLen, 00053 q15_t * pCoef16, 00054 uint32_t twidCoefModifier); 00055 00056 void arm_bitreversal_q15( 00057 q15_t * pSrc, 00058 uint32_t fftLen, 00059 uint16_t bitRevFactor, 00060 uint16_t * pBitRevTab); 00061 00062 /*-------------------------------------------------------------------- 00063 * Internal functions prototypes 00064 --------------------------------------------------------------------*/ 00065 00066 void arm_split_rfft_q15( 00067 q15_t * pSrc, 00068 uint32_t fftLen, 00069 q15_t * pATable, 00070 q15_t * pBTable, 00071 q15_t * pDst, 00072 uint32_t modifier); 00073 00074 void arm_split_rifft_q15( 00075 q15_t * pSrc, 00076 uint32_t fftLen, 00077 q15_t * pATable, 00078 q15_t * pBTable, 00079 q15_t * pDst, 00080 uint32_t modifier); 00081 00082 /** 00083 * @addtogroup RealFFT 00084 * @{ 00085 */ 00086 00087 /** 00088 * @brief Processing function for the Q15 RFFT/RIFFT. 00089 * @param[in] *S points to an instance of the Q15 RFFT/RIFFT structure. 00090 * @param[in] *pSrc points to the input buffer. 00091 * @param[out] *pDst points to the output buffer. 00092 * @return none. 00093 * 00094 * \par Input an output formats: 00095 * \par 00096 * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process. 00097 * Hence the output format is different for different RFFT sizes. 00098 * 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: 00099 * \par 00100 * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT" 00101 * \par 00102 * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT" 00103 */ 00104 00105 void arm_rfft_q15( 00106 const arm_rfft_instance_q15 * S, 00107 q15_t * pSrc, 00108 q15_t * pDst) 00109 { 00110 const arm_cfft_radix4_instance_q15 *S_CFFT = S->pCfft; 00111 00112 /* Calculation of RIFFT of input */ 00113 if(S->ifftFlagR == 1u) 00114 { 00115 /* Real IFFT core process */ 00116 arm_split_rifft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal, 00117 S->pTwiddleBReal, pDst, S->twidCoefRModifier); 00118 00119 /* Complex readix-4 IFFT process */ 00120 arm_radix4_butterfly_inverse_q15(pDst, S_CFFT->fftLen, 00121 S_CFFT->pTwiddle, 00122 S_CFFT->twidCoefModifier); 00123 00124 /* Bit reversal process */ 00125 if(S->bitReverseFlagR == 1u) 00126 { 00127 arm_bitreversal_q15(pDst, S_CFFT->fftLen, 00128 S_CFFT->bitRevFactor, S_CFFT->pBitRevTable); 00129 } 00130 } 00131 else 00132 { 00133 /* Calculation of RFFT of input */ 00134 00135 /* Complex readix-4 FFT process */ 00136 arm_radix4_butterfly_q15(pSrc, S_CFFT->fftLen, 00137 S_CFFT->pTwiddle, S_CFFT->twidCoefModifier); 00138 00139 /* Bit reversal process */ 00140 if(S->bitReverseFlagR == 1u) 00141 { 00142 arm_bitreversal_q15(pSrc, S_CFFT->fftLen, 00143 S_CFFT->bitRevFactor, S_CFFT->pBitRevTable); 00144 } 00145 00146 arm_split_rfft_q15(pSrc, S->fftLenBy2, S->pTwiddleAReal, 00147 S->pTwiddleBReal, pDst, S->twidCoefRModifier); 00148 } 00149 00150 } 00151 00152 /** 00153 * @} end of RealFFT group 00154 */ 00155 00156 /** 00157 * @brief Core Real FFT process 00158 * @param *pSrc points to the input buffer. 00159 * @param fftLen length of FFT. 00160 * @param *pATable points to the A twiddle Coef buffer. 00161 * @param *pBTable points to the B twiddle Coef buffer. 00162 * @param *pDst points to the output buffer. 00163 * @param modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. 00164 * @return none. 00165 * The function implements a Real FFT 00166 */ 00167 00168 void arm_split_rfft_q15( 00169 q15_t * pSrc, 00170 uint32_t fftLen, 00171 q15_t * pATable, 00172 q15_t * pBTable, 00173 q15_t * pDst, 00174 uint32_t modifier) 00175 { 00176 uint32_t i; /* Loop Counter */ 00177 q31_t outR, outI; /* Temporary variables for output */ 00178 q15_t *pCoefA, *pCoefB; /* Temporary pointers for twiddle factors */ 00179 q15_t *pSrc1, *pSrc2; 00180 00181 00182 // pSrc[2u * fftLen] = pSrc[0]; 00183 // pSrc[(2u * fftLen) + 1u] = pSrc[1]; 00184 00185 pCoefA = &pATable[modifier * 2u]; 00186 pCoefB = &pBTable[modifier * 2u]; 00187 00188 pSrc1 = &pSrc[2]; 00189 pSrc2 = &pSrc[(2u * fftLen) - 2u]; 00190 00191 #ifndef ARM_MATH_CM0_FAMILY 00192 00193 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00194 00195 i = 1u; 00196 00197 while(i < fftLen) 00198 { 00199 /* 00200 outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] 00201 + pSrc[2 * n - 2 * i] * pBTable[2 * i] + 00202 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]); 00203 */ 00204 00205 /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] + 00206 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] - 00207 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */ 00208 00209 00210 #ifndef ARM_MATH_BIG_ENDIAN 00211 00212 /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */ 00213 outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)); 00214 00215 #else 00216 00217 /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */ 00218 outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA))); 00219 00220 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00221 00222 /* pSrc[2 * n - 2 * i] * pBTable[2 * i] + 00223 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */ 00224 outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 15u; 00225 00226 /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] - 00227 pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */ 00228 00229 #ifndef ARM_MATH_BIG_ENDIAN 00230 00231 outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB)); 00232 00233 #else 00234 00235 outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--); 00236 00237 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00238 00239 /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */ 00240 outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI); 00241 00242 /* write output */ 00243 pDst[2u * i] = (q15_t) outR; 00244 pDst[(2u * i) + 1u] = outI >> 15u; 00245 00246 /* write complex conjugate output */ 00247 pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR; 00248 pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u); 00249 00250 /* update coefficient pointer */ 00251 pCoefB = pCoefB + (2u * modifier); 00252 pCoefA = pCoefA + (2u * modifier); 00253 00254 i++; 00255 00256 } 00257 00258 pDst[2u * fftLen] = pSrc[0] - pSrc[1]; 00259 pDst[(2u * fftLen) + 1u] = 0; 00260 00261 pDst[0] = pSrc[0] + pSrc[1]; 00262 pDst[1] = 0; 00263 00264 00265 #else 00266 00267 /* Run the below code for Cortex-M0 */ 00268 00269 i = 1u; 00270 00271 while(i < fftLen) 00272 { 00273 /* 00274 outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] 00275 + pSrc[2 * n - 2 * i] * pBTable[2 * i] + 00276 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]); 00277 */ 00278 00279 outR = *pSrc1 * *pCoefA; 00280 outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1)); 00281 outR = outR + (*pSrc2 * *pCoefB); 00282 outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 15; 00283 00284 00285 /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] + 00286 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] - 00287 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); 00288 */ 00289 00290 outI = *pSrc2 * *(pCoefB + 1); 00291 outI = outI - (*(pSrc2 + 1) * *pCoefB); 00292 outI = outI + (*(pSrc1 + 1) * *pCoefA); 00293 outI = outI + (*pSrc1 * *(pCoefA + 1)); 00294 00295 /* update input pointers */ 00296 pSrc1 += 2u; 00297 pSrc2 -= 2u; 00298 00299 /* write output */ 00300 pDst[2u * i] = (q15_t) outR; 00301 pDst[(2u * i) + 1u] = outI >> 15u; 00302 00303 /* write complex conjugate output */ 00304 pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR; 00305 pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 15u); 00306 00307 /* update coefficient pointer */ 00308 pCoefB = pCoefB + (2u * modifier); 00309 pCoefA = pCoefA + (2u * modifier); 00310 00311 i++; 00312 00313 } 00314 00315 pDst[2u * fftLen] = pSrc[0] - pSrc[1]; 00316 pDst[(2u * fftLen) + 1u] = 0; 00317 00318 pDst[0] = pSrc[0] + pSrc[1]; 00319 pDst[1] = 0; 00320 00321 #endif /* #ifndef ARM_MATH_CM0_FAMILY */ 00322 00323 } 00324 00325 00326 /** 00327 * @brief Core Real IFFT process 00328 * @param[in] *pSrc points to the input buffer. 00329 * @param[in] fftLen length of FFT. 00330 * @param[in] *pATable points to the twiddle Coef A buffer. 00331 * @param[in] *pBTable points to the twiddle Coef B buffer. 00332 * @param[out] *pDst points to the output buffer. 00333 * @param[in] modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. 00334 * @return none. 00335 * The function implements a Real IFFT 00336 */ 00337 void arm_split_rifft_q15( 00338 q15_t * pSrc, 00339 uint32_t fftLen, 00340 q15_t * pATable, 00341 q15_t * pBTable, 00342 q15_t * pDst, 00343 uint32_t modifier) 00344 { 00345 uint32_t i; /* Loop Counter */ 00346 q31_t outR, outI; /* Temporary variables for output */ 00347 q15_t *pCoefA, *pCoefB; /* Temporary pointers for twiddle factors */ 00348 q15_t *pSrc1, *pSrc2; 00349 q15_t *pDst1 = &pDst[0]; 00350 00351 pCoefA = &pATable[0]; 00352 pCoefB = &pBTable[0]; 00353 00354 pSrc1 = &pSrc[0]; 00355 pSrc2 = &pSrc[2u * fftLen]; 00356 00357 #ifndef ARM_MATH_CM0_FAMILY 00358 00359 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00360 00361 i = fftLen; 00362 00363 while(i > 0u) 00364 { 00365 00366 /* 00367 outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] + 00368 pIn[2 * n - 2 * i] * pBTable[2 * i] - 00369 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]); 00370 00371 outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] - 00372 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] - 00373 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); 00374 00375 */ 00376 00377 00378 #ifndef ARM_MATH_BIG_ENDIAN 00379 00380 /* pIn[2 * n - 2 * i] * pBTable[2 * i] - 00381 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */ 00382 outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)); 00383 00384 #else 00385 00386 /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] + 00387 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */ 00388 outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB))); 00389 00390 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00391 00392 /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] + 00393 pIn[2 * n - 2 * i] * pBTable[2 * i] */ 00394 outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 15u; 00395 00396 /* 00397 -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] + 00398 pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */ 00399 outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB)); 00400 00401 /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */ 00402 00403 #ifndef ARM_MATH_BIG_ENDIAN 00404 00405 outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI); 00406 00407 #else 00408 00409 outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI); 00410 00411 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00412 /* write output */ 00413 00414 #ifndef ARM_MATH_BIG_ENDIAN 00415 00416 *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 15u), 16); 00417 00418 #else 00419 00420 *__SIMD32(pDst1)++ = __PKHBT((outI >> 15u), outR, 16); 00421 00422 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00423 00424 /* update coefficient pointer */ 00425 pCoefB = pCoefB + (2u * modifier); 00426 pCoefA = pCoefA + (2u * modifier); 00427 00428 i--; 00429 00430 } 00431 00432 00433 #else 00434 00435 /* Run the below code for Cortex-M0 */ 00436 00437 i = fftLen; 00438 00439 while(i > 0u) 00440 { 00441 00442 /* 00443 outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] + 00444 pIn[2 * n - 2 * i] * pBTable[2 * i] - 00445 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]); 00446 */ 00447 00448 outR = *pSrc2 * *pCoefB; 00449 outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1)); 00450 outR = outR + (*pSrc1 * *pCoefA); 00451 outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 15; 00452 00453 /* 00454 outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] - 00455 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] - 00456 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); 00457 */ 00458 00459 outI = *(pSrc1 + 1) * *pCoefA; 00460 outI = outI - (*pSrc1 * *(pCoefA + 1)); 00461 outI = outI - (*pSrc2 * *(pCoefB + 1)); 00462 outI = outI - (*(pSrc2 + 1) * *(pCoefB)); 00463 00464 /* update input pointers */ 00465 pSrc1 += 2u; 00466 pSrc2 -= 2u; 00467 00468 /* write output */ 00469 *pDst1++ = (q15_t) outR; 00470 *pDst1++ = (q15_t) (outI >> 15); 00471 00472 /* update coefficient pointer */ 00473 pCoefB = pCoefB + (2u * modifier); 00474 pCoefA = pCoefA + (2u * modifier); 00475 00476 i--; 00477 00478 } 00479 00480 #endif /* #ifndef ARM_MATH_CM0_FAMILY */ 00481 00482 }
Generated on Tue Jul 12 2022 12:36:57 by 1.7.2