mbed official / mbed-dsp

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 mbed official

cmsis_dsp/TransformFunctions/arm_cfft_radix2_q15.c@2:da51fb522205, 2013-05-30 (annotated)

Committer:
emilmont
Date:
Thu May 30 17:10:11 2013 +0100
Revision:
2:da51fb522205
Parent:
1:fdd22bb7aa52
Child:
3:7a284390b0ce
Keep "cmsis-dsp" module in synch with its source

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
emilmont 1:fdd22bb7aa52 2 * Copyright (C) 2010 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
emilmont 1:fdd22bb7aa52 4 * $Date: 15. February 2012
emilmont 2:da51fb522205 5 * $Revision: V1.1.0
emilmont 1:fdd22bb7aa52 6 *
emilmont 2:da51fb522205 7 * Project: CMSIS DSP Library
emilmont 2:da51fb522205 8 * Title: arm_cfft_radix2_q15.c
emilmont 1:fdd22bb7aa52 9 *
emilmont 2:da51fb522205 10 * Description: Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function
emilmont 1:fdd22bb7aa52 11 *
emilmont 1:fdd22bb7aa52 12 *
emilmont 1:fdd22bb7aa52 13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
emilmont 1:fdd22bb7aa52 14 *
emilmont 1:fdd22bb7aa52 15 * Version 1.1.0 2012/02/15
emilmont 1:fdd22bb7aa52 16 * Updated with more optimizations, bug fixes and minor API changes.
emilmont 1:fdd22bb7aa52 17 *
emilmont 1:fdd22bb7aa52 18 * Version 0.0.3 2010/03/10
emilmont 1:fdd22bb7aa52 19 * Initial version
emilmont 1:fdd22bb7aa52 20 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 21
emilmont 1:fdd22bb7aa52 22 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 23
emilmont 1:fdd22bb7aa52 24 /**
emilmont 1:fdd22bb7aa52 25 * @ingroup groupTransforms
emilmont 1:fdd22bb7aa52 26 */
emilmont 1:fdd22bb7aa52 27
emilmont 1:fdd22bb7aa52 28 /**
emilmont 1:fdd22bb7aa52 29 * @defgroup Radix2_CFFT_CIFFT Radix-2 Complex FFT Functions
emilmont 1:fdd22bb7aa52 30 *
emilmont 1:fdd22bb7aa52 31 * \par
emilmont 1:fdd22bb7aa52 32 * Complex Fast Fourier Transform(CFFT) and Complex Inverse Fast Fourier Transform(CIFFT) is an efficient algorithm to compute Discrete Fourier Transform(DFT) and Inverse Discrete Fourier Transform(IDFT).
emilmont 1:fdd22bb7aa52 33 * Computational complexity of CFFT reduces drastically when compared to DFT.
emilmont 1:fdd22bb7aa52 34 */
emilmont 1:fdd22bb7aa52 35
emilmont 1:fdd22bb7aa52 36
emilmont 1:fdd22bb7aa52 37 /**
emilmont 1:fdd22bb7aa52 38 * @addtogroup Radix2_CFFT_CIFFT
emilmont 1:fdd22bb7aa52 39 * @{
emilmont 1:fdd22bb7aa52 40 */
emilmont 1:fdd22bb7aa52 41
emilmont 1:fdd22bb7aa52 42 /**
emilmont 1:fdd22bb7aa52 43 * @details
emilmont 1:fdd22bb7aa52 44 * @brief Processing function for the fixed-point CFFT/CIFFT.
emilmont 1:fdd22bb7aa52 45 * @param[in] *S points to an instance of the fixed-point CFFT/CIFFT structure.
emilmont 1:fdd22bb7aa52 46 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
emilmont 1:fdd22bb7aa52 47 * @return none.
emilmont 1:fdd22bb7aa52 48 */
emilmont 1:fdd22bb7aa52 49
emilmont 1:fdd22bb7aa52 50 void arm_cfft_radix2_q15(
emilmont 1:fdd22bb7aa52 51 const arm_cfft_radix2_instance_q15 * S,
emilmont 1:fdd22bb7aa52 52 q15_t * pSrc)
emilmont 1:fdd22bb7aa52 53 {
emilmont 1:fdd22bb7aa52 54
emilmont 1:fdd22bb7aa52 55 if(S->ifftFlag == 1u)
emilmont 1:fdd22bb7aa52 56 {
emilmont 1:fdd22bb7aa52 57 arm_radix2_butterfly_inverse_q15(pSrc, S->fftLen,
emilmont 1:fdd22bb7aa52 58 S->pTwiddle, S->twidCoefModifier);
emilmont 1:fdd22bb7aa52 59 }
emilmont 1:fdd22bb7aa52 60 else
emilmont 1:fdd22bb7aa52 61 {
emilmont 1:fdd22bb7aa52 62 arm_radix2_butterfly_q15(pSrc, S->fftLen,
emilmont 1:fdd22bb7aa52 63 S->pTwiddle, S->twidCoefModifier);
emilmont 1:fdd22bb7aa52 64 }
emilmont 1:fdd22bb7aa52 65
emilmont 1:fdd22bb7aa52 66 arm_bitreversal_q15(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
emilmont 1:fdd22bb7aa52 67 }
emilmont 1:fdd22bb7aa52 68
emilmont 1:fdd22bb7aa52 69 /**
emilmont 1:fdd22bb7aa52 70 * @} end of Radix2_CFFT_CIFFT group
emilmont 1:fdd22bb7aa52 71 */
emilmont 1:fdd22bb7aa52 72
emilmont 1:fdd22bb7aa52 73 void arm_radix2_butterfly_q15(
emilmont 1:fdd22bb7aa52 74 q15_t * pSrc,
emilmont 1:fdd22bb7aa52 75 uint32_t fftLen,
emilmont 1:fdd22bb7aa52 76 q15_t * pCoef,
emilmont 1:fdd22bb7aa52 77 uint16_t twidCoefModifier)
emilmont 1:fdd22bb7aa52 78 {
emilmont 1:fdd22bb7aa52 79 #ifndef ARM_MATH_CM0
emilmont 1:fdd22bb7aa52 80
emilmont 1:fdd22bb7aa52 81 int i, j, k, l;
emilmont 1:fdd22bb7aa52 82 int n1, n2, ia;
emilmont 1:fdd22bb7aa52 83 q15_t in;
emilmont 1:fdd22bb7aa52 84 q31_t T, S, R;
emilmont 1:fdd22bb7aa52 85 q31_t coeff, out1, out2;
emilmont 1:fdd22bb7aa52 86
emilmont 1:fdd22bb7aa52 87 //N = fftLen;
emilmont 1:fdd22bb7aa52 88 n2 = fftLen;
emilmont 1:fdd22bb7aa52 89
emilmont 1:fdd22bb7aa52 90 n1 = n2;
emilmont 1:fdd22bb7aa52 91 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 92 ia = 0;
emilmont 1:fdd22bb7aa52 93
emilmont 1:fdd22bb7aa52 94 // loop for groups
emilmont 1:fdd22bb7aa52 95 for (i = 0; i < n2; i++)
emilmont 1:fdd22bb7aa52 96 {
emilmont 1:fdd22bb7aa52 97 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 98
emilmont 1:fdd22bb7aa52 99 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 100
emilmont 1:fdd22bb7aa52 101 l = i + n2;
emilmont 1:fdd22bb7aa52 102
emilmont 1:fdd22bb7aa52 103 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 104 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 105 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 106
emilmont 1:fdd22bb7aa52 107 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 108 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 109 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 110
emilmont 1:fdd22bb7aa52 111 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 112
emilmont 1:fdd22bb7aa52 113 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 114
emilmont 1:fdd22bb7aa52 115 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 116
emilmont 1:fdd22bb7aa52 117 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 118 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 119
emilmont 1:fdd22bb7aa52 120 #else
emilmont 1:fdd22bb7aa52 121
emilmont 1:fdd22bb7aa52 122 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 123 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 124
emilmont 1:fdd22bb7aa52 125 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 126
emilmont 1:fdd22bb7aa52 127 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 128 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 129
emilmont 1:fdd22bb7aa52 130 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 131
emilmont 1:fdd22bb7aa52 132 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 133
emilmont 1:fdd22bb7aa52 134 // loop for butterfly
emilmont 1:fdd22bb7aa52 135 i++;
emilmont 1:fdd22bb7aa52 136 l++;
emilmont 1:fdd22bb7aa52 137
emilmont 1:fdd22bb7aa52 138 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 139 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 140 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 141
emilmont 1:fdd22bb7aa52 142 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 143 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 144 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 145
emilmont 1:fdd22bb7aa52 146 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 147
emilmont 1:fdd22bb7aa52 148 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 149
emilmont 1:fdd22bb7aa52 150 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 151
emilmont 1:fdd22bb7aa52 152 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 153 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 154
emilmont 1:fdd22bb7aa52 155 #else
emilmont 1:fdd22bb7aa52 156
emilmont 1:fdd22bb7aa52 157 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 158 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 159
emilmont 1:fdd22bb7aa52 160 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 161
emilmont 1:fdd22bb7aa52 162 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 163 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 164
emilmont 1:fdd22bb7aa52 165 } // groups loop end
emilmont 1:fdd22bb7aa52 166
emilmont 1:fdd22bb7aa52 167 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 168
emilmont 1:fdd22bb7aa52 169 // loop for stage
emilmont 1:fdd22bb7aa52 170 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 171 {
emilmont 1:fdd22bb7aa52 172 n1 = n2;
emilmont 1:fdd22bb7aa52 173 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 174 ia = 0;
emilmont 1:fdd22bb7aa52 175
emilmont 1:fdd22bb7aa52 176 // loop for groups
emilmont 1:fdd22bb7aa52 177 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 178 {
emilmont 1:fdd22bb7aa52 179 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 180
emilmont 1:fdd22bb7aa52 181 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 182
emilmont 1:fdd22bb7aa52 183 // loop for butterfly
emilmont 1:fdd22bb7aa52 184 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 185 {
emilmont 1:fdd22bb7aa52 186 l = i + n2;
emilmont 1:fdd22bb7aa52 187
emilmont 1:fdd22bb7aa52 188 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 189
emilmont 1:fdd22bb7aa52 190 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 191
emilmont 1:fdd22bb7aa52 192 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 193
emilmont 1:fdd22bb7aa52 194 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 195
emilmont 1:fdd22bb7aa52 196 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 197
emilmont 1:fdd22bb7aa52 198 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 199 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 200
emilmont 1:fdd22bb7aa52 201 #else
emilmont 1:fdd22bb7aa52 202
emilmont 1:fdd22bb7aa52 203 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 204 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 205
emilmont 1:fdd22bb7aa52 206 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 207
emilmont 1:fdd22bb7aa52 208 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 209 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 210
emilmont 1:fdd22bb7aa52 211 i += n1;
emilmont 1:fdd22bb7aa52 212
emilmont 1:fdd22bb7aa52 213 l = i + n2;
emilmont 1:fdd22bb7aa52 214
emilmont 1:fdd22bb7aa52 215 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 216
emilmont 1:fdd22bb7aa52 217 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 218
emilmont 1:fdd22bb7aa52 219 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 220
emilmont 1:fdd22bb7aa52 221 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 222
emilmont 1:fdd22bb7aa52 223 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 224
emilmont 1:fdd22bb7aa52 225 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 226 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 227
emilmont 1:fdd22bb7aa52 228 #else
emilmont 1:fdd22bb7aa52 229
emilmont 1:fdd22bb7aa52 230 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 231 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 232
emilmont 1:fdd22bb7aa52 233 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 234
emilmont 1:fdd22bb7aa52 235 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 236 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 237
emilmont 1:fdd22bb7aa52 238 } // butterfly loop end
emilmont 1:fdd22bb7aa52 239
emilmont 1:fdd22bb7aa52 240 } // groups loop end
emilmont 1:fdd22bb7aa52 241
emilmont 1:fdd22bb7aa52 242 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 243 } // stages loop end
emilmont 1:fdd22bb7aa52 244
emilmont 1:fdd22bb7aa52 245 n1 = n2;
emilmont 1:fdd22bb7aa52 246 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 247 ia = 0;
emilmont 1:fdd22bb7aa52 248
emilmont 1:fdd22bb7aa52 249 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 250
emilmont 1:fdd22bb7aa52 251 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 252
emilmont 1:fdd22bb7aa52 253 // loop for butterfly
emilmont 1:fdd22bb7aa52 254 for (i = 0; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 255 {
emilmont 1:fdd22bb7aa52 256 l = i + n2;
emilmont 1:fdd22bb7aa52 257
emilmont 1:fdd22bb7aa52 258 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 259
emilmont 1:fdd22bb7aa52 260 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 261
emilmont 1:fdd22bb7aa52 262 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 263
emilmont 1:fdd22bb7aa52 264 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 265
emilmont 1:fdd22bb7aa52 266 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 267
emilmont 1:fdd22bb7aa52 268 i += n1;
emilmont 1:fdd22bb7aa52 269 l = i + n2;
emilmont 1:fdd22bb7aa52 270
emilmont 1:fdd22bb7aa52 271 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 272
emilmont 1:fdd22bb7aa52 273 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 274
emilmont 1:fdd22bb7aa52 275 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 276
emilmont 1:fdd22bb7aa52 277 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 278
emilmont 1:fdd22bb7aa52 279 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 280
emilmont 1:fdd22bb7aa52 281 } // groups loop end
emilmont 1:fdd22bb7aa52 282
emilmont 1:fdd22bb7aa52 283
emilmont 1:fdd22bb7aa52 284 #else
emilmont 1:fdd22bb7aa52 285
emilmont 1:fdd22bb7aa52 286 int i, j, k, l;
emilmont 1:fdd22bb7aa52 287 int n1, n2, ia;
emilmont 1:fdd22bb7aa52 288 q15_t xt, yt, cosVal, sinVal;
emilmont 1:fdd22bb7aa52 289
emilmont 1:fdd22bb7aa52 290
emilmont 1:fdd22bb7aa52 291 //N = fftLen;
emilmont 1:fdd22bb7aa52 292 n2 = fftLen;
emilmont 1:fdd22bb7aa52 293
emilmont 1:fdd22bb7aa52 294 n1 = n2;
emilmont 1:fdd22bb7aa52 295 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 296 ia = 0;
emilmont 1:fdd22bb7aa52 297
emilmont 1:fdd22bb7aa52 298 // loop for groups
emilmont 1:fdd22bb7aa52 299 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 300 {
emilmont 1:fdd22bb7aa52 301 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 302 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 303 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 304
emilmont 1:fdd22bb7aa52 305 // loop for butterfly
emilmont 1:fdd22bb7aa52 306 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 307 {
emilmont 1:fdd22bb7aa52 308 l = i + n2;
emilmont 1:fdd22bb7aa52 309 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
emilmont 1:fdd22bb7aa52 310 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 311
emilmont 1:fdd22bb7aa52 312 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
emilmont 1:fdd22bb7aa52 313 pSrc[2 * i + 1] =
emilmont 1:fdd22bb7aa52 314 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 315
emilmont 1:fdd22bb7aa52 316 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 317 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 318
emilmont 1:fdd22bb7aa52 319 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 320 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 321
emilmont 1:fdd22bb7aa52 322 } // butterfly loop end
emilmont 1:fdd22bb7aa52 323
emilmont 1:fdd22bb7aa52 324 } // groups loop end
emilmont 1:fdd22bb7aa52 325
emilmont 1:fdd22bb7aa52 326 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 327
emilmont 1:fdd22bb7aa52 328 // loop for stage
emilmont 1:fdd22bb7aa52 329 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 330 {
emilmont 1:fdd22bb7aa52 331 n1 = n2;
emilmont 1:fdd22bb7aa52 332 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 333 ia = 0;
emilmont 1:fdd22bb7aa52 334
emilmont 1:fdd22bb7aa52 335 // loop for groups
emilmont 1:fdd22bb7aa52 336 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 337 {
emilmont 1:fdd22bb7aa52 338 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 339 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 340 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 341
emilmont 1:fdd22bb7aa52 342 // loop for butterfly
emilmont 1:fdd22bb7aa52 343 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 344 {
emilmont 1:fdd22bb7aa52 345 l = i + n2;
emilmont 1:fdd22bb7aa52 346 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 347 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
emilmont 1:fdd22bb7aa52 348
emilmont 1:fdd22bb7aa52 349 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 350 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
emilmont 1:fdd22bb7aa52 351
emilmont 1:fdd22bb7aa52 352 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 353 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 354
emilmont 1:fdd22bb7aa52 355 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 356 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 357
emilmont 1:fdd22bb7aa52 358 } // butterfly loop end
emilmont 1:fdd22bb7aa52 359
emilmont 1:fdd22bb7aa52 360 } // groups loop end
emilmont 1:fdd22bb7aa52 361
emilmont 1:fdd22bb7aa52 362 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 363 } // stages loop end
emilmont 1:fdd22bb7aa52 364
emilmont 1:fdd22bb7aa52 365 n1 = n2;
emilmont 1:fdd22bb7aa52 366 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 367 ia = 0;
emilmont 1:fdd22bb7aa52 368
emilmont 1:fdd22bb7aa52 369 // loop for groups
emilmont 1:fdd22bb7aa52 370 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 371 {
emilmont 1:fdd22bb7aa52 372 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 373 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 374
emilmont 1:fdd22bb7aa52 375 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 376
emilmont 1:fdd22bb7aa52 377 // loop for butterfly
emilmont 1:fdd22bb7aa52 378 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 379 {
emilmont 1:fdd22bb7aa52 380 l = i + n2;
emilmont 1:fdd22bb7aa52 381 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 382 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
emilmont 1:fdd22bb7aa52 383
emilmont 1:fdd22bb7aa52 384 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 385 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
emilmont 1:fdd22bb7aa52 386
emilmont 1:fdd22bb7aa52 387 pSrc[2u * l] = xt;
emilmont 1:fdd22bb7aa52 388
emilmont 1:fdd22bb7aa52 389 pSrc[2u * l + 1u] = yt;
emilmont 1:fdd22bb7aa52 390
emilmont 1:fdd22bb7aa52 391 } // butterfly loop end
emilmont 1:fdd22bb7aa52 392
emilmont 1:fdd22bb7aa52 393 } // groups loop end
emilmont 1:fdd22bb7aa52 394
emilmont 1:fdd22bb7aa52 395 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 396
emilmont 1:fdd22bb7aa52 397 #endif // #ifndef ARM_MATH_CM0
emilmont 1:fdd22bb7aa52 398
emilmont 1:fdd22bb7aa52 399 }
emilmont 1:fdd22bb7aa52 400
emilmont 1:fdd22bb7aa52 401
emilmont 1:fdd22bb7aa52 402 void arm_radix2_butterfly_inverse_q15(
emilmont 1:fdd22bb7aa52 403 q15_t * pSrc,
emilmont 1:fdd22bb7aa52 404 uint32_t fftLen,
emilmont 1:fdd22bb7aa52 405 q15_t * pCoef,
emilmont 1:fdd22bb7aa52 406 uint16_t twidCoefModifier)
emilmont 1:fdd22bb7aa52 407 {
emilmont 1:fdd22bb7aa52 408 #ifndef ARM_MATH_CM0
emilmont 1:fdd22bb7aa52 409
emilmont 1:fdd22bb7aa52 410 int i, j, k, l;
emilmont 1:fdd22bb7aa52 411 int n1, n2, ia;
emilmont 1:fdd22bb7aa52 412 q15_t in;
emilmont 1:fdd22bb7aa52 413 q31_t T, S, R;
emilmont 1:fdd22bb7aa52 414 q31_t coeff, out1, out2;
emilmont 1:fdd22bb7aa52 415
emilmont 1:fdd22bb7aa52 416 //N = fftLen;
emilmont 1:fdd22bb7aa52 417 n2 = fftLen;
emilmont 1:fdd22bb7aa52 418
emilmont 1:fdd22bb7aa52 419 n1 = n2;
emilmont 1:fdd22bb7aa52 420 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 421 ia = 0;
emilmont 1:fdd22bb7aa52 422
emilmont 1:fdd22bb7aa52 423 // loop for groups
emilmont 1:fdd22bb7aa52 424 for (i = 0; i < n2; i++)
emilmont 1:fdd22bb7aa52 425 {
emilmont 1:fdd22bb7aa52 426 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 427
emilmont 1:fdd22bb7aa52 428 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 429
emilmont 1:fdd22bb7aa52 430 l = i + n2;
emilmont 1:fdd22bb7aa52 431
emilmont 1:fdd22bb7aa52 432 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 433 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 434 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 435
emilmont 1:fdd22bb7aa52 436 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 437 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 438 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 439
emilmont 1:fdd22bb7aa52 440 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 441
emilmont 1:fdd22bb7aa52 442 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 443
emilmont 1:fdd22bb7aa52 444 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 445
emilmont 1:fdd22bb7aa52 446 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 447 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 448 #else
emilmont 1:fdd22bb7aa52 449
emilmont 1:fdd22bb7aa52 450 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 451 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 452
emilmont 1:fdd22bb7aa52 453 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 454
emilmont 1:fdd22bb7aa52 455 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 456 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 457
emilmont 1:fdd22bb7aa52 458 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 459
emilmont 1:fdd22bb7aa52 460 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 461
emilmont 1:fdd22bb7aa52 462 // loop for butterfly
emilmont 1:fdd22bb7aa52 463 i++;
emilmont 1:fdd22bb7aa52 464 l++;
emilmont 1:fdd22bb7aa52 465
emilmont 1:fdd22bb7aa52 466 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 467 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 468 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 469
emilmont 1:fdd22bb7aa52 470 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 471 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 472 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 473
emilmont 1:fdd22bb7aa52 474 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 475
emilmont 1:fdd22bb7aa52 476 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 477
emilmont 1:fdd22bb7aa52 478 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 479
emilmont 1:fdd22bb7aa52 480 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 481 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 482 #else
emilmont 1:fdd22bb7aa52 483
emilmont 1:fdd22bb7aa52 484 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 485 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 486
emilmont 1:fdd22bb7aa52 487 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 488
emilmont 1:fdd22bb7aa52 489 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 490 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 491
emilmont 1:fdd22bb7aa52 492 } // groups loop end
emilmont 1:fdd22bb7aa52 493
emilmont 1:fdd22bb7aa52 494 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 495
emilmont 1:fdd22bb7aa52 496 // loop for stage
emilmont 1:fdd22bb7aa52 497 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 498 {
emilmont 1:fdd22bb7aa52 499 n1 = n2;
emilmont 1:fdd22bb7aa52 500 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 501 ia = 0;
emilmont 1:fdd22bb7aa52 502
emilmont 1:fdd22bb7aa52 503 // loop for groups
emilmont 1:fdd22bb7aa52 504 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 505 {
emilmont 1:fdd22bb7aa52 506 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 507
emilmont 1:fdd22bb7aa52 508 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 509
emilmont 1:fdd22bb7aa52 510 // loop for butterfly
emilmont 1:fdd22bb7aa52 511 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 512 {
emilmont 1:fdd22bb7aa52 513 l = i + n2;
emilmont 1:fdd22bb7aa52 514
emilmont 1:fdd22bb7aa52 515 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 516
emilmont 1:fdd22bb7aa52 517 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 518
emilmont 1:fdd22bb7aa52 519 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 520
emilmont 1:fdd22bb7aa52 521 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 522
emilmont 1:fdd22bb7aa52 523 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 524
emilmont 1:fdd22bb7aa52 525 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 526 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 527
emilmont 1:fdd22bb7aa52 528 #else
emilmont 1:fdd22bb7aa52 529
emilmont 1:fdd22bb7aa52 530 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 531 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 532
emilmont 1:fdd22bb7aa52 533 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 534
emilmont 1:fdd22bb7aa52 535 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 536 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 537
emilmont 1:fdd22bb7aa52 538 i += n1;
emilmont 1:fdd22bb7aa52 539
emilmont 1:fdd22bb7aa52 540 l = i + n2;
emilmont 1:fdd22bb7aa52 541
emilmont 1:fdd22bb7aa52 542 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 543
emilmont 1:fdd22bb7aa52 544 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 545
emilmont 1:fdd22bb7aa52 546 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 547
emilmont 1:fdd22bb7aa52 548 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 549
emilmont 1:fdd22bb7aa52 550 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 551
emilmont 1:fdd22bb7aa52 552 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 553 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 554 #else
emilmont 1:fdd22bb7aa52 555
emilmont 1:fdd22bb7aa52 556 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 557 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 558
emilmont 1:fdd22bb7aa52 559 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 560
emilmont 1:fdd22bb7aa52 561 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 562 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 563
emilmont 1:fdd22bb7aa52 564 } // butterfly loop end
emilmont 1:fdd22bb7aa52 565
emilmont 1:fdd22bb7aa52 566 } // groups loop end
emilmont 1:fdd22bb7aa52 567
emilmont 1:fdd22bb7aa52 568 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 569 } // stages loop end
emilmont 1:fdd22bb7aa52 570
emilmont 1:fdd22bb7aa52 571 n1 = n2;
emilmont 1:fdd22bb7aa52 572 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 573 ia = 0;
emilmont 1:fdd22bb7aa52 574
emilmont 1:fdd22bb7aa52 575 // loop for groups
emilmont 1:fdd22bb7aa52 576 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 577 {
emilmont 1:fdd22bb7aa52 578 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 579
emilmont 1:fdd22bb7aa52 580 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 581
emilmont 1:fdd22bb7aa52 582 // loop for butterfly
emilmont 1:fdd22bb7aa52 583 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 584 {
emilmont 1:fdd22bb7aa52 585 l = i + n2;
emilmont 1:fdd22bb7aa52 586
emilmont 1:fdd22bb7aa52 587 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 588
emilmont 1:fdd22bb7aa52 589 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 590
emilmont 1:fdd22bb7aa52 591 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 592
emilmont 1:fdd22bb7aa52 593 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 594
emilmont 1:fdd22bb7aa52 595 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 596
emilmont 1:fdd22bb7aa52 597 } // butterfly loop end
emilmont 1:fdd22bb7aa52 598
emilmont 1:fdd22bb7aa52 599 } // groups loop end
emilmont 1:fdd22bb7aa52 600
emilmont 1:fdd22bb7aa52 601 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 602
emilmont 1:fdd22bb7aa52 603 #else
emilmont 1:fdd22bb7aa52 604
emilmont 1:fdd22bb7aa52 605
emilmont 1:fdd22bb7aa52 606 int i, j, k, l;
emilmont 1:fdd22bb7aa52 607 int n1, n2, ia;
emilmont 1:fdd22bb7aa52 608 q15_t xt, yt, cosVal, sinVal;
emilmont 1:fdd22bb7aa52 609
emilmont 1:fdd22bb7aa52 610 //N = fftLen;
emilmont 1:fdd22bb7aa52 611 n2 = fftLen;
emilmont 1:fdd22bb7aa52 612
emilmont 1:fdd22bb7aa52 613 n1 = n2;
emilmont 1:fdd22bb7aa52 614 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 615 ia = 0;
emilmont 1:fdd22bb7aa52 616
emilmont 1:fdd22bb7aa52 617 // loop for groups
emilmont 1:fdd22bb7aa52 618 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 619 {
emilmont 1:fdd22bb7aa52 620 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 621 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 622 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 623
emilmont 1:fdd22bb7aa52 624 // loop for butterfly
emilmont 1:fdd22bb7aa52 625 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 626 {
emilmont 1:fdd22bb7aa52 627 l = i + n2;
emilmont 1:fdd22bb7aa52 628 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
emilmont 1:fdd22bb7aa52 629 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 630
emilmont 1:fdd22bb7aa52 631 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
emilmont 1:fdd22bb7aa52 632 pSrc[2 * i + 1] =
emilmont 1:fdd22bb7aa52 633 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 634
emilmont 1:fdd22bb7aa52 635 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 636 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 637
emilmont 1:fdd22bb7aa52 638 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 639 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 640
emilmont 1:fdd22bb7aa52 641 } // butterfly loop end
emilmont 1:fdd22bb7aa52 642
emilmont 1:fdd22bb7aa52 643 } // groups loop end
emilmont 1:fdd22bb7aa52 644
emilmont 1:fdd22bb7aa52 645 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 646
emilmont 1:fdd22bb7aa52 647 // loop for stage
emilmont 1:fdd22bb7aa52 648 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 649 {
emilmont 1:fdd22bb7aa52 650 n1 = n2;
emilmont 1:fdd22bb7aa52 651 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 652 ia = 0;
emilmont 1:fdd22bb7aa52 653
emilmont 1:fdd22bb7aa52 654 // loop for groups
emilmont 1:fdd22bb7aa52 655 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 656 {
emilmont 1:fdd22bb7aa52 657 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 658 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 659 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 660
emilmont 1:fdd22bb7aa52 661 // loop for butterfly
emilmont 1:fdd22bb7aa52 662 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 663 {
emilmont 1:fdd22bb7aa52 664 l = i + n2;
emilmont 1:fdd22bb7aa52 665 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 666 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
emilmont 1:fdd22bb7aa52 667
emilmont 1:fdd22bb7aa52 668 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 669 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
emilmont 1:fdd22bb7aa52 670
emilmont 1:fdd22bb7aa52 671 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 672 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 673
emilmont 1:fdd22bb7aa52 674 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 675 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 676
emilmont 1:fdd22bb7aa52 677 } // butterfly loop end
emilmont 1:fdd22bb7aa52 678
emilmont 1:fdd22bb7aa52 679 } // groups loop end
emilmont 1:fdd22bb7aa52 680
emilmont 1:fdd22bb7aa52 681 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 682 } // stages loop end
emilmont 1:fdd22bb7aa52 683
emilmont 1:fdd22bb7aa52 684 n1 = n2;
emilmont 1:fdd22bb7aa52 685 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 686 ia = 0;
emilmont 1:fdd22bb7aa52 687
emilmont 1:fdd22bb7aa52 688 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 689 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 690
emilmont 1:fdd22bb7aa52 691 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 692
emilmont 1:fdd22bb7aa52 693 // loop for butterfly
emilmont 1:fdd22bb7aa52 694 for (i = 0; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 695 {
emilmont 1:fdd22bb7aa52 696 l = i + n2;
emilmont 1:fdd22bb7aa52 697 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 698 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
emilmont 1:fdd22bb7aa52 699
emilmont 1:fdd22bb7aa52 700 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 701 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
emilmont 1:fdd22bb7aa52 702
emilmont 1:fdd22bb7aa52 703 pSrc[2u * l] = xt;
emilmont 1:fdd22bb7aa52 704
emilmont 1:fdd22bb7aa52 705 pSrc[2u * l + 1u] = yt;
emilmont 1:fdd22bb7aa52 706
emilmont 1:fdd22bb7aa52 707 } // groups loop end
emilmont 1:fdd22bb7aa52 708
emilmont 1:fdd22bb7aa52 709
emilmont 1:fdd22bb7aa52 710 #endif // #ifndef ARM_MATH_CM0
emilmont 1:fdd22bb7aa52 711
emilmont 1:fdd22bb7aa52 712 }