Joep D
The CMSIS DSP 5 library
Dependents: Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more
functions/TransformFunctions/arm_cfft_radix4_f32.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?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| xorjoep | 1:24714b45cd1b | 1 | /* ---------------------------------------------------------------------- |
| xorjoep | 1:24714b45cd1b | 2 | * Project: CMSIS DSP Library |
| xorjoep | 1:24714b45cd1b | 3 | * Title: arm_cfft_radix4_f32.c |
| xorjoep | 1:24714b45cd1b | 4 | * Description: Radix-4 Decimation in Frequency CFFT & CIFFT Floating point processing 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 | extern void arm_bitreversal_f32( |
| xorjoep | 1:24714b45cd1b | 32 | float32_t * pSrc, |
| xorjoep | 1:24714b45cd1b | 33 | uint16_t fftSize, |
| xorjoep | 1:24714b45cd1b | 34 | uint16_t bitRevFactor, |
| xorjoep | 1:24714b45cd1b | 35 | uint16_t * pBitRevTab); |
| xorjoep | 1:24714b45cd1b | 36 | |
| xorjoep | 1:24714b45cd1b | 37 | void arm_radix4_butterfly_f32( |
| xorjoep | 1:24714b45cd1b | 38 | float32_t * pSrc, |
| xorjoep | 1:24714b45cd1b | 39 | uint16_t fftLen, |
| xorjoep | 1:24714b45cd1b | 40 | float32_t * pCoef, |
| xorjoep | 1:24714b45cd1b | 41 | uint16_t twidCoefModifier); |
| xorjoep | 1:24714b45cd1b | 42 | |
| xorjoep | 1:24714b45cd1b | 43 | void arm_radix4_butterfly_inverse_f32( |
| xorjoep | 1:24714b45cd1b | 44 | float32_t * pSrc, |
| xorjoep | 1:24714b45cd1b | 45 | uint16_t fftLen, |
| xorjoep | 1:24714b45cd1b | 46 | float32_t * pCoef, |
| xorjoep | 1:24714b45cd1b | 47 | uint16_t twidCoefModifier, |
| xorjoep | 1:24714b45cd1b | 48 | float32_t onebyfftLen); |
| xorjoep | 1:24714b45cd1b | 49 | |
| xorjoep | 1:24714b45cd1b | 50 | |
| xorjoep | 1:24714b45cd1b | 51 | /** |
| xorjoep | 1:24714b45cd1b | 52 | * @ingroup groupTransforms |
| xorjoep | 1:24714b45cd1b | 53 | */ |
| xorjoep | 1:24714b45cd1b | 54 | |
| xorjoep | 1:24714b45cd1b | 55 | /** |
| xorjoep | 1:24714b45cd1b | 56 | * @addtogroup ComplexFFT |
| xorjoep | 1:24714b45cd1b | 57 | * @{ |
| xorjoep | 1:24714b45cd1b | 58 | */ |
| xorjoep | 1:24714b45cd1b | 59 | |
| xorjoep | 1:24714b45cd1b | 60 | /** |
| xorjoep | 1:24714b45cd1b | 61 | * @details |
| xorjoep | 1:24714b45cd1b | 62 | * @brief Processing function for the floating-point Radix-4 CFFT/CIFFT. |
| xorjoep | 1:24714b45cd1b | 63 | * @deprecated Do not use this function. It has been superseded by \ref arm_cfft_f32 and will be removed |
| xorjoep | 1:24714b45cd1b | 64 | * in the future. |
| xorjoep | 1:24714b45cd1b | 65 | * @param[in] *S points to an instance of the floating-point Radix-4 CFFT/CIFFT structure. |
| xorjoep | 1:24714b45cd1b | 66 | * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place. |
| xorjoep | 1:24714b45cd1b | 67 | * @return none. |
| xorjoep | 1:24714b45cd1b | 68 | */ |
| xorjoep | 1:24714b45cd1b | 69 | |
| xorjoep | 1:24714b45cd1b | 70 | void arm_cfft_radix4_f32( |
| xorjoep | 1:24714b45cd1b | 71 | const arm_cfft_radix4_instance_f32 * S, |
| xorjoep | 1:24714b45cd1b | 72 | float32_t * pSrc) |
| xorjoep | 1:24714b45cd1b | 73 | { |
| xorjoep | 1:24714b45cd1b | 74 | if (S->ifftFlag == 1U) |
| xorjoep | 1:24714b45cd1b | 75 | { |
| xorjoep | 1:24714b45cd1b | 76 | /* Complex IFFT radix-4 */ |
| xorjoep | 1:24714b45cd1b | 77 | arm_radix4_butterfly_inverse_f32(pSrc, S->fftLen, S->pTwiddle, S->twidCoefModifier, S->onebyfftLen); |
| xorjoep | 1:24714b45cd1b | 78 | } |
| xorjoep | 1:24714b45cd1b | 79 | else |
| xorjoep | 1:24714b45cd1b | 80 | { |
| xorjoep | 1:24714b45cd1b | 81 | /* Complex FFT radix-4 */ |
| xorjoep | 1:24714b45cd1b | 82 | arm_radix4_butterfly_f32(pSrc, S->fftLen, S->pTwiddle, S->twidCoefModifier); |
| xorjoep | 1:24714b45cd1b | 83 | } |
| xorjoep | 1:24714b45cd1b | 84 | |
| xorjoep | 1:24714b45cd1b | 85 | if (S->bitReverseFlag == 1U) |
| xorjoep | 1:24714b45cd1b | 86 | { |
| xorjoep | 1:24714b45cd1b | 87 | /* Bit Reversal */ |
| xorjoep | 1:24714b45cd1b | 88 | arm_bitreversal_f32(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); |
| xorjoep | 1:24714b45cd1b | 89 | } |
| xorjoep | 1:24714b45cd1b | 90 | |
| xorjoep | 1:24714b45cd1b | 91 | } |
| xorjoep | 1:24714b45cd1b | 92 | |
| xorjoep | 1:24714b45cd1b | 93 | /** |
| xorjoep | 1:24714b45cd1b | 94 | * @} end of ComplexFFT group |
| xorjoep | 1:24714b45cd1b | 95 | */ |
| xorjoep | 1:24714b45cd1b | 96 | |
| xorjoep | 1:24714b45cd1b | 97 | /* ---------------------------------------------------------------------- |
| xorjoep | 1:24714b45cd1b | 98 | * Internal helper function used by the FFTs |
| xorjoep | 1:24714b45cd1b | 99 | * ---------------------------------------------------------------------- */ |
| xorjoep | 1:24714b45cd1b | 100 | |
| xorjoep | 1:24714b45cd1b | 101 | /* |
| xorjoep | 1:24714b45cd1b | 102 | * @brief Core function for the floating-point CFFT butterfly process. |
| xorjoep | 1:24714b45cd1b | 103 | * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. |
| xorjoep | 1:24714b45cd1b | 104 | * @param[in] fftLen length of the FFT. |
| xorjoep | 1:24714b45cd1b | 105 | * @param[in] *pCoef points to the twiddle coefficient buffer. |
| xorjoep | 1:24714b45cd1b | 106 | * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. |
| xorjoep | 1:24714b45cd1b | 107 | * @return none. |
| xorjoep | 1:24714b45cd1b | 108 | */ |
| xorjoep | 1:24714b45cd1b | 109 | |
| xorjoep | 1:24714b45cd1b | 110 | void arm_radix4_butterfly_f32( |
| xorjoep | 1:24714b45cd1b | 111 | float32_t * pSrc, |
| xorjoep | 1:24714b45cd1b | 112 | uint16_t fftLen, |
| xorjoep | 1:24714b45cd1b | 113 | float32_t * pCoef, |
| xorjoep | 1:24714b45cd1b | 114 | uint16_t twidCoefModifier) |
| xorjoep | 1:24714b45cd1b | 115 | { |
| xorjoep | 1:24714b45cd1b | 116 | |
| xorjoep | 1:24714b45cd1b | 117 | float32_t co1, co2, co3, si1, si2, si3; |
| xorjoep | 1:24714b45cd1b | 118 | uint32_t ia1, ia2, ia3; |
| xorjoep | 1:24714b45cd1b | 119 | uint32_t i0, i1, i2, i3; |
| xorjoep | 1:24714b45cd1b | 120 | uint32_t n1, n2, j, k; |
| xorjoep | 1:24714b45cd1b | 121 | |
| xorjoep | 1:24714b45cd1b | 122 | #if defined (ARM_MATH_DSP) |
| xorjoep | 1:24714b45cd1b | 123 | |
| xorjoep | 1:24714b45cd1b | 124 | /* Run the below code for Cortex-M4 and Cortex-M3 */ |
| xorjoep | 1:24714b45cd1b | 125 | |
| xorjoep | 1:24714b45cd1b | 126 | float32_t xaIn, yaIn, xbIn, ybIn, xcIn, ycIn, xdIn, ydIn; |
| xorjoep | 1:24714b45cd1b | 127 | float32_t Xaplusc, Xbplusd, Yaplusc, Ybplusd, Xaminusc, Xbminusd, Yaminusc, |
| xorjoep | 1:24714b45cd1b | 128 | Ybminusd; |
| xorjoep | 1:24714b45cd1b | 129 | float32_t Xb12C_out, Yb12C_out, Xc12C_out, Yc12C_out, Xd12C_out, Yd12C_out; |
| xorjoep | 1:24714b45cd1b | 130 | float32_t Xb12_out, Yb12_out, Xc12_out, Yc12_out, Xd12_out, Yd12_out; |
| xorjoep | 1:24714b45cd1b | 131 | float32_t *ptr1; |
| xorjoep | 1:24714b45cd1b | 132 | float32_t p0,p1,p2,p3,p4,p5; |
| xorjoep | 1:24714b45cd1b | 133 | float32_t a0,a1,a2,a3,a4,a5,a6,a7; |
| xorjoep | 1:24714b45cd1b | 134 | |
| xorjoep | 1:24714b45cd1b | 135 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 136 | n2 = fftLen; |
| xorjoep | 1:24714b45cd1b | 137 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 138 | |
| xorjoep | 1:24714b45cd1b | 139 | /* n2 = fftLen/4 */ |
| xorjoep | 1:24714b45cd1b | 140 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 141 | i0 = 0U; |
| xorjoep | 1:24714b45cd1b | 142 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 143 | |
| xorjoep | 1:24714b45cd1b | 144 | j = n2; |
| xorjoep | 1:24714b45cd1b | 145 | |
| xorjoep | 1:24714b45cd1b | 146 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 147 | do |
| xorjoep | 1:24714b45cd1b | 148 | { |
| xorjoep | 1:24714b45cd1b | 149 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 150 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 151 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 152 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 153 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 154 | |
| xorjoep | 1:24714b45cd1b | 155 | xaIn = pSrc[(2U * i0)]; |
| xorjoep | 1:24714b45cd1b | 156 | yaIn = pSrc[(2U * i0) + 1U]; |
| xorjoep | 1:24714b45cd1b | 157 | |
| xorjoep | 1:24714b45cd1b | 158 | xbIn = pSrc[(2U * i1)]; |
| xorjoep | 1:24714b45cd1b | 159 | ybIn = pSrc[(2U * i1) + 1U]; |
| xorjoep | 1:24714b45cd1b | 160 | |
| xorjoep | 1:24714b45cd1b | 161 | xcIn = pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 162 | ycIn = pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 163 | |
| xorjoep | 1:24714b45cd1b | 164 | xdIn = pSrc[(2U * i3)]; |
| xorjoep | 1:24714b45cd1b | 165 | ydIn = pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 166 | |
| xorjoep | 1:24714b45cd1b | 167 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 168 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 169 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 170 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 171 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 172 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 173 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 174 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 175 | |
| xorjoep | 1:24714b45cd1b | 176 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 177 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 178 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 179 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 180 | |
| xorjoep | 1:24714b45cd1b | 181 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 182 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 183 | /* xb - xd */ |
| xorjoep | 1:24714b45cd1b | 184 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 185 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 186 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 187 | /* yb - yd */ |
| xorjoep | 1:24714b45cd1b | 188 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 189 | |
| xorjoep | 1:24714b45cd1b | 190 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 191 | pSrc[(2U * i0)] = Xaplusc + Xbplusd; |
| xorjoep | 1:24714b45cd1b | 192 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 193 | pSrc[(2U * i0) + 1U] = Yaplusc + Ybplusd; |
| xorjoep | 1:24714b45cd1b | 194 | |
| xorjoep | 1:24714b45cd1b | 195 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 196 | Xb12C_out = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 197 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 198 | Yb12C_out = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 199 | /* (xa + xc) - (xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 200 | Xc12C_out = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 201 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 202 | Yc12C_out = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 203 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 204 | Xd12C_out = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 205 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 206 | Yd12C_out = (Xbminusd + Yaminusc); |
| xorjoep | 1:24714b45cd1b | 207 | |
| xorjoep | 1:24714b45cd1b | 208 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 209 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 210 | |
| xorjoep | 1:24714b45cd1b | 211 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 212 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 213 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 214 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 215 | |
| xorjoep | 1:24714b45cd1b | 216 | Xb12_out = Xb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 217 | Yb12_out = Yb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 218 | Xc12_out = Xc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 219 | Yc12_out = Yc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 220 | Xd12_out = Xd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 221 | Yd12_out = Yd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 222 | |
| xorjoep | 1:24714b45cd1b | 223 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 224 | //Xb12_out -= Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 225 | p0 = Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 226 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 227 | //Yb12_out += Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 228 | p1 = Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 229 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 230 | //Xc12_out -= Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 231 | p2 = Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 232 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 233 | //Yc12_out += Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 234 | p3 = Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 235 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 236 | //Xd12_out -= Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 237 | p4 = Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 238 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 239 | //Yd12_out += Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 240 | p5 = Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 241 | |
| xorjoep | 1:24714b45cd1b | 242 | Xb12_out += p0; |
| xorjoep | 1:24714b45cd1b | 243 | Yb12_out -= p1; |
| xorjoep | 1:24714b45cd1b | 244 | Xc12_out += p2; |
| xorjoep | 1:24714b45cd1b | 245 | Yc12_out -= p3; |
| xorjoep | 1:24714b45cd1b | 246 | Xd12_out += p4; |
| xorjoep | 1:24714b45cd1b | 247 | Yd12_out -= p5; |
| xorjoep | 1:24714b45cd1b | 248 | |
| xorjoep | 1:24714b45cd1b | 249 | /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 250 | pSrc[2U * i1] = Xc12_out; |
| xorjoep | 1:24714b45cd1b | 251 | |
| xorjoep | 1:24714b45cd1b | 252 | /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 253 | pSrc[(2U * i1) + 1U] = Yc12_out; |
| xorjoep | 1:24714b45cd1b | 254 | |
| xorjoep | 1:24714b45cd1b | 255 | /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 256 | pSrc[2U * i2] = Xb12_out; |
| xorjoep | 1:24714b45cd1b | 257 | |
| xorjoep | 1:24714b45cd1b | 258 | /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 259 | pSrc[(2U * i2) + 1U] = Yb12_out; |
| xorjoep | 1:24714b45cd1b | 260 | |
| xorjoep | 1:24714b45cd1b | 261 | /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 262 | pSrc[2U * i3] = Xd12_out; |
| xorjoep | 1:24714b45cd1b | 263 | |
| xorjoep | 1:24714b45cd1b | 264 | /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 265 | pSrc[(2U * i3) + 1U] = Yd12_out; |
| xorjoep | 1:24714b45cd1b | 266 | |
| xorjoep | 1:24714b45cd1b | 267 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 268 | ia1 += twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 269 | |
| xorjoep | 1:24714b45cd1b | 270 | /* Updating input index */ |
| xorjoep | 1:24714b45cd1b | 271 | i0++; |
| xorjoep | 1:24714b45cd1b | 272 | |
| xorjoep | 1:24714b45cd1b | 273 | } |
| xorjoep | 1:24714b45cd1b | 274 | while (--j); |
| xorjoep | 1:24714b45cd1b | 275 | |
| xorjoep | 1:24714b45cd1b | 276 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 277 | |
| xorjoep | 1:24714b45cd1b | 278 | /* Calculation of second stage to excluding last stage */ |
| xorjoep | 1:24714b45cd1b | 279 | for (k = fftLen >> 2U; k > 4U; k >>= 2U) |
| xorjoep | 1:24714b45cd1b | 280 | { |
| xorjoep | 1:24714b45cd1b | 281 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 282 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 283 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 284 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 285 | |
| xorjoep | 1:24714b45cd1b | 286 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 287 | j = 0; |
| xorjoep | 1:24714b45cd1b | 288 | do |
| xorjoep | 1:24714b45cd1b | 289 | { |
| xorjoep | 1:24714b45cd1b | 290 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 291 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 292 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 293 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 294 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 295 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 296 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 297 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 298 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 299 | |
| xorjoep | 1:24714b45cd1b | 300 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 301 | ia1 += twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 302 | |
| xorjoep | 1:24714b45cd1b | 303 | i0 = j; |
| xorjoep | 1:24714b45cd1b | 304 | do |
| xorjoep | 1:24714b45cd1b | 305 | { |
| xorjoep | 1:24714b45cd1b | 306 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 307 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 308 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 309 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 310 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 311 | |
| xorjoep | 1:24714b45cd1b | 312 | xaIn = pSrc[(2U * i0)]; |
| xorjoep | 1:24714b45cd1b | 313 | yaIn = pSrc[(2U * i0) + 1U]; |
| xorjoep | 1:24714b45cd1b | 314 | |
| xorjoep | 1:24714b45cd1b | 315 | xbIn = pSrc[(2U * i1)]; |
| xorjoep | 1:24714b45cd1b | 316 | ybIn = pSrc[(2U * i1) + 1U]; |
| xorjoep | 1:24714b45cd1b | 317 | |
| xorjoep | 1:24714b45cd1b | 318 | xcIn = pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 319 | ycIn = pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 320 | |
| xorjoep | 1:24714b45cd1b | 321 | xdIn = pSrc[(2U * i3)]; |
| xorjoep | 1:24714b45cd1b | 322 | ydIn = pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 323 | |
| xorjoep | 1:24714b45cd1b | 324 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 325 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 326 | /* (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 327 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 328 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 329 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 330 | /* (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 331 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 332 | |
| xorjoep | 1:24714b45cd1b | 333 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 334 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 335 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 336 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 337 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 338 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 339 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 340 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 341 | |
| xorjoep | 1:24714b45cd1b | 342 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 343 | Xb12C_out = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 344 | /* (ya - yc) - (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 345 | Yb12C_out = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 346 | /* xa + xc -(xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 347 | Xc12C_out = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 348 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 349 | Yc12C_out = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 350 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 351 | Xd12C_out = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 352 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 353 | Yd12C_out = (Xbminusd + Yaminusc); |
| xorjoep | 1:24714b45cd1b | 354 | |
| xorjoep | 1:24714b45cd1b | 355 | pSrc[(2U * i0)] = Xaplusc + Xbplusd; |
| xorjoep | 1:24714b45cd1b | 356 | pSrc[(2U * i0) + 1U] = Yaplusc + Ybplusd; |
| xorjoep | 1:24714b45cd1b | 357 | |
| xorjoep | 1:24714b45cd1b | 358 | Xb12_out = Xb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 359 | Yb12_out = Yb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 360 | Xc12_out = Xc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 361 | Yc12_out = Yc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 362 | Xd12_out = Xd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 363 | Yd12_out = Yd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 364 | |
| xorjoep | 1:24714b45cd1b | 365 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 366 | //Xb12_out -= Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 367 | p0 = Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 368 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 369 | //Yb12_out += Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 370 | p1 = Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 371 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 372 | //Xc12_out -= Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 373 | p2 = Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 374 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 375 | //Yc12_out += Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 376 | p3 = Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 377 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 378 | //Xd12_out -= Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 379 | p4 = Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 380 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 381 | //Yd12_out += Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 382 | p5 = Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 383 | |
| xorjoep | 1:24714b45cd1b | 384 | Xb12_out += p0; |
| xorjoep | 1:24714b45cd1b | 385 | Yb12_out -= p1; |
| xorjoep | 1:24714b45cd1b | 386 | Xc12_out += p2; |
| xorjoep | 1:24714b45cd1b | 387 | Yc12_out -= p3; |
| xorjoep | 1:24714b45cd1b | 388 | Xd12_out += p4; |
| xorjoep | 1:24714b45cd1b | 389 | Yd12_out -= p5; |
| xorjoep | 1:24714b45cd1b | 390 | |
| xorjoep | 1:24714b45cd1b | 391 | /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 392 | pSrc[2U * i1] = Xc12_out; |
| xorjoep | 1:24714b45cd1b | 393 | |
| xorjoep | 1:24714b45cd1b | 394 | /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 395 | pSrc[(2U * i1) + 1U] = Yc12_out; |
| xorjoep | 1:24714b45cd1b | 396 | |
| xorjoep | 1:24714b45cd1b | 397 | /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 398 | pSrc[2U * i2] = Xb12_out; |
| xorjoep | 1:24714b45cd1b | 399 | |
| xorjoep | 1:24714b45cd1b | 400 | /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 401 | pSrc[(2U * i2) + 1U] = Yb12_out; |
| xorjoep | 1:24714b45cd1b | 402 | |
| xorjoep | 1:24714b45cd1b | 403 | /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 404 | pSrc[2U * i3] = Xd12_out; |
| xorjoep | 1:24714b45cd1b | 405 | |
| xorjoep | 1:24714b45cd1b | 406 | /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 407 | pSrc[(2U * i3) + 1U] = Yd12_out; |
| xorjoep | 1:24714b45cd1b | 408 | |
| xorjoep | 1:24714b45cd1b | 409 | i0 += n1; |
| xorjoep | 1:24714b45cd1b | 410 | } while (i0 < fftLen); |
| xorjoep | 1:24714b45cd1b | 411 | j++; |
| xorjoep | 1:24714b45cd1b | 412 | } while (j <= (n2 - 1U)); |
| xorjoep | 1:24714b45cd1b | 413 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 414 | } |
| xorjoep | 1:24714b45cd1b | 415 | |
| xorjoep | 1:24714b45cd1b | 416 | j = fftLen >> 2; |
| xorjoep | 1:24714b45cd1b | 417 | ptr1 = &pSrc[0]; |
| xorjoep | 1:24714b45cd1b | 418 | |
| xorjoep | 1:24714b45cd1b | 419 | /* Calculations of last stage */ |
| xorjoep | 1:24714b45cd1b | 420 | do |
| xorjoep | 1:24714b45cd1b | 421 | { |
| xorjoep | 1:24714b45cd1b | 422 | xaIn = ptr1[0]; |
| xorjoep | 1:24714b45cd1b | 423 | yaIn = ptr1[1]; |
| xorjoep | 1:24714b45cd1b | 424 | xbIn = ptr1[2]; |
| xorjoep | 1:24714b45cd1b | 425 | ybIn = ptr1[3]; |
| xorjoep | 1:24714b45cd1b | 426 | xcIn = ptr1[4]; |
| xorjoep | 1:24714b45cd1b | 427 | ycIn = ptr1[5]; |
| xorjoep | 1:24714b45cd1b | 428 | xdIn = ptr1[6]; |
| xorjoep | 1:24714b45cd1b | 429 | ydIn = ptr1[7]; |
| xorjoep | 1:24714b45cd1b | 430 | |
| xorjoep | 1:24714b45cd1b | 431 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 432 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 433 | |
| xorjoep | 1:24714b45cd1b | 434 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 435 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 436 | |
| xorjoep | 1:24714b45cd1b | 437 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 438 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 439 | |
| xorjoep | 1:24714b45cd1b | 440 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 441 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 442 | |
| xorjoep | 1:24714b45cd1b | 443 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 444 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 445 | |
| xorjoep | 1:24714b45cd1b | 446 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 447 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 448 | |
| xorjoep | 1:24714b45cd1b | 449 | /* (xb-xd) */ |
| xorjoep | 1:24714b45cd1b | 450 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 451 | |
| xorjoep | 1:24714b45cd1b | 452 | /* (yb-yd) */ |
| xorjoep | 1:24714b45cd1b | 453 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 454 | |
| xorjoep | 1:24714b45cd1b | 455 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 456 | a0 = (Xaplusc + Xbplusd); |
| xorjoep | 1:24714b45cd1b | 457 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 458 | a1 = (Yaplusc + Ybplusd); |
| xorjoep | 1:24714b45cd1b | 459 | /* xc' = (xa-xb+xc-xd) */ |
| xorjoep | 1:24714b45cd1b | 460 | a2 = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 461 | /* yc' = (ya-yb+yc-yd) */ |
| xorjoep | 1:24714b45cd1b | 462 | a3 = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 463 | /* xb' = (xa+yb-xc-yd) */ |
| xorjoep | 1:24714b45cd1b | 464 | a4 = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 465 | /* yb' = (ya-xb-yc+xd) */ |
| xorjoep | 1:24714b45cd1b | 466 | a5 = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 467 | /* xd' = (xa-yb-xc+yd)) */ |
| xorjoep | 1:24714b45cd1b | 468 | a6 = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 469 | /* yd' = (ya+xb-yc-xd) */ |
| xorjoep | 1:24714b45cd1b | 470 | a7 = (Xbminusd + Yaminusc); |
| xorjoep | 1:24714b45cd1b | 471 | |
| xorjoep | 1:24714b45cd1b | 472 | ptr1[0] = a0; |
| xorjoep | 1:24714b45cd1b | 473 | ptr1[1] = a1; |
| xorjoep | 1:24714b45cd1b | 474 | ptr1[2] = a2; |
| xorjoep | 1:24714b45cd1b | 475 | ptr1[3] = a3; |
| xorjoep | 1:24714b45cd1b | 476 | ptr1[4] = a4; |
| xorjoep | 1:24714b45cd1b | 477 | ptr1[5] = a5; |
| xorjoep | 1:24714b45cd1b | 478 | ptr1[6] = a6; |
| xorjoep | 1:24714b45cd1b | 479 | ptr1[7] = a7; |
| xorjoep | 1:24714b45cd1b | 480 | |
| xorjoep | 1:24714b45cd1b | 481 | /* increment pointer by 8 */ |
| xorjoep | 1:24714b45cd1b | 482 | ptr1 += 8U; |
| xorjoep | 1:24714b45cd1b | 483 | } while (--j); |
| xorjoep | 1:24714b45cd1b | 484 | |
| xorjoep | 1:24714b45cd1b | 485 | #else |
| xorjoep | 1:24714b45cd1b | 486 | |
| xorjoep | 1:24714b45cd1b | 487 | float32_t t1, t2, r1, r2, s1, s2; |
| xorjoep | 1:24714b45cd1b | 488 | |
| xorjoep | 1:24714b45cd1b | 489 | /* Run the below code for Cortex-M0 */ |
| xorjoep | 1:24714b45cd1b | 490 | |
| xorjoep | 1:24714b45cd1b | 491 | /* Initializations for the fft calculation */ |
| xorjoep | 1:24714b45cd1b | 492 | n2 = fftLen; |
| xorjoep | 1:24714b45cd1b | 493 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 494 | for (k = fftLen; k > 1U; k >>= 2U) |
| xorjoep | 1:24714b45cd1b | 495 | { |
| xorjoep | 1:24714b45cd1b | 496 | /* Initializations for the fft calculation */ |
| xorjoep | 1:24714b45cd1b | 497 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 498 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 499 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 500 | |
| xorjoep | 1:24714b45cd1b | 501 | /* FFT Calculation */ |
| xorjoep | 1:24714b45cd1b | 502 | j = 0; |
| xorjoep | 1:24714b45cd1b | 503 | do |
| xorjoep | 1:24714b45cd1b | 504 | { |
| xorjoep | 1:24714b45cd1b | 505 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 506 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 507 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 508 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 509 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 510 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 511 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 512 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 513 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 514 | |
| xorjoep | 1:24714b45cd1b | 515 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 516 | ia1 = ia1 + twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 517 | |
| xorjoep | 1:24714b45cd1b | 518 | i0 = j; |
| xorjoep | 1:24714b45cd1b | 519 | do |
| xorjoep | 1:24714b45cd1b | 520 | { |
| xorjoep | 1:24714b45cd1b | 521 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 522 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 523 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 524 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 525 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 526 | |
| xorjoep | 1:24714b45cd1b | 527 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 528 | r1 = pSrc[(2U * i0)] + pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 529 | |
| xorjoep | 1:24714b45cd1b | 530 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 531 | r2 = pSrc[(2U * i0)] - pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 532 | |
| xorjoep | 1:24714b45cd1b | 533 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 534 | s1 = pSrc[(2U * i0) + 1U] + pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 535 | |
| xorjoep | 1:24714b45cd1b | 536 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 537 | s2 = pSrc[(2U * i0) + 1U] - pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 538 | |
| xorjoep | 1:24714b45cd1b | 539 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 540 | t1 = pSrc[2U * i1] + pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 541 | |
| xorjoep | 1:24714b45cd1b | 542 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 543 | pSrc[2U * i0] = r1 + t1; |
| xorjoep | 1:24714b45cd1b | 544 | |
| xorjoep | 1:24714b45cd1b | 545 | /* xa + xc -(xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 546 | r1 = r1 - t1; |
| xorjoep | 1:24714b45cd1b | 547 | |
| xorjoep | 1:24714b45cd1b | 548 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 549 | t2 = pSrc[(2U * i1) + 1U] + pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 550 | |
| xorjoep | 1:24714b45cd1b | 551 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 552 | pSrc[(2U * i0) + 1U] = s1 + t2; |
| xorjoep | 1:24714b45cd1b | 553 | |
| xorjoep | 1:24714b45cd1b | 554 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 555 | s1 = s1 - t2; |
| xorjoep | 1:24714b45cd1b | 556 | |
| xorjoep | 1:24714b45cd1b | 557 | /* (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 558 | t1 = pSrc[(2U * i1) + 1U] - pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 559 | |
| xorjoep | 1:24714b45cd1b | 560 | /* (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 561 | t2 = pSrc[2U * i1] - pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 562 | |
| xorjoep | 1:24714b45cd1b | 563 | /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 564 | pSrc[2U * i1] = (r1 * co2) + (s1 * si2); |
| xorjoep | 1:24714b45cd1b | 565 | |
| xorjoep | 1:24714b45cd1b | 566 | /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 567 | pSrc[(2U * i1) + 1U] = (s1 * co2) - (r1 * si2); |
| xorjoep | 1:24714b45cd1b | 568 | |
| xorjoep | 1:24714b45cd1b | 569 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 570 | r1 = r2 + t1; |
| xorjoep | 1:24714b45cd1b | 571 | |
| xorjoep | 1:24714b45cd1b | 572 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 573 | r2 = r2 - t1; |
| xorjoep | 1:24714b45cd1b | 574 | |
| xorjoep | 1:24714b45cd1b | 575 | /* (ya - yc) - (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 576 | s1 = s2 - t2; |
| xorjoep | 1:24714b45cd1b | 577 | |
| xorjoep | 1:24714b45cd1b | 578 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 579 | s2 = s2 + t2; |
| xorjoep | 1:24714b45cd1b | 580 | |
| xorjoep | 1:24714b45cd1b | 581 | /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 582 | pSrc[2U * i2] = (r1 * co1) + (s1 * si1); |
| xorjoep | 1:24714b45cd1b | 583 | |
| xorjoep | 1:24714b45cd1b | 584 | /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 585 | pSrc[(2U * i2) + 1U] = (s1 * co1) - (r1 * si1); |
| xorjoep | 1:24714b45cd1b | 586 | |
| xorjoep | 1:24714b45cd1b | 587 | /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 588 | pSrc[2U * i3] = (r2 * co3) + (s2 * si3); |
| xorjoep | 1:24714b45cd1b | 589 | |
| xorjoep | 1:24714b45cd1b | 590 | /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 591 | pSrc[(2U * i3) + 1U] = (s2 * co3) - (r2 * si3); |
| xorjoep | 1:24714b45cd1b | 592 | |
| xorjoep | 1:24714b45cd1b | 593 | i0 += n1; |
| xorjoep | 1:24714b45cd1b | 594 | } while ( i0 < fftLen); |
| xorjoep | 1:24714b45cd1b | 595 | j++; |
| xorjoep | 1:24714b45cd1b | 596 | } while (j <= (n2 - 1U)); |
| xorjoep | 1:24714b45cd1b | 597 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 598 | } |
| xorjoep | 1:24714b45cd1b | 599 | |
| xorjoep | 1:24714b45cd1b | 600 | #endif /* #if defined (ARM_MATH_DSP) */ |
| xorjoep | 1:24714b45cd1b | 601 | |
| xorjoep | 1:24714b45cd1b | 602 | } |
| xorjoep | 1:24714b45cd1b | 603 | |
| xorjoep | 1:24714b45cd1b | 604 | /* |
| xorjoep | 1:24714b45cd1b | 605 | * @brief Core function for the floating-point CIFFT butterfly process. |
| xorjoep | 1:24714b45cd1b | 606 | * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. |
| xorjoep | 1:24714b45cd1b | 607 | * @param[in] fftLen length of the FFT. |
| xorjoep | 1:24714b45cd1b | 608 | * @param[in] *pCoef points to twiddle coefficient buffer. |
| xorjoep | 1:24714b45cd1b | 609 | * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. |
| xorjoep | 1:24714b45cd1b | 610 | * @param[in] onebyfftLen value of 1/fftLen. |
| xorjoep | 1:24714b45cd1b | 611 | * @return none. |
| xorjoep | 1:24714b45cd1b | 612 | */ |
| xorjoep | 1:24714b45cd1b | 613 | |
| xorjoep | 1:24714b45cd1b | 614 | void arm_radix4_butterfly_inverse_f32( |
| xorjoep | 1:24714b45cd1b | 615 | float32_t * pSrc, |
| xorjoep | 1:24714b45cd1b | 616 | uint16_t fftLen, |
| xorjoep | 1:24714b45cd1b | 617 | float32_t * pCoef, |
| xorjoep | 1:24714b45cd1b | 618 | uint16_t twidCoefModifier, |
| xorjoep | 1:24714b45cd1b | 619 | float32_t onebyfftLen) |
| xorjoep | 1:24714b45cd1b | 620 | { |
| xorjoep | 1:24714b45cd1b | 621 | float32_t co1, co2, co3, si1, si2, si3; |
| xorjoep | 1:24714b45cd1b | 622 | uint32_t ia1, ia2, ia3; |
| xorjoep | 1:24714b45cd1b | 623 | uint32_t i0, i1, i2, i3; |
| xorjoep | 1:24714b45cd1b | 624 | uint32_t n1, n2, j, k; |
| xorjoep | 1:24714b45cd1b | 625 | |
| xorjoep | 1:24714b45cd1b | 626 | #if defined (ARM_MATH_DSP) |
| xorjoep | 1:24714b45cd1b | 627 | |
| xorjoep | 1:24714b45cd1b | 628 | float32_t xaIn, yaIn, xbIn, ybIn, xcIn, ycIn, xdIn, ydIn; |
| xorjoep | 1:24714b45cd1b | 629 | float32_t Xaplusc, Xbplusd, Yaplusc, Ybplusd, Xaminusc, Xbminusd, Yaminusc, |
| xorjoep | 1:24714b45cd1b | 630 | Ybminusd; |
| xorjoep | 1:24714b45cd1b | 631 | float32_t Xb12C_out, Yb12C_out, Xc12C_out, Yc12C_out, Xd12C_out, Yd12C_out; |
| xorjoep | 1:24714b45cd1b | 632 | float32_t Xb12_out, Yb12_out, Xc12_out, Yc12_out, Xd12_out, Yd12_out; |
| xorjoep | 1:24714b45cd1b | 633 | float32_t *ptr1; |
| xorjoep | 1:24714b45cd1b | 634 | float32_t p0,p1,p2,p3,p4,p5,p6,p7; |
| xorjoep | 1:24714b45cd1b | 635 | float32_t a0,a1,a2,a3,a4,a5,a6,a7; |
| xorjoep | 1:24714b45cd1b | 636 | |
| xorjoep | 1:24714b45cd1b | 637 | |
| xorjoep | 1:24714b45cd1b | 638 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 639 | n2 = fftLen; |
| xorjoep | 1:24714b45cd1b | 640 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 641 | |
| xorjoep | 1:24714b45cd1b | 642 | /* n2 = fftLen/4 */ |
| xorjoep | 1:24714b45cd1b | 643 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 644 | i0 = 0U; |
| xorjoep | 1:24714b45cd1b | 645 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 646 | |
| xorjoep | 1:24714b45cd1b | 647 | j = n2; |
| xorjoep | 1:24714b45cd1b | 648 | |
| xorjoep | 1:24714b45cd1b | 649 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 650 | do |
| xorjoep | 1:24714b45cd1b | 651 | { |
| xorjoep | 1:24714b45cd1b | 652 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 653 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 654 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 655 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 656 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 657 | |
| xorjoep | 1:24714b45cd1b | 658 | /* Butterfly implementation */ |
| xorjoep | 1:24714b45cd1b | 659 | xaIn = pSrc[(2U * i0)]; |
| xorjoep | 1:24714b45cd1b | 660 | yaIn = pSrc[(2U * i0) + 1U]; |
| xorjoep | 1:24714b45cd1b | 661 | |
| xorjoep | 1:24714b45cd1b | 662 | xcIn = pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 663 | ycIn = pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 664 | |
| xorjoep | 1:24714b45cd1b | 665 | xbIn = pSrc[(2U * i1)]; |
| xorjoep | 1:24714b45cd1b | 666 | ybIn = pSrc[(2U * i1) + 1U]; |
| xorjoep | 1:24714b45cd1b | 667 | |
| xorjoep | 1:24714b45cd1b | 668 | xdIn = pSrc[(2U * i3)]; |
| xorjoep | 1:24714b45cd1b | 669 | ydIn = pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 670 | |
| xorjoep | 1:24714b45cd1b | 671 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 672 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 673 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 674 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 675 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 676 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 677 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 678 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 679 | |
| xorjoep | 1:24714b45cd1b | 680 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 681 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 682 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 683 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 684 | |
| xorjoep | 1:24714b45cd1b | 685 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 686 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 687 | /* xb - xd */ |
| xorjoep | 1:24714b45cd1b | 688 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 689 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 690 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 691 | /* yb - yd */ |
| xorjoep | 1:24714b45cd1b | 692 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 693 | |
| xorjoep | 1:24714b45cd1b | 694 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 695 | pSrc[(2U * i0)] = Xaplusc + Xbplusd; |
| xorjoep | 1:24714b45cd1b | 696 | |
| xorjoep | 1:24714b45cd1b | 697 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 698 | pSrc[(2U * i0) + 1U] = Yaplusc + Ybplusd; |
| xorjoep | 1:24714b45cd1b | 699 | |
| xorjoep | 1:24714b45cd1b | 700 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 701 | Xb12C_out = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 702 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 703 | Yb12C_out = (Yaminusc + Xbminusd); |
| xorjoep | 1:24714b45cd1b | 704 | /* (xa + xc) - (xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 705 | Xc12C_out = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 706 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 707 | Yc12C_out = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 708 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 709 | Xd12C_out = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 710 | /* (ya - yc) - (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 711 | Yd12C_out = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 712 | |
| xorjoep | 1:24714b45cd1b | 713 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 714 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 715 | |
| xorjoep | 1:24714b45cd1b | 716 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 717 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 718 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 719 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 720 | |
| xorjoep | 1:24714b45cd1b | 721 | Xb12_out = Xb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 722 | Yb12_out = Yb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 723 | Xc12_out = Xc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 724 | Yc12_out = Yc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 725 | Xd12_out = Xd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 726 | Yd12_out = Yd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 727 | |
| xorjoep | 1:24714b45cd1b | 728 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 729 | //Xb12_out -= Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 730 | p0 = Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 731 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 732 | //Yb12_out += Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 733 | p1 = Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 734 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 735 | //Xc12_out -= Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 736 | p2 = Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 737 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 738 | //Yc12_out += Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 739 | p3 = Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 740 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 741 | //Xd12_out -= Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 742 | p4 = Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 743 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 744 | //Yd12_out += Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 745 | p5 = Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 746 | |
| xorjoep | 1:24714b45cd1b | 747 | Xb12_out -= p0; |
| xorjoep | 1:24714b45cd1b | 748 | Yb12_out += p1; |
| xorjoep | 1:24714b45cd1b | 749 | Xc12_out -= p2; |
| xorjoep | 1:24714b45cd1b | 750 | Yc12_out += p3; |
| xorjoep | 1:24714b45cd1b | 751 | Xd12_out -= p4; |
| xorjoep | 1:24714b45cd1b | 752 | Yd12_out += p5; |
| xorjoep | 1:24714b45cd1b | 753 | |
| xorjoep | 1:24714b45cd1b | 754 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 755 | pSrc[2U * i1] = Xc12_out; |
| xorjoep | 1:24714b45cd1b | 756 | |
| xorjoep | 1:24714b45cd1b | 757 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 758 | pSrc[(2U * i1) + 1U] = Yc12_out; |
| xorjoep | 1:24714b45cd1b | 759 | |
| xorjoep | 1:24714b45cd1b | 760 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 761 | pSrc[2U * i2] = Xb12_out; |
| xorjoep | 1:24714b45cd1b | 762 | |
| xorjoep | 1:24714b45cd1b | 763 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 764 | pSrc[(2U * i2) + 1U] = Yb12_out; |
| xorjoep | 1:24714b45cd1b | 765 | |
| xorjoep | 1:24714b45cd1b | 766 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 767 | pSrc[2U * i3] = Xd12_out; |
| xorjoep | 1:24714b45cd1b | 768 | |
| xorjoep | 1:24714b45cd1b | 769 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 770 | pSrc[(2U * i3) + 1U] = Yd12_out; |
| xorjoep | 1:24714b45cd1b | 771 | |
| xorjoep | 1:24714b45cd1b | 772 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 773 | ia1 = ia1 + twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 774 | |
| xorjoep | 1:24714b45cd1b | 775 | /* Updating input index */ |
| xorjoep | 1:24714b45cd1b | 776 | i0 = i0 + 1U; |
| xorjoep | 1:24714b45cd1b | 777 | |
| xorjoep | 1:24714b45cd1b | 778 | } while (--j); |
| xorjoep | 1:24714b45cd1b | 779 | |
| xorjoep | 1:24714b45cd1b | 780 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 781 | |
| xorjoep | 1:24714b45cd1b | 782 | /* Calculation of second stage to excluding last stage */ |
| xorjoep | 1:24714b45cd1b | 783 | for (k = fftLen >> 2U; k > 4U; k >>= 2U) |
| xorjoep | 1:24714b45cd1b | 784 | { |
| xorjoep | 1:24714b45cd1b | 785 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 786 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 787 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 788 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 789 | |
| xorjoep | 1:24714b45cd1b | 790 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 791 | j = 0; |
| xorjoep | 1:24714b45cd1b | 792 | do |
| xorjoep | 1:24714b45cd1b | 793 | { |
| xorjoep | 1:24714b45cd1b | 794 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 795 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 796 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 797 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 798 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 799 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 800 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 801 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 802 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 803 | |
| xorjoep | 1:24714b45cd1b | 804 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 805 | ia1 = ia1 + twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 806 | |
| xorjoep | 1:24714b45cd1b | 807 | i0 = j; |
| xorjoep | 1:24714b45cd1b | 808 | do |
| xorjoep | 1:24714b45cd1b | 809 | { |
| xorjoep | 1:24714b45cd1b | 810 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 811 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 812 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 813 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 814 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 815 | |
| xorjoep | 1:24714b45cd1b | 816 | xaIn = pSrc[(2U * i0)]; |
| xorjoep | 1:24714b45cd1b | 817 | yaIn = pSrc[(2U * i0) + 1U]; |
| xorjoep | 1:24714b45cd1b | 818 | |
| xorjoep | 1:24714b45cd1b | 819 | xbIn = pSrc[(2U * i1)]; |
| xorjoep | 1:24714b45cd1b | 820 | ybIn = pSrc[(2U * i1) + 1U]; |
| xorjoep | 1:24714b45cd1b | 821 | |
| xorjoep | 1:24714b45cd1b | 822 | xcIn = pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 823 | ycIn = pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 824 | |
| xorjoep | 1:24714b45cd1b | 825 | xdIn = pSrc[(2U * i3)]; |
| xorjoep | 1:24714b45cd1b | 826 | ydIn = pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 827 | |
| xorjoep | 1:24714b45cd1b | 828 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 829 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 830 | /* (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 831 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 832 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 833 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 834 | /* (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 835 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 836 | |
| xorjoep | 1:24714b45cd1b | 837 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 838 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 839 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 840 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 841 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 842 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 843 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 844 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 845 | |
| xorjoep | 1:24714b45cd1b | 846 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 847 | Xb12C_out = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 848 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 849 | Yb12C_out = (Yaminusc + Xbminusd); |
| xorjoep | 1:24714b45cd1b | 850 | /* xa + xc -(xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 851 | Xc12C_out = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 852 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 853 | Yc12C_out = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 854 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 855 | Xd12C_out = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 856 | /* (ya - yc) - (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 857 | Yd12C_out = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 858 | |
| xorjoep | 1:24714b45cd1b | 859 | pSrc[(2U * i0)] = Xaplusc + Xbplusd; |
| xorjoep | 1:24714b45cd1b | 860 | pSrc[(2U * i0) + 1U] = Yaplusc + Ybplusd; |
| xorjoep | 1:24714b45cd1b | 861 | |
| xorjoep | 1:24714b45cd1b | 862 | Xb12_out = Xb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 863 | Yb12_out = Yb12C_out * co1; |
| xorjoep | 1:24714b45cd1b | 864 | Xc12_out = Xc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 865 | Yc12_out = Yc12C_out * co2; |
| xorjoep | 1:24714b45cd1b | 866 | Xd12_out = Xd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 867 | Yd12_out = Yd12C_out * co3; |
| xorjoep | 1:24714b45cd1b | 868 | |
| xorjoep | 1:24714b45cd1b | 869 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 870 | //Xb12_out -= Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 871 | p0 = Yb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 872 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 873 | //Yb12_out += Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 874 | p1 = Xb12C_out * si1; |
| xorjoep | 1:24714b45cd1b | 875 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 876 | //Xc12_out -= Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 877 | p2 = Yc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 878 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 879 | //Yc12_out += Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 880 | p3 = Xc12C_out * si2; |
| xorjoep | 1:24714b45cd1b | 881 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 882 | //Xd12_out -= Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 883 | p4 = Yd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 884 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 885 | //Yd12_out += Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 886 | p5 = Xd12C_out * si3; |
| xorjoep | 1:24714b45cd1b | 887 | |
| xorjoep | 1:24714b45cd1b | 888 | Xb12_out -= p0; |
| xorjoep | 1:24714b45cd1b | 889 | Yb12_out += p1; |
| xorjoep | 1:24714b45cd1b | 890 | Xc12_out -= p2; |
| xorjoep | 1:24714b45cd1b | 891 | Yc12_out += p3; |
| xorjoep | 1:24714b45cd1b | 892 | Xd12_out -= p4; |
| xorjoep | 1:24714b45cd1b | 893 | Yd12_out += p5; |
| xorjoep | 1:24714b45cd1b | 894 | |
| xorjoep | 1:24714b45cd1b | 895 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 896 | pSrc[2U * i1] = Xc12_out; |
| xorjoep | 1:24714b45cd1b | 897 | |
| xorjoep | 1:24714b45cd1b | 898 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 899 | pSrc[(2U * i1) + 1U] = Yc12_out; |
| xorjoep | 1:24714b45cd1b | 900 | |
| xorjoep | 1:24714b45cd1b | 901 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 902 | pSrc[2U * i2] = Xb12_out; |
| xorjoep | 1:24714b45cd1b | 903 | |
| xorjoep | 1:24714b45cd1b | 904 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 905 | pSrc[(2U * i2) + 1U] = Yb12_out; |
| xorjoep | 1:24714b45cd1b | 906 | |
| xorjoep | 1:24714b45cd1b | 907 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 908 | pSrc[2U * i3] = Xd12_out; |
| xorjoep | 1:24714b45cd1b | 909 | |
| xorjoep | 1:24714b45cd1b | 910 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 911 | pSrc[(2U * i3) + 1U] = Yd12_out; |
| xorjoep | 1:24714b45cd1b | 912 | |
| xorjoep | 1:24714b45cd1b | 913 | i0 += n1; |
| xorjoep | 1:24714b45cd1b | 914 | } while (i0 < fftLen); |
| xorjoep | 1:24714b45cd1b | 915 | j++; |
| xorjoep | 1:24714b45cd1b | 916 | } while (j <= (n2 - 1U)); |
| xorjoep | 1:24714b45cd1b | 917 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 918 | } |
| xorjoep | 1:24714b45cd1b | 919 | /* Initializations of last stage */ |
| xorjoep | 1:24714b45cd1b | 920 | |
| xorjoep | 1:24714b45cd1b | 921 | j = fftLen >> 2; |
| xorjoep | 1:24714b45cd1b | 922 | ptr1 = &pSrc[0]; |
| xorjoep | 1:24714b45cd1b | 923 | |
| xorjoep | 1:24714b45cd1b | 924 | /* Calculations of last stage */ |
| xorjoep | 1:24714b45cd1b | 925 | do |
| xorjoep | 1:24714b45cd1b | 926 | { |
| xorjoep | 1:24714b45cd1b | 927 | xaIn = ptr1[0]; |
| xorjoep | 1:24714b45cd1b | 928 | yaIn = ptr1[1]; |
| xorjoep | 1:24714b45cd1b | 929 | xbIn = ptr1[2]; |
| xorjoep | 1:24714b45cd1b | 930 | ybIn = ptr1[3]; |
| xorjoep | 1:24714b45cd1b | 931 | xcIn = ptr1[4]; |
| xorjoep | 1:24714b45cd1b | 932 | ycIn = ptr1[5]; |
| xorjoep | 1:24714b45cd1b | 933 | xdIn = ptr1[6]; |
| xorjoep | 1:24714b45cd1b | 934 | ydIn = ptr1[7]; |
| xorjoep | 1:24714b45cd1b | 935 | |
| xorjoep | 1:24714b45cd1b | 936 | /* Butterfly implementation */ |
| xorjoep | 1:24714b45cd1b | 937 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 938 | Xaplusc = xaIn + xcIn; |
| xorjoep | 1:24714b45cd1b | 939 | |
| xorjoep | 1:24714b45cd1b | 940 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 941 | Xaminusc = xaIn - xcIn; |
| xorjoep | 1:24714b45cd1b | 942 | |
| xorjoep | 1:24714b45cd1b | 943 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 944 | Yaplusc = yaIn + ycIn; |
| xorjoep | 1:24714b45cd1b | 945 | |
| xorjoep | 1:24714b45cd1b | 946 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 947 | Yaminusc = yaIn - ycIn; |
| xorjoep | 1:24714b45cd1b | 948 | |
| xorjoep | 1:24714b45cd1b | 949 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 950 | Xbplusd = xbIn + xdIn; |
| xorjoep | 1:24714b45cd1b | 951 | |
| xorjoep | 1:24714b45cd1b | 952 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 953 | Ybplusd = ybIn + ydIn; |
| xorjoep | 1:24714b45cd1b | 954 | |
| xorjoep | 1:24714b45cd1b | 955 | /* (xb-xd) */ |
| xorjoep | 1:24714b45cd1b | 956 | Xbminusd = xbIn - xdIn; |
| xorjoep | 1:24714b45cd1b | 957 | |
| xorjoep | 1:24714b45cd1b | 958 | /* (yb-yd) */ |
| xorjoep | 1:24714b45cd1b | 959 | Ybminusd = ybIn - ydIn; |
| xorjoep | 1:24714b45cd1b | 960 | |
| xorjoep | 1:24714b45cd1b | 961 | /* xa' = (xa+xb+xc+xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 962 | a0 = (Xaplusc + Xbplusd); |
| xorjoep | 1:24714b45cd1b | 963 | /* ya' = (ya+yb+yc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 964 | a1 = (Yaplusc + Ybplusd); |
| xorjoep | 1:24714b45cd1b | 965 | /* xc' = (xa-xb+xc-xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 966 | a2 = (Xaplusc - Xbplusd); |
| xorjoep | 1:24714b45cd1b | 967 | /* yc' = (ya-yb+yc-yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 968 | a3 = (Yaplusc - Ybplusd); |
| xorjoep | 1:24714b45cd1b | 969 | /* xb' = (xa-yb-xc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 970 | a4 = (Xaminusc - Ybminusd); |
| xorjoep | 1:24714b45cd1b | 971 | /* yb' = (ya+xb-yc-xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 972 | a5 = (Yaminusc + Xbminusd); |
| xorjoep | 1:24714b45cd1b | 973 | /* xd' = (xa-yb-xc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 974 | a6 = (Xaminusc + Ybminusd); |
| xorjoep | 1:24714b45cd1b | 975 | /* yd' = (ya-xb-yc+xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 976 | a7 = (Yaminusc - Xbminusd); |
| xorjoep | 1:24714b45cd1b | 977 | |
| xorjoep | 1:24714b45cd1b | 978 | p0 = a0 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 979 | p1 = a1 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 980 | p2 = a2 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 981 | p3 = a3 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 982 | p4 = a4 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 983 | p5 = a5 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 984 | p6 = a6 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 985 | p7 = a7 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 986 | |
| xorjoep | 1:24714b45cd1b | 987 | /* xa' = (xa+xb+xc+xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 988 | ptr1[0] = p0; |
| xorjoep | 1:24714b45cd1b | 989 | /* ya' = (ya+yb+yc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 990 | ptr1[1] = p1; |
| xorjoep | 1:24714b45cd1b | 991 | /* xc' = (xa-xb+xc-xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 992 | ptr1[2] = p2; |
| xorjoep | 1:24714b45cd1b | 993 | /* yc' = (ya-yb+yc-yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 994 | ptr1[3] = p3; |
| xorjoep | 1:24714b45cd1b | 995 | /* xb' = (xa-yb-xc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 996 | ptr1[4] = p4; |
| xorjoep | 1:24714b45cd1b | 997 | /* yb' = (ya+xb-yc-xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 998 | ptr1[5] = p5; |
| xorjoep | 1:24714b45cd1b | 999 | /* xd' = (xa-yb-xc+yd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 1000 | ptr1[6] = p6; |
| xorjoep | 1:24714b45cd1b | 1001 | /* yd' = (ya-xb-yc+xd) * onebyfftLen */ |
| xorjoep | 1:24714b45cd1b | 1002 | ptr1[7] = p7; |
| xorjoep | 1:24714b45cd1b | 1003 | |
| xorjoep | 1:24714b45cd1b | 1004 | /* increment source pointer by 8 for next calculations */ |
| xorjoep | 1:24714b45cd1b | 1005 | ptr1 = ptr1 + 8U; |
| xorjoep | 1:24714b45cd1b | 1006 | |
| xorjoep | 1:24714b45cd1b | 1007 | } while (--j); |
| xorjoep | 1:24714b45cd1b | 1008 | |
| xorjoep | 1:24714b45cd1b | 1009 | #else |
| xorjoep | 1:24714b45cd1b | 1010 | |
| xorjoep | 1:24714b45cd1b | 1011 | float32_t t1, t2, r1, r2, s1, s2; |
| xorjoep | 1:24714b45cd1b | 1012 | |
| xorjoep | 1:24714b45cd1b | 1013 | /* Run the below code for Cortex-M0 */ |
| xorjoep | 1:24714b45cd1b | 1014 | |
| xorjoep | 1:24714b45cd1b | 1015 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 1016 | n2 = fftLen; |
| xorjoep | 1:24714b45cd1b | 1017 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 1018 | |
| xorjoep | 1:24714b45cd1b | 1019 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 1020 | for (k = fftLen; k > 4U; k >>= 2U) |
| xorjoep | 1:24714b45cd1b | 1021 | { |
| xorjoep | 1:24714b45cd1b | 1022 | /* Initializations for the first stage */ |
| xorjoep | 1:24714b45cd1b | 1023 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 1024 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 1025 | ia1 = 0U; |
| xorjoep | 1:24714b45cd1b | 1026 | |
| xorjoep | 1:24714b45cd1b | 1027 | /* Calculation of first stage */ |
| xorjoep | 1:24714b45cd1b | 1028 | j = 0; |
| xorjoep | 1:24714b45cd1b | 1029 | do |
| xorjoep | 1:24714b45cd1b | 1030 | { |
| xorjoep | 1:24714b45cd1b | 1031 | /* index calculation for the coefficients */ |
| xorjoep | 1:24714b45cd1b | 1032 | ia2 = ia1 + ia1; |
| xorjoep | 1:24714b45cd1b | 1033 | ia3 = ia2 + ia1; |
| xorjoep | 1:24714b45cd1b | 1034 | co1 = pCoef[ia1 * 2U]; |
| xorjoep | 1:24714b45cd1b | 1035 | si1 = pCoef[(ia1 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1036 | co2 = pCoef[ia2 * 2U]; |
| xorjoep | 1:24714b45cd1b | 1037 | si2 = pCoef[(ia2 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1038 | co3 = pCoef[ia3 * 2U]; |
| xorjoep | 1:24714b45cd1b | 1039 | si3 = pCoef[(ia3 * 2U) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1040 | |
| xorjoep | 1:24714b45cd1b | 1041 | /* Twiddle coefficients index modifier */ |
| xorjoep | 1:24714b45cd1b | 1042 | ia1 = ia1 + twidCoefModifier; |
| xorjoep | 1:24714b45cd1b | 1043 | |
| xorjoep | 1:24714b45cd1b | 1044 | i0 = j; |
| xorjoep | 1:24714b45cd1b | 1045 | do |
| xorjoep | 1:24714b45cd1b | 1046 | { |
| xorjoep | 1:24714b45cd1b | 1047 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 1048 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 1049 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 1050 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 1051 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 1052 | |
| xorjoep | 1:24714b45cd1b | 1053 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 1054 | r1 = pSrc[(2U * i0)] + pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 1055 | |
| xorjoep | 1:24714b45cd1b | 1056 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 1057 | r2 = pSrc[(2U * i0)] - pSrc[(2U * i2)]; |
| xorjoep | 1:24714b45cd1b | 1058 | |
| xorjoep | 1:24714b45cd1b | 1059 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 1060 | s1 = pSrc[(2U * i0) + 1U] + pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1061 | |
| xorjoep | 1:24714b45cd1b | 1062 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 1063 | s2 = pSrc[(2U * i0) + 1U] - pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1064 | |
| xorjoep | 1:24714b45cd1b | 1065 | /* xb + xd */ |
| xorjoep | 1:24714b45cd1b | 1066 | t1 = pSrc[2U * i1] + pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 1067 | |
| xorjoep | 1:24714b45cd1b | 1068 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 1069 | pSrc[2U * i0] = r1 + t1; |
| xorjoep | 1:24714b45cd1b | 1070 | |
| xorjoep | 1:24714b45cd1b | 1071 | /* xa + xc -(xb + xd) */ |
| xorjoep | 1:24714b45cd1b | 1072 | r1 = r1 - t1; |
| xorjoep | 1:24714b45cd1b | 1073 | |
| xorjoep | 1:24714b45cd1b | 1074 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 1075 | t2 = pSrc[(2U * i1) + 1U] + pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1076 | |
| xorjoep | 1:24714b45cd1b | 1077 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 1078 | pSrc[(2U * i0) + 1U] = s1 + t2; |
| xorjoep | 1:24714b45cd1b | 1079 | |
| xorjoep | 1:24714b45cd1b | 1080 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 1081 | s1 = s1 - t2; |
| xorjoep | 1:24714b45cd1b | 1082 | |
| xorjoep | 1:24714b45cd1b | 1083 | /* (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 1084 | t1 = pSrc[(2U * i1) + 1U] - pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1085 | |
| xorjoep | 1:24714b45cd1b | 1086 | /* (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 1087 | t2 = pSrc[2U * i1] - pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 1088 | |
| xorjoep | 1:24714b45cd1b | 1089 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 1090 | pSrc[2U * i1] = (r1 * co2) - (s1 * si2); |
| xorjoep | 1:24714b45cd1b | 1091 | |
| xorjoep | 1:24714b45cd1b | 1092 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 1093 | pSrc[(2U * i1) + 1U] = (s1 * co2) + (r1 * si2); |
| xorjoep | 1:24714b45cd1b | 1094 | |
| xorjoep | 1:24714b45cd1b | 1095 | /* (xa - xc) - (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 1096 | r1 = r2 - t1; |
| xorjoep | 1:24714b45cd1b | 1097 | |
| xorjoep | 1:24714b45cd1b | 1098 | /* (xa - xc) + (yb - yd) */ |
| xorjoep | 1:24714b45cd1b | 1099 | r2 = r2 + t1; |
| xorjoep | 1:24714b45cd1b | 1100 | |
| xorjoep | 1:24714b45cd1b | 1101 | /* (ya - yc) + (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 1102 | s1 = s2 + t2; |
| xorjoep | 1:24714b45cd1b | 1103 | |
| xorjoep | 1:24714b45cd1b | 1104 | /* (ya - yc) - (xb - xd) */ |
| xorjoep | 1:24714b45cd1b | 1105 | s2 = s2 - t2; |
| xorjoep | 1:24714b45cd1b | 1106 | |
| xorjoep | 1:24714b45cd1b | 1107 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 1108 | pSrc[2U * i2] = (r1 * co1) - (s1 * si1); |
| xorjoep | 1:24714b45cd1b | 1109 | |
| xorjoep | 1:24714b45cd1b | 1110 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 1111 | pSrc[(2U * i2) + 1U] = (s1 * co1) + (r1 * si1); |
| xorjoep | 1:24714b45cd1b | 1112 | |
| xorjoep | 1:24714b45cd1b | 1113 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 1114 | pSrc[2U * i3] = (r2 * co3) - (s2 * si3); |
| xorjoep | 1:24714b45cd1b | 1115 | |
| xorjoep | 1:24714b45cd1b | 1116 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 1117 | pSrc[(2U * i3) + 1U] = (s2 * co3) + (r2 * si3); |
| xorjoep | 1:24714b45cd1b | 1118 | |
| xorjoep | 1:24714b45cd1b | 1119 | i0 += n1; |
| xorjoep | 1:24714b45cd1b | 1120 | } while ( i0 < fftLen); |
| xorjoep | 1:24714b45cd1b | 1121 | j++; |
| xorjoep | 1:24714b45cd1b | 1122 | } while (j <= (n2 - 1U)); |
| xorjoep | 1:24714b45cd1b | 1123 | twidCoefModifier <<= 2U; |
| xorjoep | 1:24714b45cd1b | 1124 | } |
| xorjoep | 1:24714b45cd1b | 1125 | /* Initializations of last stage */ |
| xorjoep | 1:24714b45cd1b | 1126 | n1 = n2; |
| xorjoep | 1:24714b45cd1b | 1127 | n2 >>= 2U; |
| xorjoep | 1:24714b45cd1b | 1128 | |
| xorjoep | 1:24714b45cd1b | 1129 | /* Calculations of last stage */ |
| xorjoep | 1:24714b45cd1b | 1130 | for (i0 = 0U; i0 <= (fftLen - n1); i0 += n1) |
| xorjoep | 1:24714b45cd1b | 1131 | { |
| xorjoep | 1:24714b45cd1b | 1132 | /* index calculation for the input as, */ |
| xorjoep | 1:24714b45cd1b | 1133 | /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ |
| xorjoep | 1:24714b45cd1b | 1134 | i1 = i0 + n2; |
| xorjoep | 1:24714b45cd1b | 1135 | i2 = i1 + n2; |
| xorjoep | 1:24714b45cd1b | 1136 | i3 = i2 + n2; |
| xorjoep | 1:24714b45cd1b | 1137 | |
| xorjoep | 1:24714b45cd1b | 1138 | /* Butterfly implementation */ |
| xorjoep | 1:24714b45cd1b | 1139 | /* xa + xc */ |
| xorjoep | 1:24714b45cd1b | 1140 | r1 = pSrc[2U * i0] + pSrc[2U * i2]; |
| xorjoep | 1:24714b45cd1b | 1141 | |
| xorjoep | 1:24714b45cd1b | 1142 | /* xa - xc */ |
| xorjoep | 1:24714b45cd1b | 1143 | r2 = pSrc[2U * i0] - pSrc[2U * i2]; |
| xorjoep | 1:24714b45cd1b | 1144 | |
| xorjoep | 1:24714b45cd1b | 1145 | /* ya + yc */ |
| xorjoep | 1:24714b45cd1b | 1146 | s1 = pSrc[(2U * i0) + 1U] + pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1147 | |
| xorjoep | 1:24714b45cd1b | 1148 | /* ya - yc */ |
| xorjoep | 1:24714b45cd1b | 1149 | s2 = pSrc[(2U * i0) + 1U] - pSrc[(2U * i2) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1150 | |
| xorjoep | 1:24714b45cd1b | 1151 | /* xc + xd */ |
| xorjoep | 1:24714b45cd1b | 1152 | t1 = pSrc[2U * i1] + pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 1153 | |
| xorjoep | 1:24714b45cd1b | 1154 | /* xa' = xa + xb + xc + xd */ |
| xorjoep | 1:24714b45cd1b | 1155 | pSrc[2U * i0] = (r1 + t1) * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1156 | |
| xorjoep | 1:24714b45cd1b | 1157 | /* (xa + xb) - (xc + xd) */ |
| xorjoep | 1:24714b45cd1b | 1158 | r1 = r1 - t1; |
| xorjoep | 1:24714b45cd1b | 1159 | |
| xorjoep | 1:24714b45cd1b | 1160 | /* yb + yd */ |
| xorjoep | 1:24714b45cd1b | 1161 | t2 = pSrc[(2U * i1) + 1U] + pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1162 | |
| xorjoep | 1:24714b45cd1b | 1163 | /* ya' = ya + yb + yc + yd */ |
| xorjoep | 1:24714b45cd1b | 1164 | pSrc[(2U * i0) + 1U] = (s1 + t2) * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1165 | |
| xorjoep | 1:24714b45cd1b | 1166 | /* (ya + yc) - (yb + yd) */ |
| xorjoep | 1:24714b45cd1b | 1167 | s1 = s1 - t2; |
| xorjoep | 1:24714b45cd1b | 1168 | |
| xorjoep | 1:24714b45cd1b | 1169 | /* (yb-yd) */ |
| xorjoep | 1:24714b45cd1b | 1170 | t1 = pSrc[(2U * i1) + 1U] - pSrc[(2U * i3) + 1U]; |
| xorjoep | 1:24714b45cd1b | 1171 | |
| xorjoep | 1:24714b45cd1b | 1172 | /* (xb-xd) */ |
| xorjoep | 1:24714b45cd1b | 1173 | t2 = pSrc[2U * i1] - pSrc[2U * i3]; |
| xorjoep | 1:24714b45cd1b | 1174 | |
| xorjoep | 1:24714b45cd1b | 1175 | /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 1176 | pSrc[2U * i1] = r1 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1177 | |
| xorjoep | 1:24714b45cd1b | 1178 | /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ |
| xorjoep | 1:24714b45cd1b | 1179 | pSrc[(2U * i1) + 1U] = s1 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1180 | |
| xorjoep | 1:24714b45cd1b | 1181 | /* (xa - xc) - (yb-yd) */ |
| xorjoep | 1:24714b45cd1b | 1182 | r1 = r2 - t1; |
| xorjoep | 1:24714b45cd1b | 1183 | |
| xorjoep | 1:24714b45cd1b | 1184 | /* (xa - xc) + (yb-yd) */ |
| xorjoep | 1:24714b45cd1b | 1185 | r2 = r2 + t1; |
| xorjoep | 1:24714b45cd1b | 1186 | |
| xorjoep | 1:24714b45cd1b | 1187 | /* (ya - yc) + (xb-xd) */ |
| xorjoep | 1:24714b45cd1b | 1188 | s1 = s2 + t2; |
| xorjoep | 1:24714b45cd1b | 1189 | |
| xorjoep | 1:24714b45cd1b | 1190 | /* (ya - yc) - (xb-xd) */ |
| xorjoep | 1:24714b45cd1b | 1191 | s2 = s2 - t2; |
| xorjoep | 1:24714b45cd1b | 1192 | |
| xorjoep | 1:24714b45cd1b | 1193 | /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 1194 | pSrc[2U * i2] = r1 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1195 | |
| xorjoep | 1:24714b45cd1b | 1196 | /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ |
| xorjoep | 1:24714b45cd1b | 1197 | pSrc[(2U * i2) + 1U] = s1 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1198 | |
| xorjoep | 1:24714b45cd1b | 1199 | /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 1200 | pSrc[2U * i3] = r2 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1201 | |
| xorjoep | 1:24714b45cd1b | 1202 | /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ |
| xorjoep | 1:24714b45cd1b | 1203 | pSrc[(2U * i3) + 1U] = s2 * onebyfftLen; |
| xorjoep | 1:24714b45cd1b | 1204 | } |
| xorjoep | 1:24714b45cd1b | 1205 | |
| xorjoep | 1:24714b45cd1b | 1206 | #endif /* #if defined (ARM_MATH_DSP) */ |
| xorjoep | 1:24714b45cd1b | 1207 | } |
| xorjoep | 1:24714b45cd1b | 1208 | |
| xorjoep | 1:24714b45cd1b | 1209 |