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@3:7a284390b0ce, 2013-11-08 (annotated)

Committer:
mbed_official
Date:
Fri Nov 08 13:45:10 2013 +0000
Revision:
3:7a284390b0ce
Parent:
2:da51fb522205 
Synchronized with git revision e69956aba2f68a2a26ac26b051f8d349deaa1ce8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
mbed_official 3:7a284390b0ce 2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
mbed_official 3:7a284390b0ce 4 * $Date: 17. January 2013
mbed_official 3:7a284390b0ce 5 * $Revision: V1.4.1
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 *
mbed_official 3:7a284390b0ce 15 * Redistribution and use in source and binary forms, with or without
mbed_official 3:7a284390b0ce 16 * modification, are permitted provided that the following conditions
mbed_official 3:7a284390b0ce 17 * are met:
mbed_official 3:7a284390b0ce 18 * - Redistributions of source code must retain the above copyright
mbed_official 3:7a284390b0ce 19 * notice, this list of conditions and the following disclaimer.
mbed_official 3:7a284390b0ce 20 * - Redistributions in binary form must reproduce the above copyright
mbed_official 3:7a284390b0ce 21 * notice, this list of conditions and the following disclaimer in
mbed_official 3:7a284390b0ce 22 * the documentation and/or other materials provided with the
mbed_official 3:7a284390b0ce 23 * distribution.
mbed_official 3:7a284390b0ce 24 * - Neither the name of ARM LIMITED nor the names of its contributors
mbed_official 3:7a284390b0ce 25 * may be used to endorse or promote products derived from this
mbed_official 3:7a284390b0ce 26 * software without specific prior written permission.
mbed_official 3:7a284390b0ce 27 *
mbed_official 3:7a284390b0ce 28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
mbed_official 3:7a284390b0ce 29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
mbed_official 3:7a284390b0ce 30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
mbed_official 3:7a284390b0ce 31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
mbed_official 3:7a284390b0ce 32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
mbed_official 3:7a284390b0ce 33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
mbed_official 3:7a284390b0ce 34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
mbed_official 3:7a284390b0ce 35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
mbed_official 3:7a284390b0ce 36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
mbed_official 3:7a284390b0ce 37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
mbed_official 3:7a284390b0ce 38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 3:7a284390b0ce 39 * POSSIBILITY OF SUCH DAMAGE.
emilmont 1:fdd22bb7aa52 40 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 41
emilmont 1:fdd22bb7aa52 42 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 43
mbed_official 3:7a284390b0ce 44 void arm_radix2_butterfly_q15(
mbed_official 3:7a284390b0ce 45 q15_t * pSrc,
mbed_official 3:7a284390b0ce 46 uint32_t fftLen,
mbed_official 3:7a284390b0ce 47 q15_t * pCoef,
mbed_official 3:7a284390b0ce 48 uint16_t twidCoefModifier);
mbed_official 3:7a284390b0ce 49
mbed_official 3:7a284390b0ce 50 void arm_radix2_butterfly_inverse_q15(
mbed_official 3:7a284390b0ce 51 q15_t * pSrc,
mbed_official 3:7a284390b0ce 52 uint32_t fftLen,
mbed_official 3:7a284390b0ce 53 q15_t * pCoef,
mbed_official 3:7a284390b0ce 54 uint16_t twidCoefModifier);
mbed_official 3:7a284390b0ce 55
mbed_official 3:7a284390b0ce 56 void arm_bitreversal_q15(
mbed_official 3:7a284390b0ce 57 q15_t * pSrc,
mbed_official 3:7a284390b0ce 58 uint32_t fftLen,
mbed_official 3:7a284390b0ce 59 uint16_t bitRevFactor,
mbed_official 3:7a284390b0ce 60 uint16_t * pBitRevTab);
mbed_official 3:7a284390b0ce 61
emilmont 1:fdd22bb7aa52 62 /**
emilmont 1:fdd22bb7aa52 63 * @ingroup groupTransforms
emilmont 1:fdd22bb7aa52 64 */
emilmont 1:fdd22bb7aa52 65
emilmont 1:fdd22bb7aa52 66 /**
mbed_official 3:7a284390b0ce 67 * @addtogroup ComplexFFT
emilmont 1:fdd22bb7aa52 68 * @{
emilmont 1:fdd22bb7aa52 69 */
emilmont 1:fdd22bb7aa52 70
emilmont 1:fdd22bb7aa52 71 /**
emilmont 1:fdd22bb7aa52 72 * @details
emilmont 1:fdd22bb7aa52 73 * @brief Processing function for the fixed-point CFFT/CIFFT.
emilmont 1:fdd22bb7aa52 74 * @param[in] *S points to an instance of the fixed-point CFFT/CIFFT structure.
emilmont 1:fdd22bb7aa52 75 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
emilmont 1:fdd22bb7aa52 76 * @return none.
emilmont 1:fdd22bb7aa52 77 */
emilmont 1:fdd22bb7aa52 78
emilmont 1:fdd22bb7aa52 79 void arm_cfft_radix2_q15(
emilmont 1:fdd22bb7aa52 80 const arm_cfft_radix2_instance_q15 * S,
emilmont 1:fdd22bb7aa52 81 q15_t * pSrc)
emilmont 1:fdd22bb7aa52 82 {
emilmont 1:fdd22bb7aa52 83
emilmont 1:fdd22bb7aa52 84 if(S->ifftFlag == 1u)
emilmont 1:fdd22bb7aa52 85 {
emilmont 1:fdd22bb7aa52 86 arm_radix2_butterfly_inverse_q15(pSrc, S->fftLen,
emilmont 1:fdd22bb7aa52 87 S->pTwiddle, S->twidCoefModifier);
emilmont 1:fdd22bb7aa52 88 }
emilmont 1:fdd22bb7aa52 89 else
emilmont 1:fdd22bb7aa52 90 {
emilmont 1:fdd22bb7aa52 91 arm_radix2_butterfly_q15(pSrc, S->fftLen,
emilmont 1:fdd22bb7aa52 92 S->pTwiddle, S->twidCoefModifier);
emilmont 1:fdd22bb7aa52 93 }
emilmont 1:fdd22bb7aa52 94
emilmont 1:fdd22bb7aa52 95 arm_bitreversal_q15(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
emilmont 1:fdd22bb7aa52 96 }
emilmont 1:fdd22bb7aa52 97
emilmont 1:fdd22bb7aa52 98 /**
mbed_official 3:7a284390b0ce 99 * @} end of ComplexFFT group
emilmont 1:fdd22bb7aa52 100 */
emilmont 1:fdd22bb7aa52 101
emilmont 1:fdd22bb7aa52 102 void arm_radix2_butterfly_q15(
emilmont 1:fdd22bb7aa52 103 q15_t * pSrc,
emilmont 1:fdd22bb7aa52 104 uint32_t fftLen,
emilmont 1:fdd22bb7aa52 105 q15_t * pCoef,
emilmont 1:fdd22bb7aa52 106 uint16_t twidCoefModifier)
emilmont 1:fdd22bb7aa52 107 {
mbed_official 3:7a284390b0ce 108 #ifndef ARM_MATH_CM0_FAMILY
emilmont 1:fdd22bb7aa52 109
mbed_official 3:7a284390b0ce 110 unsigned i, j, k, l;
mbed_official 3:7a284390b0ce 111 unsigned n1, n2, ia;
emilmont 1:fdd22bb7aa52 112 q15_t in;
emilmont 1:fdd22bb7aa52 113 q31_t T, S, R;
emilmont 1:fdd22bb7aa52 114 q31_t coeff, out1, out2;
emilmont 1:fdd22bb7aa52 115
emilmont 1:fdd22bb7aa52 116 //N = fftLen;
emilmont 1:fdd22bb7aa52 117 n2 = fftLen;
emilmont 1:fdd22bb7aa52 118
emilmont 1:fdd22bb7aa52 119 n1 = n2;
emilmont 1:fdd22bb7aa52 120 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 121 ia = 0;
emilmont 1:fdd22bb7aa52 122
emilmont 1:fdd22bb7aa52 123 // loop for groups
emilmont 1:fdd22bb7aa52 124 for (i = 0; i < n2; i++)
emilmont 1:fdd22bb7aa52 125 {
emilmont 1:fdd22bb7aa52 126 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 127
emilmont 1:fdd22bb7aa52 128 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 129
emilmont 1:fdd22bb7aa52 130 l = i + n2;
emilmont 1:fdd22bb7aa52 131
emilmont 1:fdd22bb7aa52 132 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 133 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 134 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 135
emilmont 1:fdd22bb7aa52 136 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 137 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 138 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 139
emilmont 1:fdd22bb7aa52 140 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 141
emilmont 1:fdd22bb7aa52 142 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 143
emilmont 1:fdd22bb7aa52 144 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 145
emilmont 1:fdd22bb7aa52 146 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 147 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 148
emilmont 1:fdd22bb7aa52 149 #else
emilmont 1:fdd22bb7aa52 150
emilmont 1:fdd22bb7aa52 151 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 152 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 153
emilmont 1:fdd22bb7aa52 154 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 155
emilmont 1:fdd22bb7aa52 156 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 157 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 158
emilmont 1:fdd22bb7aa52 159 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 160
emilmont 1:fdd22bb7aa52 161 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 162
emilmont 1:fdd22bb7aa52 163 // loop for butterfly
emilmont 1:fdd22bb7aa52 164 i++;
emilmont 1:fdd22bb7aa52 165 l++;
emilmont 1:fdd22bb7aa52 166
emilmont 1:fdd22bb7aa52 167 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 168 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 169 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 170
emilmont 1:fdd22bb7aa52 171 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 172 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 173 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 174
emilmont 1:fdd22bb7aa52 175 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 176
emilmont 1:fdd22bb7aa52 177 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 178
emilmont 1:fdd22bb7aa52 179 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 180
emilmont 1:fdd22bb7aa52 181 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 182 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 183
emilmont 1:fdd22bb7aa52 184 #else
emilmont 1:fdd22bb7aa52 185
emilmont 1:fdd22bb7aa52 186 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 187 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 188
emilmont 1:fdd22bb7aa52 189 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 190
emilmont 1:fdd22bb7aa52 191 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 192 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 193
emilmont 1:fdd22bb7aa52 194 } // groups loop end
emilmont 1:fdd22bb7aa52 195
emilmont 1:fdd22bb7aa52 196 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 197
emilmont 1:fdd22bb7aa52 198 // loop for stage
emilmont 1:fdd22bb7aa52 199 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 200 {
emilmont 1:fdd22bb7aa52 201 n1 = n2;
emilmont 1:fdd22bb7aa52 202 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 203 ia = 0;
emilmont 1:fdd22bb7aa52 204
emilmont 1:fdd22bb7aa52 205 // loop for groups
emilmont 1:fdd22bb7aa52 206 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 207 {
emilmont 1:fdd22bb7aa52 208 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 209
emilmont 1:fdd22bb7aa52 210 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 211
emilmont 1:fdd22bb7aa52 212 // loop for butterfly
emilmont 1:fdd22bb7aa52 213 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 214 {
emilmont 1:fdd22bb7aa52 215 l = i + n2;
emilmont 1:fdd22bb7aa52 216
emilmont 1:fdd22bb7aa52 217 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 218
emilmont 1:fdd22bb7aa52 219 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 220
emilmont 1:fdd22bb7aa52 221 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 222
emilmont 1:fdd22bb7aa52 223 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 224
emilmont 1:fdd22bb7aa52 225 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 226
emilmont 1:fdd22bb7aa52 227 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 228 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 229
emilmont 1:fdd22bb7aa52 230 #else
emilmont 1:fdd22bb7aa52 231
emilmont 1:fdd22bb7aa52 232 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 233 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 234
emilmont 1:fdd22bb7aa52 235 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 236
emilmont 1:fdd22bb7aa52 237 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 238 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 239
emilmont 1:fdd22bb7aa52 240 i += n1;
emilmont 1:fdd22bb7aa52 241
emilmont 1:fdd22bb7aa52 242 l = i + n2;
emilmont 1:fdd22bb7aa52 243
emilmont 1:fdd22bb7aa52 244 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 245
emilmont 1:fdd22bb7aa52 246 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 247
emilmont 1:fdd22bb7aa52 248 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 249
emilmont 1:fdd22bb7aa52 250 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 251
emilmont 1:fdd22bb7aa52 252 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 253
emilmont 1:fdd22bb7aa52 254 out1 = __SMUAD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 255 out2 = __SMUSDX(coeff, R);
emilmont 1:fdd22bb7aa52 256
emilmont 1:fdd22bb7aa52 257 #else
emilmont 1:fdd22bb7aa52 258
emilmont 1:fdd22bb7aa52 259 out1 = __SMUSDX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 260 out2 = __SMUAD(coeff, R);
emilmont 1:fdd22bb7aa52 261
emilmont 1:fdd22bb7aa52 262 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 263
emilmont 1:fdd22bb7aa52 264 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 265 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 266
emilmont 1:fdd22bb7aa52 267 } // butterfly loop end
emilmont 1:fdd22bb7aa52 268
emilmont 1:fdd22bb7aa52 269 } // groups loop end
emilmont 1:fdd22bb7aa52 270
emilmont 1:fdd22bb7aa52 271 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 272 } // stages loop end
emilmont 1:fdd22bb7aa52 273
emilmont 1:fdd22bb7aa52 274 n1 = n2;
emilmont 1:fdd22bb7aa52 275 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 276 ia = 0;
emilmont 1:fdd22bb7aa52 277
emilmont 1:fdd22bb7aa52 278 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 279
emilmont 1:fdd22bb7aa52 280 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 281
emilmont 1:fdd22bb7aa52 282 // loop for butterfly
emilmont 1:fdd22bb7aa52 283 for (i = 0; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 284 {
emilmont 1:fdd22bb7aa52 285 l = i + n2;
emilmont 1:fdd22bb7aa52 286
emilmont 1:fdd22bb7aa52 287 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 288
emilmont 1:fdd22bb7aa52 289 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 290
emilmont 1:fdd22bb7aa52 291 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 292
emilmont 1:fdd22bb7aa52 293 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 294
emilmont 1:fdd22bb7aa52 295 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 296
emilmont 1:fdd22bb7aa52 297 i += n1;
emilmont 1:fdd22bb7aa52 298 l = i + n2;
emilmont 1:fdd22bb7aa52 299
emilmont 1:fdd22bb7aa52 300 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 301
emilmont 1:fdd22bb7aa52 302 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 303
emilmont 1:fdd22bb7aa52 304 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 305
emilmont 1:fdd22bb7aa52 306 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 307
emilmont 1:fdd22bb7aa52 308 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 309
emilmont 1:fdd22bb7aa52 310 } // groups loop end
emilmont 1:fdd22bb7aa52 311
emilmont 1:fdd22bb7aa52 312
emilmont 1:fdd22bb7aa52 313 #else
emilmont 1:fdd22bb7aa52 314
mbed_official 3:7a284390b0ce 315 unsigned i, j, k, l;
mbed_official 3:7a284390b0ce 316 unsigned n1, n2, ia;
emilmont 1:fdd22bb7aa52 317 q15_t xt, yt, cosVal, sinVal;
emilmont 1:fdd22bb7aa52 318
emilmont 1:fdd22bb7aa52 319
emilmont 1:fdd22bb7aa52 320 //N = fftLen;
emilmont 1:fdd22bb7aa52 321 n2 = fftLen;
emilmont 1:fdd22bb7aa52 322
emilmont 1:fdd22bb7aa52 323 n1 = n2;
emilmont 1:fdd22bb7aa52 324 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 325 ia = 0;
emilmont 1:fdd22bb7aa52 326
emilmont 1:fdd22bb7aa52 327 // loop for groups
emilmont 1:fdd22bb7aa52 328 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 329 {
emilmont 1:fdd22bb7aa52 330 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 331 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 332 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 333
emilmont 1:fdd22bb7aa52 334 // loop for butterfly
emilmont 1:fdd22bb7aa52 335 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 336 {
emilmont 1:fdd22bb7aa52 337 l = i + n2;
emilmont 1:fdd22bb7aa52 338 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
emilmont 1:fdd22bb7aa52 339 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 340
emilmont 1:fdd22bb7aa52 341 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
emilmont 1:fdd22bb7aa52 342 pSrc[2 * i + 1] =
emilmont 1:fdd22bb7aa52 343 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 344
emilmont 1:fdd22bb7aa52 345 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 346 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 347
emilmont 1:fdd22bb7aa52 348 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 349 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 350
emilmont 1:fdd22bb7aa52 351 } // butterfly loop end
emilmont 1:fdd22bb7aa52 352
emilmont 1:fdd22bb7aa52 353 } // groups loop end
emilmont 1:fdd22bb7aa52 354
emilmont 1:fdd22bb7aa52 355 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 356
emilmont 1:fdd22bb7aa52 357 // loop for stage
emilmont 1:fdd22bb7aa52 358 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 359 {
emilmont 1:fdd22bb7aa52 360 n1 = n2;
emilmont 1:fdd22bb7aa52 361 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 362 ia = 0;
emilmont 1:fdd22bb7aa52 363
emilmont 1:fdd22bb7aa52 364 // loop for groups
emilmont 1:fdd22bb7aa52 365 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 366 {
emilmont 1:fdd22bb7aa52 367 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 368 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 369 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 370
emilmont 1:fdd22bb7aa52 371 // loop for butterfly
emilmont 1:fdd22bb7aa52 372 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 373 {
emilmont 1:fdd22bb7aa52 374 l = i + n2;
emilmont 1:fdd22bb7aa52 375 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 376 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
emilmont 1:fdd22bb7aa52 377
emilmont 1:fdd22bb7aa52 378 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 379 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
emilmont 1:fdd22bb7aa52 380
emilmont 1:fdd22bb7aa52 381 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 382 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 383
emilmont 1:fdd22bb7aa52 384 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 385 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 386
emilmont 1:fdd22bb7aa52 387 } // butterfly loop end
emilmont 1:fdd22bb7aa52 388
emilmont 1:fdd22bb7aa52 389 } // groups loop end
emilmont 1:fdd22bb7aa52 390
emilmont 1:fdd22bb7aa52 391 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 392 } // stages loop end
emilmont 1:fdd22bb7aa52 393
emilmont 1:fdd22bb7aa52 394 n1 = n2;
emilmont 1:fdd22bb7aa52 395 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 396 ia = 0;
emilmont 1:fdd22bb7aa52 397
emilmont 1:fdd22bb7aa52 398 // loop for groups
emilmont 1:fdd22bb7aa52 399 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 400 {
emilmont 1:fdd22bb7aa52 401 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 402 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 403
emilmont 1:fdd22bb7aa52 404 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 405
emilmont 1:fdd22bb7aa52 406 // loop for butterfly
emilmont 1:fdd22bb7aa52 407 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 408 {
emilmont 1:fdd22bb7aa52 409 l = i + n2;
emilmont 1:fdd22bb7aa52 410 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 411 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
emilmont 1:fdd22bb7aa52 412
emilmont 1:fdd22bb7aa52 413 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 414 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
emilmont 1:fdd22bb7aa52 415
emilmont 1:fdd22bb7aa52 416 pSrc[2u * l] = xt;
emilmont 1:fdd22bb7aa52 417
emilmont 1:fdd22bb7aa52 418 pSrc[2u * l + 1u] = yt;
emilmont 1:fdd22bb7aa52 419
emilmont 1:fdd22bb7aa52 420 } // butterfly loop end
emilmont 1:fdd22bb7aa52 421
emilmont 1:fdd22bb7aa52 422 } // groups loop end
emilmont 1:fdd22bb7aa52 423
emilmont 1:fdd22bb7aa52 424 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 425
mbed_official 3:7a284390b0ce 426 #endif // #ifndef ARM_MATH_CM0_FAMILY
emilmont 1:fdd22bb7aa52 427
emilmont 1:fdd22bb7aa52 428 }
emilmont 1:fdd22bb7aa52 429
emilmont 1:fdd22bb7aa52 430
emilmont 1:fdd22bb7aa52 431 void arm_radix2_butterfly_inverse_q15(
emilmont 1:fdd22bb7aa52 432 q15_t * pSrc,
emilmont 1:fdd22bb7aa52 433 uint32_t fftLen,
emilmont 1:fdd22bb7aa52 434 q15_t * pCoef,
emilmont 1:fdd22bb7aa52 435 uint16_t twidCoefModifier)
emilmont 1:fdd22bb7aa52 436 {
mbed_official 3:7a284390b0ce 437 #ifndef ARM_MATH_CM0_FAMILY
emilmont 1:fdd22bb7aa52 438
mbed_official 3:7a284390b0ce 439 unsigned i, j, k, l;
mbed_official 3:7a284390b0ce 440 unsigned n1, n2, ia;
emilmont 1:fdd22bb7aa52 441 q15_t in;
emilmont 1:fdd22bb7aa52 442 q31_t T, S, R;
emilmont 1:fdd22bb7aa52 443 q31_t coeff, out1, out2;
emilmont 1:fdd22bb7aa52 444
emilmont 1:fdd22bb7aa52 445 //N = fftLen;
emilmont 1:fdd22bb7aa52 446 n2 = fftLen;
emilmont 1:fdd22bb7aa52 447
emilmont 1:fdd22bb7aa52 448 n1 = n2;
emilmont 1:fdd22bb7aa52 449 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 450 ia = 0;
emilmont 1:fdd22bb7aa52 451
emilmont 1:fdd22bb7aa52 452 // loop for groups
emilmont 1:fdd22bb7aa52 453 for (i = 0; i < n2; i++)
emilmont 1:fdd22bb7aa52 454 {
emilmont 1:fdd22bb7aa52 455 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 456
emilmont 1:fdd22bb7aa52 457 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 458
emilmont 1:fdd22bb7aa52 459 l = i + n2;
emilmont 1:fdd22bb7aa52 460
emilmont 1:fdd22bb7aa52 461 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 462 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 463 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 464
emilmont 1:fdd22bb7aa52 465 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 466 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 467 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 468
emilmont 1:fdd22bb7aa52 469 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 470
emilmont 1:fdd22bb7aa52 471 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 472
emilmont 1:fdd22bb7aa52 473 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 474
emilmont 1:fdd22bb7aa52 475 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 476 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 477 #else
emilmont 1:fdd22bb7aa52 478
emilmont 1:fdd22bb7aa52 479 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 480 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 481
emilmont 1:fdd22bb7aa52 482 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 483
emilmont 1:fdd22bb7aa52 484 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 485 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 486
emilmont 1:fdd22bb7aa52 487 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 488
emilmont 1:fdd22bb7aa52 489 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 490
emilmont 1:fdd22bb7aa52 491 // loop for butterfly
emilmont 1:fdd22bb7aa52 492 i++;
emilmont 1:fdd22bb7aa52 493 l++;
emilmont 1:fdd22bb7aa52 494
emilmont 1:fdd22bb7aa52 495 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 496 in = ((int16_t) (T & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 497 T = ((T >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 498
emilmont 1:fdd22bb7aa52 499 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 500 in = ((int16_t) (S & 0xFFFF)) >> 2;
emilmont 1:fdd22bb7aa52 501 S = ((S >> 2) & 0xFFFF0000) | (in & 0xFFFF);
emilmont 1:fdd22bb7aa52 502
emilmont 1:fdd22bb7aa52 503 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 504
emilmont 1:fdd22bb7aa52 505 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 506
emilmont 1:fdd22bb7aa52 507 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 508
emilmont 1:fdd22bb7aa52 509 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 510 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 511 #else
emilmont 1:fdd22bb7aa52 512
emilmont 1:fdd22bb7aa52 513 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 514 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 515
emilmont 1:fdd22bb7aa52 516 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 517
emilmont 1:fdd22bb7aa52 518 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 519 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 520
emilmont 1:fdd22bb7aa52 521 } // groups loop end
emilmont 1:fdd22bb7aa52 522
emilmont 1:fdd22bb7aa52 523 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 524
emilmont 1:fdd22bb7aa52 525 // loop for stage
emilmont 1:fdd22bb7aa52 526 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 527 {
emilmont 1:fdd22bb7aa52 528 n1 = n2;
emilmont 1:fdd22bb7aa52 529 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 530 ia = 0;
emilmont 1:fdd22bb7aa52 531
emilmont 1:fdd22bb7aa52 532 // loop for groups
emilmont 1:fdd22bb7aa52 533 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 534 {
emilmont 1:fdd22bb7aa52 535 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 536
emilmont 1:fdd22bb7aa52 537 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 538
emilmont 1:fdd22bb7aa52 539 // loop for butterfly
emilmont 1:fdd22bb7aa52 540 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 541 {
emilmont 1:fdd22bb7aa52 542 l = i + n2;
emilmont 1:fdd22bb7aa52 543
emilmont 1:fdd22bb7aa52 544 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 545
emilmont 1:fdd22bb7aa52 546 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 547
emilmont 1:fdd22bb7aa52 548 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 549
emilmont 1:fdd22bb7aa52 550 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 551
emilmont 1:fdd22bb7aa52 552 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 553
emilmont 1:fdd22bb7aa52 554 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 555 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 556
emilmont 1:fdd22bb7aa52 557 #else
emilmont 1:fdd22bb7aa52 558
emilmont 1:fdd22bb7aa52 559 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 560 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 561
emilmont 1:fdd22bb7aa52 562 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 563
emilmont 1:fdd22bb7aa52 564 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 565 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 566
emilmont 1:fdd22bb7aa52 567 i += n1;
emilmont 1:fdd22bb7aa52 568
emilmont 1:fdd22bb7aa52 569 l = i + n2;
emilmont 1:fdd22bb7aa52 570
emilmont 1:fdd22bb7aa52 571 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 572
emilmont 1:fdd22bb7aa52 573 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 574
emilmont 1:fdd22bb7aa52 575 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 576
emilmont 1:fdd22bb7aa52 577 _SIMD32_OFFSET(pSrc + (2 * i)) = __SHADD16(T, S);
emilmont 1:fdd22bb7aa52 578
emilmont 1:fdd22bb7aa52 579 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 580
emilmont 1:fdd22bb7aa52 581 out1 = __SMUSD(coeff, R) >> 16;
emilmont 1:fdd22bb7aa52 582 out2 = __SMUADX(coeff, R);
emilmont 1:fdd22bb7aa52 583 #else
emilmont 1:fdd22bb7aa52 584
emilmont 1:fdd22bb7aa52 585 out1 = __SMUADX(R, coeff) >> 16u;
emilmont 1:fdd22bb7aa52 586 out2 = __SMUSD(__QSUB(0, coeff), R);
emilmont 1:fdd22bb7aa52 587
emilmont 1:fdd22bb7aa52 588 #endif // #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 589
emilmont 1:fdd22bb7aa52 590 _SIMD32_OFFSET(pSrc + (2u * l)) =
emilmont 1:fdd22bb7aa52 591 (q31_t) ((out2) & 0xFFFF0000) | (out1 & 0x0000FFFF);
emilmont 1:fdd22bb7aa52 592
emilmont 1:fdd22bb7aa52 593 } // butterfly loop end
emilmont 1:fdd22bb7aa52 594
emilmont 1:fdd22bb7aa52 595 } // groups loop end
emilmont 1:fdd22bb7aa52 596
emilmont 1:fdd22bb7aa52 597 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 598 } // stages loop end
emilmont 1:fdd22bb7aa52 599
emilmont 1:fdd22bb7aa52 600 n1 = n2;
emilmont 1:fdd22bb7aa52 601 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 602 ia = 0;
emilmont 1:fdd22bb7aa52 603
emilmont 1:fdd22bb7aa52 604 // loop for groups
emilmont 1:fdd22bb7aa52 605 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 606 {
emilmont 1:fdd22bb7aa52 607 coeff = _SIMD32_OFFSET(pCoef + (ia * 2u));
emilmont 1:fdd22bb7aa52 608
emilmont 1:fdd22bb7aa52 609 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 610
emilmont 1:fdd22bb7aa52 611 // loop for butterfly
emilmont 1:fdd22bb7aa52 612 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 613 {
emilmont 1:fdd22bb7aa52 614 l = i + n2;
emilmont 1:fdd22bb7aa52 615
emilmont 1:fdd22bb7aa52 616 T = _SIMD32_OFFSET(pSrc + (2 * i));
emilmont 1:fdd22bb7aa52 617
emilmont 1:fdd22bb7aa52 618 S = _SIMD32_OFFSET(pSrc + (2 * l));
emilmont 1:fdd22bb7aa52 619
emilmont 1:fdd22bb7aa52 620 R = __QSUB16(T, S);
emilmont 1:fdd22bb7aa52 621
emilmont 1:fdd22bb7aa52 622 _SIMD32_OFFSET(pSrc + (2 * i)) = __QADD16(T, S);
emilmont 1:fdd22bb7aa52 623
emilmont 1:fdd22bb7aa52 624 _SIMD32_OFFSET(pSrc + (2u * l)) = R;
emilmont 1:fdd22bb7aa52 625
emilmont 1:fdd22bb7aa52 626 } // butterfly loop end
emilmont 1:fdd22bb7aa52 627
emilmont 1:fdd22bb7aa52 628 } // groups loop end
emilmont 1:fdd22bb7aa52 629
emilmont 1:fdd22bb7aa52 630 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 631
emilmont 1:fdd22bb7aa52 632 #else
emilmont 1:fdd22bb7aa52 633
emilmont 1:fdd22bb7aa52 634
mbed_official 3:7a284390b0ce 635 unsigned i, j, k, l;
mbed_official 3:7a284390b0ce 636 unsigned n1, n2, ia;
emilmont 1:fdd22bb7aa52 637 q15_t xt, yt, cosVal, sinVal;
emilmont 1:fdd22bb7aa52 638
emilmont 1:fdd22bb7aa52 639 //N = fftLen;
emilmont 1:fdd22bb7aa52 640 n2 = fftLen;
emilmont 1:fdd22bb7aa52 641
emilmont 1:fdd22bb7aa52 642 n1 = n2;
emilmont 1:fdd22bb7aa52 643 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 644 ia = 0;
emilmont 1:fdd22bb7aa52 645
emilmont 1:fdd22bb7aa52 646 // loop for groups
emilmont 1:fdd22bb7aa52 647 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 648 {
emilmont 1:fdd22bb7aa52 649 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 650 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 651 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 652
emilmont 1:fdd22bb7aa52 653 // loop for butterfly
emilmont 1:fdd22bb7aa52 654 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 655 {
emilmont 1:fdd22bb7aa52 656 l = i + n2;
emilmont 1:fdd22bb7aa52 657 xt = (pSrc[2 * i] >> 2u) - (pSrc[2 * l] >> 2u);
emilmont 1:fdd22bb7aa52 658 pSrc[2 * i] = ((pSrc[2 * i] >> 2u) + (pSrc[2 * l] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 659
emilmont 1:fdd22bb7aa52 660 yt = (pSrc[2 * i + 1] >> 2u) - (pSrc[2 * l + 1] >> 2u);
emilmont 1:fdd22bb7aa52 661 pSrc[2 * i + 1] =
emilmont 1:fdd22bb7aa52 662 ((pSrc[2 * l + 1] >> 2u) + (pSrc[2 * i + 1] >> 2u)) >> 1u;
emilmont 1:fdd22bb7aa52 663
emilmont 1:fdd22bb7aa52 664 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 665 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 666
emilmont 1:fdd22bb7aa52 667 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 668 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 669
emilmont 1:fdd22bb7aa52 670 } // butterfly loop end
emilmont 1:fdd22bb7aa52 671
emilmont 1:fdd22bb7aa52 672 } // groups loop end
emilmont 1:fdd22bb7aa52 673
emilmont 1:fdd22bb7aa52 674 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 675
emilmont 1:fdd22bb7aa52 676 // loop for stage
emilmont 1:fdd22bb7aa52 677 for (k = fftLen / 2; k > 2; k = k >> 1)
emilmont 1:fdd22bb7aa52 678 {
emilmont 1:fdd22bb7aa52 679 n1 = n2;
emilmont 1:fdd22bb7aa52 680 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 681 ia = 0;
emilmont 1:fdd22bb7aa52 682
emilmont 1:fdd22bb7aa52 683 // loop for groups
emilmont 1:fdd22bb7aa52 684 for (j = 0; j < n2; j++)
emilmont 1:fdd22bb7aa52 685 {
emilmont 1:fdd22bb7aa52 686 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 687 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 688 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 689
emilmont 1:fdd22bb7aa52 690 // loop for butterfly
emilmont 1:fdd22bb7aa52 691 for (i = j; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 692 {
emilmont 1:fdd22bb7aa52 693 l = i + n2;
emilmont 1:fdd22bb7aa52 694 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 695 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]) >> 1u;
emilmont 1:fdd22bb7aa52 696
emilmont 1:fdd22bb7aa52 697 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 698 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]) >> 1u;
emilmont 1:fdd22bb7aa52 699
emilmont 1:fdd22bb7aa52 700 pSrc[2u * l] = (((int16_t) (((q31_t) xt * cosVal) >> 16)) -
emilmont 1:fdd22bb7aa52 701 ((int16_t) (((q31_t) yt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 702
emilmont 1:fdd22bb7aa52 703 pSrc[2u * l + 1u] = (((int16_t) (((q31_t) yt * cosVal) >> 16)) +
emilmont 1:fdd22bb7aa52 704 ((int16_t) (((q31_t) xt * sinVal) >> 16)));
emilmont 1:fdd22bb7aa52 705
emilmont 1:fdd22bb7aa52 706 } // butterfly loop end
emilmont 1:fdd22bb7aa52 707
emilmont 1:fdd22bb7aa52 708 } // groups loop end
emilmont 1:fdd22bb7aa52 709
emilmont 1:fdd22bb7aa52 710 twidCoefModifier = twidCoefModifier << 1u;
emilmont 1:fdd22bb7aa52 711 } // stages loop end
emilmont 1:fdd22bb7aa52 712
emilmont 1:fdd22bb7aa52 713 n1 = n2;
emilmont 1:fdd22bb7aa52 714 n2 = n2 >> 1;
emilmont 1:fdd22bb7aa52 715 ia = 0;
emilmont 1:fdd22bb7aa52 716
emilmont 1:fdd22bb7aa52 717 cosVal = pCoef[ia * 2];
emilmont 1:fdd22bb7aa52 718 sinVal = pCoef[(ia * 2) + 1];
emilmont 1:fdd22bb7aa52 719
emilmont 1:fdd22bb7aa52 720 ia = ia + twidCoefModifier;
emilmont 1:fdd22bb7aa52 721
emilmont 1:fdd22bb7aa52 722 // loop for butterfly
emilmont 1:fdd22bb7aa52 723 for (i = 0; i < fftLen; i += n1)
emilmont 1:fdd22bb7aa52 724 {
emilmont 1:fdd22bb7aa52 725 l = i + n2;
emilmont 1:fdd22bb7aa52 726 xt = pSrc[2 * i] - pSrc[2 * l];
emilmont 1:fdd22bb7aa52 727 pSrc[2 * i] = (pSrc[2 * i] + pSrc[2 * l]);
emilmont 1:fdd22bb7aa52 728
emilmont 1:fdd22bb7aa52 729 yt = pSrc[2 * i + 1] - pSrc[2 * l + 1];
emilmont 1:fdd22bb7aa52 730 pSrc[2 * i + 1] = (pSrc[2 * l + 1] + pSrc[2 * i + 1]);
emilmont 1:fdd22bb7aa52 731
emilmont 1:fdd22bb7aa52 732 pSrc[2u * l] = xt;
emilmont 1:fdd22bb7aa52 733
emilmont 1:fdd22bb7aa52 734 pSrc[2u * l + 1u] = yt;
emilmont 1:fdd22bb7aa52 735
emilmont 1:fdd22bb7aa52 736 } // groups loop end
emilmont 1:fdd22bb7aa52 737
emilmont 1:fdd22bb7aa52 738
mbed_official 3:7a284390b0ce 739 #endif // #ifndef ARM_MATH_CM0_FAMILY
emilmont 1:fdd22bb7aa52 740
emilmont 1:fdd22bb7aa52 741 }