Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

Dependents:   Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more

functions/TransformFunctions/arm_rfft_q15.c@3:4098b9d3d571, 2018-06-21 (annotated)

Committer:
xorjoep
Date:
Thu Jun 21 11:56:27 2018 +0000
Revision:
3:4098b9d3d571
Parent:
1:24714b45cd1b 
headers is a folder not a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 1:24714b45cd1b 1 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 2 * Project: CMSIS DSP Library
xorjoep 1:24714b45cd1b 3 * Title: arm_rfft_q15.c
xorjoep 1:24714b45cd1b 4 * Description: RFFT & RIFFT Q15 process function
xorjoep 1:24714b45cd1b 5 *
xorjoep 1:24714b45cd1b 6 * $Date: 27. January 2017
xorjoep 1:24714b45cd1b 7 * $Revision: V.1.5.1
xorjoep 1:24714b45cd1b 8 *
xorjoep 1:24714b45cd1b 9 * Target Processor: Cortex-M cores
xorjoep 1:24714b45cd1b 10 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 11 /*
xorjoep 1:24714b45cd1b 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
xorjoep 1:24714b45cd1b 13 *
xorjoep 1:24714b45cd1b 14 * SPDX-License-Identifier: Apache-2.0
xorjoep 1:24714b45cd1b 15 *
xorjoep 1:24714b45cd1b 16 * Licensed under the Apache License, Version 2.0 (the License); you may
xorjoep 1:24714b45cd1b 17 * not use this file except in compliance with the License.
xorjoep 1:24714b45cd1b 18 * You may obtain a copy of the License at
xorjoep 1:24714b45cd1b 19 *
xorjoep 1:24714b45cd1b 20 * www.apache.org/licenses/LICENSE-2.0
xorjoep 1:24714b45cd1b 21 *
xorjoep 1:24714b45cd1b 22 * Unless required by applicable law or agreed to in writing, software
xorjoep 1:24714b45cd1b 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
xorjoep 1:24714b45cd1b 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
xorjoep 1:24714b45cd1b 25 * See the License for the specific language governing permissions and
xorjoep 1:24714b45cd1b 26 * limitations under the License.
xorjoep 1:24714b45cd1b 27 */
xorjoep 1:24714b45cd1b 28
xorjoep 1:24714b45cd1b 29 #include "arm_math.h"
xorjoep 1:24714b45cd1b 30
xorjoep 1:24714b45cd1b 31 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 32 * Internal functions prototypes
xorjoep 1:24714b45cd1b 33 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 34
xorjoep 1:24714b45cd1b 35 void arm_split_rfft_q15(
xorjoep 1:24714b45cd1b 36 q15_t * pSrc,
xorjoep 1:24714b45cd1b 37 uint32_t fftLen,
xorjoep 1:24714b45cd1b 38 q15_t * pATable,
xorjoep 1:24714b45cd1b 39 q15_t * pBTable,
xorjoep 1:24714b45cd1b 40 q15_t * pDst,
xorjoep 1:24714b45cd1b 41 uint32_t modifier);
xorjoep 1:24714b45cd1b 42
xorjoep 1:24714b45cd1b 43 void arm_split_rifft_q15(
xorjoep 1:24714b45cd1b 44 q15_t * pSrc,
xorjoep 1:24714b45cd1b 45 uint32_t fftLen,
xorjoep 1:24714b45cd1b 46 q15_t * pATable,
xorjoep 1:24714b45cd1b 47 q15_t * pBTable,
xorjoep 1:24714b45cd1b 48 q15_t * pDst,
xorjoep 1:24714b45cd1b 49 uint32_t modifier);
xorjoep 1:24714b45cd1b 50
xorjoep 1:24714b45cd1b 51 /**
xorjoep 1:24714b45cd1b 52 * @addtogroup RealFFT
xorjoep 1:24714b45cd1b 53 * @{
xorjoep 1:24714b45cd1b 54 */
xorjoep 1:24714b45cd1b 55
xorjoep 1:24714b45cd1b 56 /**
xorjoep 1:24714b45cd1b 57 * @brief Processing function for the Q15 RFFT/RIFFT.
xorjoep 1:24714b45cd1b 58 * @param[in] *S points to an instance of the Q15 RFFT/RIFFT structure.
xorjoep 1:24714b45cd1b 59 * @param[in] *pSrc points to the input buffer.
xorjoep 1:24714b45cd1b 60 * @param[out] *pDst points to the output buffer.
xorjoep 1:24714b45cd1b 61 * @return none.
xorjoep 1:24714b45cd1b 62 *
xorjoep 1:24714b45cd1b 63 * \par Input an output formats:
xorjoep 1:24714b45cd1b 64 * \par
xorjoep 1:24714b45cd1b 65 * Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
xorjoep 1:24714b45cd1b 66 * Hence the output format is different for different RFFT sizes.
xorjoep 1:24714b45cd1b 67 * 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:
xorjoep 1:24714b45cd1b 68 * \par
xorjoep 1:24714b45cd1b 69 * \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"
xorjoep 1:24714b45cd1b 70 * \par
xorjoep 1:24714b45cd1b 71 * \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"
xorjoep 1:24714b45cd1b 72 */
xorjoep 1:24714b45cd1b 73
xorjoep 1:24714b45cd1b 74 void arm_rfft_q15(
xorjoep 1:24714b45cd1b 75 const arm_rfft_instance_q15 * S,
xorjoep 1:24714b45cd1b 76 q15_t * pSrc,
xorjoep 1:24714b45cd1b 77 q15_t * pDst)
xorjoep 1:24714b45cd1b 78 {
xorjoep 1:24714b45cd1b 79 const arm_cfft_instance_q15 *S_CFFT = S->pCfft;
xorjoep 1:24714b45cd1b 80 uint32_t i;
xorjoep 1:24714b45cd1b 81 uint32_t L2 = S->fftLenReal >> 1;
xorjoep 1:24714b45cd1b 82
xorjoep 1:24714b45cd1b 83 /* Calculation of RIFFT of input */
xorjoep 1:24714b45cd1b 84 if (S->ifftFlagR == 1U)
xorjoep 1:24714b45cd1b 85 {
xorjoep 1:24714b45cd1b 86 /* Real IFFT core process */
xorjoep 1:24714b45cd1b 87 arm_split_rifft_q15(pSrc, L2, S->pTwiddleAReal,
xorjoep 1:24714b45cd1b 88 S->pTwiddleBReal, pDst, S->twidCoefRModifier);
xorjoep 1:24714b45cd1b 89
xorjoep 1:24714b45cd1b 90 /* Complex IFFT process */
xorjoep 1:24714b45cd1b 91 arm_cfft_q15(S_CFFT, pDst, S->ifftFlagR, S->bitReverseFlagR);
xorjoep 1:24714b45cd1b 92
xorjoep 1:24714b45cd1b 93 for(i=0;i<S->fftLenReal;i++)
xorjoep 1:24714b45cd1b 94 {
xorjoep 1:24714b45cd1b 95 pDst[i] = pDst[i] << 1;
xorjoep 1:24714b45cd1b 96 }
xorjoep 1:24714b45cd1b 97 }
xorjoep 1:24714b45cd1b 98 else
xorjoep 1:24714b45cd1b 99 {
xorjoep 1:24714b45cd1b 100 /* Calculation of RFFT of input */
xorjoep 1:24714b45cd1b 101
xorjoep 1:24714b45cd1b 102 /* Complex FFT process */
xorjoep 1:24714b45cd1b 103 arm_cfft_q15(S_CFFT, pSrc, S->ifftFlagR, S->bitReverseFlagR);
xorjoep 1:24714b45cd1b 104
xorjoep 1:24714b45cd1b 105 /* Real FFT core process */
xorjoep 1:24714b45cd1b 106 arm_split_rfft_q15(pSrc, L2, S->pTwiddleAReal,
xorjoep 1:24714b45cd1b 107 S->pTwiddleBReal, pDst, S->twidCoefRModifier);
xorjoep 1:24714b45cd1b 108 }
xorjoep 1:24714b45cd1b 109 }
xorjoep 1:24714b45cd1b 110
xorjoep 1:24714b45cd1b 111 /**
xorjoep 1:24714b45cd1b 112 * @} end of RealFFT group
xorjoep 1:24714b45cd1b 113 */
xorjoep 1:24714b45cd1b 114
xorjoep 1:24714b45cd1b 115 /**
xorjoep 1:24714b45cd1b 116 * @brief Core Real FFT process
xorjoep 1:24714b45cd1b 117 * @param *pSrc points to the input buffer.
xorjoep 1:24714b45cd1b 118 * @param fftLen length of FFT.
xorjoep 1:24714b45cd1b 119 * @param *pATable points to the A twiddle Coef buffer.
xorjoep 1:24714b45cd1b 120 * @param *pBTable points to the B twiddle Coef buffer.
xorjoep 1:24714b45cd1b 121 * @param *pDst points to the output buffer.
xorjoep 1:24714b45cd1b 122 * @param modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
xorjoep 1:24714b45cd1b 123 * @return none.
xorjoep 1:24714b45cd1b 124 * The function implements a Real FFT
xorjoep 1:24714b45cd1b 125 */
xorjoep 1:24714b45cd1b 126
xorjoep 1:24714b45cd1b 127 void arm_split_rfft_q15(
xorjoep 1:24714b45cd1b 128 q15_t * pSrc,
xorjoep 1:24714b45cd1b 129 uint32_t fftLen,
xorjoep 1:24714b45cd1b 130 q15_t * pATable,
xorjoep 1:24714b45cd1b 131 q15_t * pBTable,
xorjoep 1:24714b45cd1b 132 q15_t * pDst,
xorjoep 1:24714b45cd1b 133 uint32_t modifier)
xorjoep 1:24714b45cd1b 134 {
xorjoep 1:24714b45cd1b 135 uint32_t i; /* Loop Counter */
xorjoep 1:24714b45cd1b 136 q31_t outR, outI; /* Temporary variables for output */
xorjoep 1:24714b45cd1b 137 q15_t *pCoefA, *pCoefB; /* Temporary pointers for twiddle factors */
xorjoep 1:24714b45cd1b 138 q15_t *pSrc1, *pSrc2;
xorjoep 1:24714b45cd1b 139 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 140 q15_t *pD1, *pD2;
xorjoep 1:24714b45cd1b 141 #endif
xorjoep 1:24714b45cd1b 142
xorjoep 1:24714b45cd1b 143 // pSrc[2U * fftLen] = pSrc[0];
xorjoep 1:24714b45cd1b 144 // pSrc[(2U * fftLen) + 1U] = pSrc[1];
xorjoep 1:24714b45cd1b 145
xorjoep 1:24714b45cd1b 146 pCoefA = &pATable[modifier * 2U];
xorjoep 1:24714b45cd1b 147 pCoefB = &pBTable[modifier * 2U];
xorjoep 1:24714b45cd1b 148
xorjoep 1:24714b45cd1b 149 pSrc1 = &pSrc[2];
xorjoep 1:24714b45cd1b 150 pSrc2 = &pSrc[(2U * fftLen) - 2U];
xorjoep 1:24714b45cd1b 151
xorjoep 1:24714b45cd1b 152 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 153
xorjoep 1:24714b45cd1b 154 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 155 i = 1U;
xorjoep 1:24714b45cd1b 156 pD1 = pDst + 2;
xorjoep 1:24714b45cd1b 157 pD2 = pDst + (4U * fftLen) - 2;
xorjoep 1:24714b45cd1b 158
xorjoep 1:24714b45cd1b 159 for(i = fftLen - 1; i > 0; i--)
xorjoep 1:24714b45cd1b 160 {
xorjoep 1:24714b45cd1b 161 /*
xorjoep 1:24714b45cd1b 162 outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
xorjoep 1:24714b45cd1b 163 + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
xorjoep 1:24714b45cd1b 164 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
xorjoep 1:24714b45cd1b 165 */
xorjoep 1:24714b45cd1b 166
xorjoep 1:24714b45cd1b 167 /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
xorjoep 1:24714b45cd1b 168 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
xorjoep 1:24714b45cd1b 169 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
xorjoep 1:24714b45cd1b 170
xorjoep 1:24714b45cd1b 171
xorjoep 1:24714b45cd1b 172 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 173
xorjoep 1:24714b45cd1b 174 /* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
xorjoep 1:24714b45cd1b 175 outR = __SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA));
xorjoep 1:24714b45cd1b 176
xorjoep 1:24714b45cd1b 177 #else
xorjoep 1:24714b45cd1b 178
xorjoep 1:24714b45cd1b 179 /* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
xorjoep 1:24714b45cd1b 180 outR = -(__SMUSD(*__SIMD32(pSrc1), *__SIMD32(pCoefA)));
xorjoep 1:24714b45cd1b 181
xorjoep 1:24714b45cd1b 182 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 183
xorjoep 1:24714b45cd1b 184 /* pSrc[2 * n - 2 * i] * pBTable[2 * i] +
xorjoep 1:24714b45cd1b 185 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
xorjoep 1:24714b45cd1b 186 outR = __SMLAD(*__SIMD32(pSrc2), *__SIMD32(pCoefB), outR) >> 16U;
xorjoep 1:24714b45cd1b 187
xorjoep 1:24714b45cd1b 188 /* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
xorjoep 1:24714b45cd1b 189 pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
xorjoep 1:24714b45cd1b 190
xorjoep 1:24714b45cd1b 191 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 192
xorjoep 1:24714b45cd1b 193 outI = __SMUSDX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
xorjoep 1:24714b45cd1b 194
xorjoep 1:24714b45cd1b 195 #else
xorjoep 1:24714b45cd1b 196
xorjoep 1:24714b45cd1b 197 outI = __SMUSDX(*__SIMD32(pCoefB), *__SIMD32(pSrc2)--);
xorjoep 1:24714b45cd1b 198
xorjoep 1:24714b45cd1b 199 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 200
xorjoep 1:24714b45cd1b 201 /* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
xorjoep 1:24714b45cd1b 202 outI = __SMLADX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), outI);
xorjoep 1:24714b45cd1b 203
xorjoep 1:24714b45cd1b 204 /* write output */
xorjoep 1:24714b45cd1b 205 *pD1++ = (q15_t) outR;
xorjoep 1:24714b45cd1b 206 *pD1++ = outI >> 16U;
xorjoep 1:24714b45cd1b 207
xorjoep 1:24714b45cd1b 208 /* write complex conjugate output */
xorjoep 1:24714b45cd1b 209 pD2[0] = (q15_t) outR;
xorjoep 1:24714b45cd1b 210 pD2[1] = -(outI >> 16U);
xorjoep 1:24714b45cd1b 211 pD2 -= 2;
xorjoep 1:24714b45cd1b 212
xorjoep 1:24714b45cd1b 213 /* update coefficient pointer */
xorjoep 1:24714b45cd1b 214 pCoefB = pCoefB + (2U * modifier);
xorjoep 1:24714b45cd1b 215 pCoefA = pCoefA + (2U * modifier);
xorjoep 1:24714b45cd1b 216 }
xorjoep 1:24714b45cd1b 217
xorjoep 1:24714b45cd1b 218 pDst[2U * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
xorjoep 1:24714b45cd1b 219 pDst[(2U * fftLen) + 1U] = 0;
xorjoep 1:24714b45cd1b 220
xorjoep 1:24714b45cd1b 221 pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
xorjoep 1:24714b45cd1b 222 pDst[1] = 0;
xorjoep 1:24714b45cd1b 223
xorjoep 1:24714b45cd1b 224 #else
xorjoep 1:24714b45cd1b 225
xorjoep 1:24714b45cd1b 226 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 227 i = 1U;
xorjoep 1:24714b45cd1b 228
xorjoep 1:24714b45cd1b 229 while (i < fftLen)
xorjoep 1:24714b45cd1b 230 {
xorjoep 1:24714b45cd1b 231 /*
xorjoep 1:24714b45cd1b 232 outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
xorjoep 1:24714b45cd1b 233 + pSrc[2 * n - 2 * i] * pBTable[2 * i] +
xorjoep 1:24714b45cd1b 234 pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
xorjoep 1:24714b45cd1b 235 */
xorjoep 1:24714b45cd1b 236
xorjoep 1:24714b45cd1b 237 outR = *pSrc1 * *pCoefA;
xorjoep 1:24714b45cd1b 238 outR = outR - (*(pSrc1 + 1) * *(pCoefA + 1));
xorjoep 1:24714b45cd1b 239 outR = outR + (*pSrc2 * *pCoefB);
xorjoep 1:24714b45cd1b 240 outR = (outR + (*(pSrc2 + 1) * *(pCoefB + 1))) >> 16;
xorjoep 1:24714b45cd1b 241
xorjoep 1:24714b45cd1b 242
xorjoep 1:24714b45cd1b 243 /* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
xorjoep 1:24714b45cd1b 244 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
xorjoep 1:24714b45cd1b 245 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
xorjoep 1:24714b45cd1b 246 */
xorjoep 1:24714b45cd1b 247
xorjoep 1:24714b45cd1b 248 outI = *pSrc2 * *(pCoefB + 1);
xorjoep 1:24714b45cd1b 249 outI = outI - (*(pSrc2 + 1) * *pCoefB);
xorjoep 1:24714b45cd1b 250 outI = outI + (*(pSrc1 + 1) * *pCoefA);
xorjoep 1:24714b45cd1b 251 outI = outI + (*pSrc1 * *(pCoefA + 1));
xorjoep 1:24714b45cd1b 252
xorjoep 1:24714b45cd1b 253 /* update input pointers */
xorjoep 1:24714b45cd1b 254 pSrc1 += 2U;
xorjoep 1:24714b45cd1b 255 pSrc2 -= 2U;
xorjoep 1:24714b45cd1b 256
xorjoep 1:24714b45cd1b 257 /* write output */
xorjoep 1:24714b45cd1b 258 pDst[2U * i] = (q15_t) outR;
xorjoep 1:24714b45cd1b 259 pDst[(2U * i) + 1U] = outI >> 16U;
xorjoep 1:24714b45cd1b 260
xorjoep 1:24714b45cd1b 261 /* write complex conjugate output */
xorjoep 1:24714b45cd1b 262 pDst[(4U * fftLen) - (2U * i)] = (q15_t) outR;
xorjoep 1:24714b45cd1b 263 pDst[((4U * fftLen) - (2U * i)) + 1U] = -(outI >> 16U);
xorjoep 1:24714b45cd1b 264
xorjoep 1:24714b45cd1b 265 /* update coefficient pointer */
xorjoep 1:24714b45cd1b 266 pCoefB = pCoefB + (2U * modifier);
xorjoep 1:24714b45cd1b 267 pCoefA = pCoefA + (2U * modifier);
xorjoep 1:24714b45cd1b 268
xorjoep 1:24714b45cd1b 269 i++;
xorjoep 1:24714b45cd1b 270 }
xorjoep 1:24714b45cd1b 271
xorjoep 1:24714b45cd1b 272 pDst[2U * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
xorjoep 1:24714b45cd1b 273 pDst[(2U * fftLen) + 1U] = 0;
xorjoep 1:24714b45cd1b 274
xorjoep 1:24714b45cd1b 275 pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
xorjoep 1:24714b45cd1b 276 pDst[1] = 0;
xorjoep 1:24714b45cd1b 277
xorjoep 1:24714b45cd1b 278 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 279 }
xorjoep 1:24714b45cd1b 280
xorjoep 1:24714b45cd1b 281
xorjoep 1:24714b45cd1b 282 /**
xorjoep 1:24714b45cd1b 283 * @brief Core Real IFFT process
xorjoep 1:24714b45cd1b 284 * @param[in] *pSrc points to the input buffer.
xorjoep 1:24714b45cd1b 285 * @param[in] fftLen length of FFT.
xorjoep 1:24714b45cd1b 286 * @param[in] *pATable points to the twiddle Coef A buffer.
xorjoep 1:24714b45cd1b 287 * @param[in] *pBTable points to the twiddle Coef B buffer.
xorjoep 1:24714b45cd1b 288 * @param[out] *pDst points to the output buffer.
xorjoep 1:24714b45cd1b 289 * @param[in] modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
xorjoep 1:24714b45cd1b 290 * @return none.
xorjoep 1:24714b45cd1b 291 * The function implements a Real IFFT
xorjoep 1:24714b45cd1b 292 */
xorjoep 1:24714b45cd1b 293 void arm_split_rifft_q15(
xorjoep 1:24714b45cd1b 294 q15_t * pSrc,
xorjoep 1:24714b45cd1b 295 uint32_t fftLen,
xorjoep 1:24714b45cd1b 296 q15_t * pATable,
xorjoep 1:24714b45cd1b 297 q15_t * pBTable,
xorjoep 1:24714b45cd1b 298 q15_t * pDst,
xorjoep 1:24714b45cd1b 299 uint32_t modifier)
xorjoep 1:24714b45cd1b 300 {
xorjoep 1:24714b45cd1b 301 uint32_t i; /* Loop Counter */
xorjoep 1:24714b45cd1b 302 q31_t outR, outI; /* Temporary variables for output */
xorjoep 1:24714b45cd1b 303 q15_t *pCoefA, *pCoefB; /* Temporary pointers for twiddle factors */
xorjoep 1:24714b45cd1b 304 q15_t *pSrc1, *pSrc2;
xorjoep 1:24714b45cd1b 305 q15_t *pDst1 = &pDst[0];
xorjoep 1:24714b45cd1b 306
xorjoep 1:24714b45cd1b 307 pCoefA = &pATable[0];
xorjoep 1:24714b45cd1b 308 pCoefB = &pBTable[0];
xorjoep 1:24714b45cd1b 309
xorjoep 1:24714b45cd1b 310 pSrc1 = &pSrc[0];
xorjoep 1:24714b45cd1b 311 pSrc2 = &pSrc[2U * fftLen];
xorjoep 1:24714b45cd1b 312
xorjoep 1:24714b45cd1b 313 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 314
xorjoep 1:24714b45cd1b 315 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 316 i = fftLen;
xorjoep 1:24714b45cd1b 317
xorjoep 1:24714b45cd1b 318 while (i > 0U)
xorjoep 1:24714b45cd1b 319 {
xorjoep 1:24714b45cd1b 320 /*
xorjoep 1:24714b45cd1b 321 outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
xorjoep 1:24714b45cd1b 322 pIn[2 * n - 2 * i] * pBTable[2 * i] -
xorjoep 1:24714b45cd1b 323 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
xorjoep 1:24714b45cd1b 324
xorjoep 1:24714b45cd1b 325 outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
xorjoep 1:24714b45cd1b 326 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
xorjoep 1:24714b45cd1b 327 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
xorjoep 1:24714b45cd1b 328 */
xorjoep 1:24714b45cd1b 329
xorjoep 1:24714b45cd1b 330
xorjoep 1:24714b45cd1b 331 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 332
xorjoep 1:24714b45cd1b 333 /* pIn[2 * n - 2 * i] * pBTable[2 * i] -
xorjoep 1:24714b45cd1b 334 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
xorjoep 1:24714b45cd1b 335 outR = __SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB));
xorjoep 1:24714b45cd1b 336
xorjoep 1:24714b45cd1b 337 #else
xorjoep 1:24714b45cd1b 338
xorjoep 1:24714b45cd1b 339 /* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +
xorjoep 1:24714b45cd1b 340 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
xorjoep 1:24714b45cd1b 341 outR = -(__SMUSD(*__SIMD32(pSrc2), *__SIMD32(pCoefB)));
xorjoep 1:24714b45cd1b 342
xorjoep 1:24714b45cd1b 343 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 344
xorjoep 1:24714b45cd1b 345 /* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
xorjoep 1:24714b45cd1b 346 pIn[2 * n - 2 * i] * pBTable[2 * i] */
xorjoep 1:24714b45cd1b 347 outR = __SMLAD(*__SIMD32(pSrc1), *__SIMD32(pCoefA), outR) >> 16U;
xorjoep 1:24714b45cd1b 348
xorjoep 1:24714b45cd1b 349 /*
xorjoep 1:24714b45cd1b 350 -pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +
xorjoep 1:24714b45cd1b 351 pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
xorjoep 1:24714b45cd1b 352 outI = __SMUADX(*__SIMD32(pSrc2)--, *__SIMD32(pCoefB));
xorjoep 1:24714b45cd1b 353
xorjoep 1:24714b45cd1b 354 /* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
xorjoep 1:24714b45cd1b 355
xorjoep 1:24714b45cd1b 356 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 357
xorjoep 1:24714b45cd1b 358 outI = __SMLSDX(*__SIMD32(pCoefA), *__SIMD32(pSrc1)++, -outI);
xorjoep 1:24714b45cd1b 359
xorjoep 1:24714b45cd1b 360 #else
xorjoep 1:24714b45cd1b 361
xorjoep 1:24714b45cd1b 362 outI = __SMLSDX(*__SIMD32(pSrc1)++, *__SIMD32(pCoefA), -outI);
xorjoep 1:24714b45cd1b 363
xorjoep 1:24714b45cd1b 364 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 365 /* write output */
xorjoep 1:24714b45cd1b 366
xorjoep 1:24714b45cd1b 367 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 368
xorjoep 1:24714b45cd1b 369 *__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 16U), 16);
xorjoep 1:24714b45cd1b 370
xorjoep 1:24714b45cd1b 371 #else
xorjoep 1:24714b45cd1b 372
xorjoep 1:24714b45cd1b 373 *__SIMD32(pDst1)++ = __PKHBT((outI >> 16U), outR, 16);
xorjoep 1:24714b45cd1b 374
xorjoep 1:24714b45cd1b 375 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 376
xorjoep 1:24714b45cd1b 377 /* update coefficient pointer */
xorjoep 1:24714b45cd1b 378 pCoefB = pCoefB + (2U * modifier);
xorjoep 1:24714b45cd1b 379 pCoefA = pCoefA + (2U * modifier);
xorjoep 1:24714b45cd1b 380
xorjoep 1:24714b45cd1b 381 i--;
xorjoep 1:24714b45cd1b 382 }
xorjoep 1:24714b45cd1b 383 #else
xorjoep 1:24714b45cd1b 384 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 385 i = fftLen;
xorjoep 1:24714b45cd1b 386
xorjoep 1:24714b45cd1b 387 while (i > 0U)
xorjoep 1:24714b45cd1b 388 {
xorjoep 1:24714b45cd1b 389 /*
xorjoep 1:24714b45cd1b 390 outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
xorjoep 1:24714b45cd1b 391 pIn[2 * n - 2 * i] * pBTable[2 * i] -
xorjoep 1:24714b45cd1b 392 pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
xorjoep 1:24714b45cd1b 393 */
xorjoep 1:24714b45cd1b 394
xorjoep 1:24714b45cd1b 395 outR = *pSrc2 * *pCoefB;
xorjoep 1:24714b45cd1b 396 outR = outR - (*(pSrc2 + 1) * *(pCoefB + 1));
xorjoep 1:24714b45cd1b 397 outR = outR + (*pSrc1 * *pCoefA);
xorjoep 1:24714b45cd1b 398 outR = (outR + (*(pSrc1 + 1) * *(pCoefA + 1))) >> 16;
xorjoep 1:24714b45cd1b 399
xorjoep 1:24714b45cd1b 400 /*
xorjoep 1:24714b45cd1b 401 outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
xorjoep 1:24714b45cd1b 402 pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
xorjoep 1:24714b45cd1b 403 pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
xorjoep 1:24714b45cd1b 404 */
xorjoep 1:24714b45cd1b 405
xorjoep 1:24714b45cd1b 406 outI = *(pSrc1 + 1) * *pCoefA;
xorjoep 1:24714b45cd1b 407 outI = outI - (*pSrc1 * *(pCoefA + 1));
xorjoep 1:24714b45cd1b 408 outI = outI - (*pSrc2 * *(pCoefB + 1));
xorjoep 1:24714b45cd1b 409 outI = outI - (*(pSrc2 + 1) * *(pCoefB));
xorjoep 1:24714b45cd1b 410
xorjoep 1:24714b45cd1b 411 /* update input pointers */
xorjoep 1:24714b45cd1b 412 pSrc1 += 2U;
xorjoep 1:24714b45cd1b 413 pSrc2 -= 2U;
xorjoep 1:24714b45cd1b 414
xorjoep 1:24714b45cd1b 415 /* write output */
xorjoep 1:24714b45cd1b 416 *pDst1++ = (q15_t) outR;
xorjoep 1:24714b45cd1b 417 *pDst1++ = (q15_t) (outI >> 16);
xorjoep 1:24714b45cd1b 418
xorjoep 1:24714b45cd1b 419 /* update coefficient pointer */
xorjoep 1:24714b45cd1b 420 pCoefB = pCoefB + (2U * modifier);
xorjoep 1:24714b45cd1b 421 pCoefA = pCoefA + (2U * modifier);
xorjoep 1:24714b45cd1b 422
xorjoep 1:24714b45cd1b 423 i--;
xorjoep 1:24714b45cd1b 424 }
xorjoep 1:24714b45cd1b 425 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 426 }