arm_cfft_radix4_q31.c
Go to the documentation of this file.
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2011 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 15. December 2011 00005 * $Revision: V2.0.0 00006 * 00007 * Project: Cortex-R DSP Library 00008 * Title: arm_cfft_radix4_q31.c 00009 * 00010 * Description: This file has function definition of Radix-4 FFT & IFFT function and 00011 * In-place bit reversal using bit reversal table 00012 * 00013 * Target Processor: Cortex-R4/R5 00014 * 00015 * Version 1.0.0 2011/03/08 00016 * Alpha release. 00017 * 00018 * Version 1.0.1 2011/09/30 00019 * Beta release. 00020 * 00021 * Version 2.0.0 2011/12/15 00022 * Final release. 00023 * 00024 * -------------------------------------------------------------------- */ 00025 #include "arm_math.h" 00026 00027 00055 void arm_cfft_radix4_q31( 00056 const arm_cfft_radix4_instance_q31 * S, 00057 q31_t * pSrc) 00058 { 00059 if(S->ifftFlag == 1u) 00060 { 00061 /* Complex IFFT radix-4 */ 00062 arm_radix4_butterfly_inverse_q31(pSrc, S->fftLen, S->pTwiddle, 00063 S->twidCoefModifier); 00064 } 00065 else 00066 { 00067 /* Complex FFT radix-4 */ 00068 arm_radix4_butterfly_q31(pSrc, S->fftLen, S->pTwiddle, 00069 S->twidCoefModifier); 00070 } 00071 00072 00073 if(S->bitReverseFlag == 1u) 00074 { 00075 /* Bit Reversal */ 00076 arm_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); 00077 } 00078 00079 } 00080 00085 /* 00086 * Radix-4 FFT algorithm used is : 00087 * 00088 * Input real and imaginary data: 00089 * x(n) = xa + j * ya 00090 * x(n+N/4 ) = xb + j * yb 00091 * x(n+N/2 ) = xc + j * yc 00092 * x(n+3N 4) = xd + j * yd 00093 * 00094 * 00095 * Output real and imaginary data: 00096 * x(4r) = xa'+ j * ya' 00097 * x(4r+1) = xb'+ j * yb' 00098 * x(4r+2) = xc'+ j * yc' 00099 * x(4r+3) = xd'+ j * yd' 00100 * 00101 * 00102 * Twiddle factors for radix-4 FFT: 00103 * Wn = co1 + j * (- si1) 00104 * W2n = co2 + j * (- si2) 00105 * W3n = co3 + j * (- si3) 00106 * 00107 * Butterfly implementation: 00108 * xa' = xa + xb + xc + xd 00109 * ya' = ya + yb + yc + yd 00110 * xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) 00111 * yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) 00112 * xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) 00113 * yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) 00114 * xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) 00115 * yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) 00116 * 00117 */ 00118 00128 void arm_radix4_butterfly_q31( 00129 q31_t * pSrc, 00130 uint32_t fftLen, 00131 q31_t * pCoef, 00132 uint32_t twidCoefModifier) 00133 { 00134 uint32_t n1, n2, ia1, ia2, ia3, i0, i1, i2, i3, j, k; 00135 q31_t t1, t2, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3; 00136 00137 q31_t xa, xb, xc, xd; 00138 q31_t ya, yb, yc, yd; 00139 q31_t xa_out, xb_out, xc_out, xd_out; 00140 q31_t ya_out, yb_out, yc_out, yd_out; 00141 00142 q31_t *ptr1; 00143 q63_t xaya, xbyb, xcyc, xdyd; 00144 /* Total process is divided into three stages */ 00145 00146 /* process first stage, middle stages, & last stage */ 00147 00148 00149 /* start of first stage process */ 00150 00151 /* Initializations for the first stage */ 00152 n2 = fftLen; 00153 n1 = n2; 00154 /* n2 = fftLen/4 */ 00155 n2 >>= 2u; 00156 i0 = 0u; 00157 ia1 = 0u; 00158 00159 j = n2; 00160 00161 /* Calculation of first stage */ 00162 do 00163 { 00164 /* index calculation for the input as, */ 00165 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00166 i1 = i0 + n2; 00167 i2 = i1 + n2; 00168 i3 = i2 + n2; 00169 00170 /* input is in 1.31(q31) format and provide 4 guard bits for the input */ 00171 00172 /* Butterfly implementation */ 00173 /* xa + xc */ 00174 r1 = (pSrc[(2u * i0)] >> 4u) + (pSrc[(2u * i2)] >> 4u); 00175 /* xa - xc */ 00176 r2 = (pSrc[2u * i0] >> 4u) - (pSrc[2u * i2] >> 4u); 00177 00178 /* xb + xd */ 00179 t1 = (pSrc[2u * i1] >> 4u) + (pSrc[2u * i3] >> 4u); 00180 00181 /* ya + yc */ 00182 s1 = (pSrc[(2u * i0) + 1u] >> 4u) + (pSrc[(2u * i2) + 1u] >> 4u); 00183 /* ya - yc */ 00184 s2 = (pSrc[(2u * i0) + 1u] >> 4u) - (pSrc[(2u * i2) + 1u] >> 4u); 00185 00186 /* xa' = xa + xb + xc + xd */ 00187 pSrc[2u * i0] = (r1 + t1); 00188 /* (xa + xc) - (xb + xd) */ 00189 r1 = r1 - t1; 00190 /* yb + yd */ 00191 t2 = (pSrc[(2u * i1) + 1u] >> 4u) + (pSrc[(2u * i3) + 1u] >> 4u); 00192 00193 00194 /* ya' = ya + yb + yc + yd */ 00195 pSrc[(2u * i0) + 1u] = (s1 + t2); 00196 00197 /* (ya + yc) - (yb + yd) */ 00198 s1 = s1 - t2; 00199 00200 /* yb - yd */ 00201 t1 = (pSrc[(2u * i1) + 1u] >> 4u) - (pSrc[(2u * i3) + 1u] >> 4u); 00202 /* xb - xd */ 00203 t2 = (pSrc[2u * i1] >> 4u) - (pSrc[2u * i3] >> 4u); 00204 00205 /* index calculation for the coefficients */ 00206 ia2 = 2u * ia1; 00207 co2 = pCoef[ia2 * 2u]; 00208 si2 = pCoef[(ia2 * 2u) + 1u]; 00209 00210 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00211 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) + 00212 ((int32_t) (((q63_t) s1 * si2) >> 32))) << 1u; 00213 00214 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00215 pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) - 00216 ((int32_t) (((q63_t) r1 * si2) >> 32))) << 1u; 00217 00218 /* (xa - xc) + (yb - yd) */ 00219 r1 = r2 + t1; 00220 /* (xa - xc) - (yb - yd) */ 00221 r2 = r2 - t1; 00222 00223 /* (ya - yc) - (xb - xd) */ 00224 s1 = s2 - t2; 00225 /* (ya - yc) + (xb - xd) */ 00226 s2 = s2 + t2; 00227 00228 co1 = pCoef[ia1 * 2u]; 00229 si1 = pCoef[(ia1 * 2u) + 1u]; 00230 00231 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00232 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) + 00233 ((int32_t) (((q63_t) s1 * si1) >> 32))) << 1u; 00234 00235 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00236 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) - 00237 ((int32_t) (((q63_t) r1 * si1) >> 32))) << 1u; 00238 00239 /* index calculation for the coefficients */ 00240 ia3 = 3u * ia1; 00241 co3 = pCoef[ia3 * 2u]; 00242 si3 = pCoef[(ia3 * 2u) + 1u]; 00243 00244 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00245 pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) + 00246 ((int32_t) (((q63_t) s2 * si3) >> 32))) << 1u; 00247 00248 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00249 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) - 00250 ((int32_t) (((q63_t) r2 * si3) >> 32))) << 1u; 00251 00252 /* Twiddle coefficients index modifier */ 00253 ia1 = ia1 + twidCoefModifier; 00254 00255 /* Updating input index */ 00256 i0 = i0 + 1u; 00257 00258 } while(--j); 00259 00260 /* end of first stage process */ 00261 00262 /* data is in 5.27(q27) format */ 00263 00264 00265 /* start of Middle stages process */ 00266 00267 00268 /* each stage in middle stages provides two down scaling of the input */ 00269 00270 twidCoefModifier <<= 2u; 00271 00272 00273 for (k = fftLen / 4u; k > 4u; k >>= 2u) 00274 { 00275 /* Initializations for the first stage */ 00276 n1 = n2; 00277 n2 >>= 2u; 00278 ia1 = 0u; 00279 00280 /* Calculation of first stage */ 00281 for (j = 0u; j <= (n2 - 1u); j++) 00282 { 00283 /* index calculation for the coefficients */ 00284 ia2 = ia1 + ia1; 00285 ia3 = ia2 + ia1; 00286 co1 = pCoef[ia1 * 2u]; 00287 si1 = pCoef[(ia1 * 2u) + 1u]; 00288 co2 = pCoef[ia2 * 2u]; 00289 si2 = pCoef[(ia2 * 2u) + 1u]; 00290 co3 = pCoef[ia3 * 2u]; 00291 si3 = pCoef[(ia3 * 2u) + 1u]; 00292 00293 /* Twiddle coefficients index modifier */ 00294 ia1 = ia1 + twidCoefModifier; 00295 00296 for (i0 = j; i0 < fftLen; i0 += n1) 00297 { 00298 /* index calculation for the input as, */ 00299 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00300 i1 = i0 + n2; 00301 i2 = i1 + n2; 00302 i3 = i2 + n2; 00303 00304 /* Butterfly implementation */ 00305 /* xa + xc */ 00306 r1 = pSrc[2u * i0] + pSrc[2u * i2]; 00307 /* xa - xc */ 00308 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00309 00310 /* ya + yc */ 00311 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00312 /* ya - yc */ 00313 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00314 00315 /* xb + xd */ 00316 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00317 00318 /* xa' = xa + xb + xc + xd */ 00319 pSrc[2u * i0] = (r1 + t1) >> 2u; 00320 /* xa + xc -(xb + xd) */ 00321 r1 = r1 - t1; 00322 00323 /* yb + yd */ 00324 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00325 /* ya' = ya + yb + yc + yd */ 00326 pSrc[(2u * i0) + 1u] = (s1 + t2) >> 2u; 00327 00328 /* (ya + yc) - (yb + yd) */ 00329 s1 = s1 - t2; 00330 00331 /* (yb - yd) */ 00332 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00333 /* (xb - xd) */ 00334 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00335 00336 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00337 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) + 00338 ((int32_t) (((q63_t) s1 * si2) >> 32))) >> 1u; 00339 00340 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00341 pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) - 00342 ((int32_t) (((q63_t) r1 * si2) >> 32))) >> 1u; 00343 00344 /* (xa - xc) + (yb - yd) */ 00345 r1 = r2 + t1; 00346 /* (xa - xc) - (yb - yd) */ 00347 r2 = r2 - t1; 00348 00349 /* (ya - yc) - (xb - xd) */ 00350 s1 = s2 - t2; 00351 /* (ya - yc) + (xb - xd) */ 00352 s2 = s2 + t2; 00353 00354 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00355 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) + 00356 ((int32_t) (((q63_t) s1 * si1) >> 32))) >> 1u; 00357 00358 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00359 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) - 00360 ((int32_t) (((q63_t) r1 * si1) >> 32))) >> 1u; 00361 00362 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00363 pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) + 00364 ((int32_t) (((q63_t) s2 * si3) >> 32))) >> 1u; 00365 00366 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00367 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) - 00368 ((int32_t) (((q63_t) r2 * si3) >> 32))) >> 1u; 00369 } 00370 } 00371 twidCoefModifier <<= 2u; 00372 } 00373 00374 /* End of Middle stages process */ 00375 00376 /* data is in 11.21(q21) format for the 1024 point as there are 3 middle stages */ 00377 /* data is in 9.23(q23) format for the 256 point as there are 2 middle stages */ 00378 /* data is in 7.25(q25) format for the 64 point as there are 1 middle stage */ 00379 /* data is in 5.27(q27) format for the 16 point as there are no middle stages */ 00380 00381 /* start of Last stage process */ 00382 /* Initializations for the last stage */ 00383 j = fftLen >> 2; 00384 ptr1 = &pSrc[0]; 00385 00386 /* Calculations of last stage */ 00387 do 00388 { 00389 00390 #ifndef ARM_MATH_BIG_ENDIAN 00391 00392 /* Read xa (real), ya(imag) input */ 00393 xaya = *__SIMD64(ptr1)++; xa = (q31_t)xaya; ya = (q31_t)(xaya >> 32); 00394 00395 /* Read xb (real), yb(imag) input */ 00396 xbyb = *__SIMD64(ptr1)++; xb = (q31_t)xbyb; yb = (q31_t)(xbyb >> 32); 00397 00398 /* Read xc (real), yc(imag) input */ 00399 xcyc = *__SIMD64(ptr1)++; xc = (q31_t)xcyc; yc = (q31_t)(xcyc >> 32); 00400 00401 /* Read xc (real), yc(imag) input */ 00402 xdyd = *__SIMD64(ptr1)++; xd = (q31_t)xdyd; yd = (q31_t)(xdyd >> 32); 00403 00404 #else 00405 00406 /* Read xa (real), ya(imag) input */ 00407 xaya = *__SIMD64(ptr1)++; ya = (q31_t)xaya; xa = (q31_t)(xaya >> 32); 00408 00409 /* Read xb (real), yb(imag) input */ 00410 xbyb = *__SIMD64(ptr1)++; yb = (q31_t)xbyb; xb = (q31_t)(xbyb >> 32); 00411 00412 /* Read xc (real), yc(imag) input */ 00413 xcyc = *__SIMD64(ptr1)++; yc = (q31_t)xcyc; xc = (q31_t)(xcyc >> 32); 00414 00415 /* Read xc (real), yc(imag) input */ 00416 xdyd = *__SIMD64(ptr1)++; yd = (q31_t)xdyd; xd = (q31_t)(xdyd >> 32); 00417 00418 00419 #endif 00420 00421 /* xa' = xa + xb + xc + xd */ 00422 xa_out = xa + xb + xc + xd; 00423 00424 /* ya' = ya + yb + yc + yd */ 00425 ya_out = ya + yb + yc + yd; 00426 00427 /* pointer updation for writing */ 00428 ptr1 = ptr1 - 8u; 00429 00430 /* writing xa' and ya' */ 00431 *ptr1++ = xa_out; 00432 *ptr1++ = ya_out; 00433 00434 xc_out = (xa-xb+xc-xd); 00435 yc_out = (ya-yb+yc-yd); 00436 00437 /* writing xc' and yc' */ 00438 *ptr1++ = xc_out; 00439 *ptr1++ = yc_out; 00440 00441 xb_out = (xa+yb-xc-yd); 00442 yb_out = (ya-xb-yc+xd); 00443 00444 /* writing xb' and yb' */ 00445 *ptr1++ = xb_out; 00446 *ptr1++ = yb_out; 00447 00448 xd_out = (xa-yb-xc+yd); 00449 yd_out = (ya+xb-yc-xd); 00450 00451 /* writing xd' and yd' */ 00452 *ptr1++ = xd_out; 00453 *ptr1++ = yd_out; 00454 00455 00456 }while(--j); 00457 00458 /* output is in 11.21(q21) format for the 1024 point */ 00459 /* output is in 9.23(q23) format for the 256 point */ 00460 /* output is in 7.25(q25) format for the 64 point */ 00461 /* output is in 5.27(q27) format for the 16 point */ 00462 00463 /* End of last stage process */ 00464 00465 } 00466 00467 00478 /* 00479 * Radix-4 IFFT algorithm used is : 00480 * 00481 * CIFFT uses same twiddle coefficients as CFFT Function 00482 * x[k] = x[n] + (j)k * x[n + fftLen/4] + (-1)k * x[n+fftLen/2] + (-j)k * x[n+3*fftLen/4] 00483 * 00484 * 00485 * IFFT is implemented with following changes in equations from FFT 00486 * 00487 * Input real and imaginary data: 00488 * x(n) = xa + j * ya 00489 * x(n+N/4 ) = xb + j * yb 00490 * x(n+N/2 ) = xc + j * yc 00491 * x(n+3N 4) = xd + j * yd 00492 * 00493 * 00494 * Output real and imaginary data: 00495 * x(4r) = xa'+ j * ya' 00496 * x(4r+1) = xb'+ j * yb' 00497 * x(4r+2) = xc'+ j * yc' 00498 * x(4r+3) = xd'+ j * yd' 00499 * 00500 * 00501 * Twiddle factors for radix-4 IFFT: 00502 * Wn = co1 + j * (si1) 00503 * W2n = co2 + j * (si2) 00504 * W3n = co3 + j * (si3) 00505 00506 * The real and imaginary output values for the radix-4 butterfly are 00507 * xa' = xa + xb + xc + xd 00508 * ya' = ya + yb + yc + yd 00509 * xb' = (xa-yb-xc+yd)* co1 - (ya+xb-yc-xd)* (si1) 00510 * yb' = (ya+xb-yc-xd)* co1 + (xa-yb-xc+yd)* (si1) 00511 * xc' = (xa-xb+xc-xd)* co2 - (ya-yb+yc-yd)* (si2) 00512 * yc' = (ya-yb+yc-yd)* co2 + (xa-xb+xc-xd)* (si2) 00513 * xd' = (xa+yb-xc-yd)* co3 - (ya-xb-yc+xd)* (si3) 00514 * yd' = (ya-xb-yc+xd)* co3 + (xa+yb-xc-yd)* (si3) 00515 * 00516 */ 00517 00518 void arm_radix4_butterfly_inverse_q31( 00519 q31_t * pSrc, 00520 uint32_t fftLen, 00521 q31_t * pCoef, 00522 uint32_t twidCoefModifier) 00523 { 00524 uint32_t n1, n2, ia1, ia2, ia3, i0, i1, i2, i3, j, k; 00525 q31_t t1, t2, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3; 00526 q31_t xa, xb, xc, xd; 00527 q31_t ya, yb, yc, yd; 00528 q31_t xa_out, xb_out, xc_out, xd_out; 00529 q31_t ya_out, yb_out, yc_out, yd_out; 00530 00531 q31_t *ptr1; 00532 q63_t xaya, xbyb, xcyc, xdyd; 00533 00534 /* input is be 1.31(q31) format for all FFT sizes */ 00535 /* Total process is divided into three stages */ 00536 /* process first stage, middle stages, & last stage */ 00537 00538 /* Start of first stage process */ 00539 00540 /* Initializations for the first stage */ 00541 n2 = fftLen; 00542 n1 = n2; 00543 /* n2 = fftLen/4 */ 00544 n2 >>= 2u; 00545 i0 = 0u; 00546 ia1 = 0u; 00547 00548 j = n2; 00549 00550 do 00551 { 00552 00553 /* input is in 1.31(q31) format and provide 4 guard bits for the input */ 00554 00555 /* index calculation for the input as, */ 00556 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00557 i1 = i0 + n2; 00558 i2 = i1 + n2; 00559 i3 = i2 + n2; 00560 00561 /* Butterfly implementation */ 00562 /* xa + xc */ 00563 r1 = (pSrc[2u * i0] >> 4u) + (pSrc[2u * i2] >> 4u); 00564 /* xa - xc */ 00565 r2 = (pSrc[2u * i0] >> 4u) - (pSrc[2u * i2] >> 4u); 00566 00567 /* xb + xd */ 00568 t1 = (pSrc[2u * i1] >> 4u) + (pSrc[2u * i3] >> 4u); 00569 00570 /* ya + yc */ 00571 s1 = (pSrc[(2u * i0) + 1u] >> 4u) + (pSrc[(2u * i2) + 1u] >> 4u); 00572 /* ya - yc */ 00573 s2 = (pSrc[(2u * i0) + 1u] >> 4u) - (pSrc[(2u * i2) + 1u] >> 4u); 00574 00575 00576 00577 /* xa' = xa + xb + xc + xd */ 00578 pSrc[2u * i0] = (r1 + t1); 00579 /* (xa + xc) - (xb + xd) */ 00580 r1 = r1 - t1; 00581 /* yb + yd */ 00582 t2 = (pSrc[(2u * i1) + 1u] >> 4u) + (pSrc[(2u * i3) + 1u] >> 4u); 00583 /* ya' = ya + yb + yc + yd */ 00584 pSrc[(2u * i0) + 1u] = (s1 + t2); 00585 00586 /* (ya + yc) - (yb + yd) */ 00587 s1 = s1 - t2; 00588 00589 /* yb - yd */ 00590 t1 = (pSrc[(2u * i1) + 1u] >> 4u) - (pSrc[(2u * i3) + 1u] >> 4u); 00591 /* xb - xd */ 00592 t2 = (pSrc[2u * i1] >> 4u) - (pSrc[2u * i3] >> 4u); 00593 00594 /* index calculation for the coefficients */ 00595 ia2 = 2u * ia1; 00596 co2 = pCoef[ia2 * 2u]; 00597 si2 = pCoef[(ia2 * 2u) + 1u]; 00598 00599 /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ 00600 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) - 00601 ((int32_t) (((q63_t) s1 * si2) >> 32))) << 1u; 00602 00603 /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ 00604 pSrc[2u * i1 + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) + 00605 ((int32_t) (((q63_t) r1 * si2) >> 32))) << 1u; 00606 00607 /* (xa - xc) - (yb - yd) */ 00608 r1 = r2 - t1; 00609 /* (xa - xc) + (yb - yd) */ 00610 r2 = r2 + t1; 00611 00612 /* (ya - yc) + (xb - xd) */ 00613 s1 = s2 + t2; 00614 /* (ya - yc) - (xb - xd) */ 00615 s2 = s2 - t2; 00616 00617 co1 = pCoef[ia1 * 2u]; 00618 si1 = pCoef[(ia1 * 2u) + 1u]; 00619 00620 /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ 00621 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) - 00622 ((int32_t) (((q63_t) s1 * si1) >> 32))) << 1u; 00623 00624 /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ 00625 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) + 00626 ((int32_t) (((q63_t) r1 * si1) >> 32))) << 1u; 00627 00628 /* index calculation for the coefficients */ 00629 ia3 = 3u * ia1; 00630 co3 = pCoef[ia3 * 2u]; 00631 si3 = pCoef[(ia3 * 2u) + 1u]; 00632 00633 /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ 00634 pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) - 00635 ((int32_t) (((q63_t) s2 * si3) >> 32))) << 1u; 00636 00637 /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ 00638 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) + 00639 ((int32_t) (((q63_t) r2 * si3) >> 32))) << 1u; 00640 00641 /* Twiddle coefficients index modifier */ 00642 ia1 = ia1 + twidCoefModifier; 00643 00644 /* Updating input index */ 00645 i0 = i0 + 1u; 00646 00647 } while(--j); 00648 00649 /* data is in 5.27(q27) format */ 00650 /* each stage provides two down scaling of the input */ 00651 00652 00653 /* Start of Middle stages process */ 00654 00655 twidCoefModifier <<= 2u; 00656 00657 /* Calculation of second stage to excluding last stage */ 00658 for (k = fftLen / 4u; k > 4u; k >>= 2u) 00659 { 00660 /* Initializations for the first stage */ 00661 n1 = n2; 00662 n2 >>= 2u; 00663 ia1 = 0u; 00664 00665 for (j = 0; j <= (n2 - 1u); j++) 00666 { 00667 /* index calculation for the coefficients */ 00668 ia2 = ia1 + ia1; 00669 ia3 = ia2 + ia1; 00670 co1 = pCoef[ia1 * 2u]; 00671 si1 = pCoef[(ia1 * 2u) + 1u]; 00672 co2 = pCoef[ia2 * 2u]; 00673 si2 = pCoef[(ia2 * 2u) + 1u]; 00674 co3 = pCoef[ia3 * 2u]; 00675 si3 = pCoef[(ia3 * 2u) + 1u]; 00676 /* Twiddle coefficients index modifier */ 00677 ia1 = ia1 + twidCoefModifier; 00678 00679 for (i0 = j; i0 < fftLen; i0 += n1) 00680 { 00681 /* index calculation for the input as, */ 00682 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00683 i1 = i0 + n2; 00684 i2 = i1 + n2; 00685 i3 = i2 + n2; 00686 00687 /* Butterfly implementation */ 00688 /* xa + xc */ 00689 r1 = pSrc[2u * i0] + pSrc[2u * i2]; 00690 /* xa - xc */ 00691 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00692 00693 /* ya + yc */ 00694 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00695 /* ya - yc */ 00696 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00697 00698 /* xb + xd */ 00699 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00700 00701 /* xa' = xa + xb + xc + xd */ 00702 pSrc[2u * i0] = (r1 + t1) >> 2u; 00703 /* xa + xc -(xb + xd) */ 00704 r1 = r1 - t1; 00705 /* yb + yd */ 00706 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00707 /* ya' = ya + yb + yc + yd */ 00708 pSrc[(2u * i0) + 1u] = (s1 + t2) >> 2u; 00709 00710 /* (ya + yc) - (yb + yd) */ 00711 s1 = s1 - t2; 00712 00713 /* (yb - yd) */ 00714 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00715 /* (xb - xd) */ 00716 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00717 00718 /* xc' = (xa-xb+xc-xd)co2 - (ya-yb+yc-yd)(si2) */ 00719 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32u)) - 00720 ((int32_t) (((q63_t) s1 * si2) >> 32u))) >> 1u; 00721 00722 /* yc' = (ya-yb+yc-yd)co2 + (xa-xb+xc-xd)(si2) */ 00723 pSrc[(2u * i1) + 1u] = 00724 (((int32_t) (((q63_t) s1 * co2) >> 32u)) + 00725 ((int32_t) (((q63_t) r1 * si2) >> 32u))) >> 1u; 00726 00727 /* (xa - xc) - (yb - yd) */ 00728 r1 = r2 - t1; 00729 /* (xa - xc) + (yb - yd) */ 00730 r2 = r2 + t1; 00731 00732 /* (ya - yc) + (xb - xd) */ 00733 s1 = s2 + t2; 00734 /* (ya - yc) - (xb - xd) */ 00735 s2 = s2 - t2; 00736 00737 /* xb' = (xa+yb-xc-yd)co1 - (ya-xb-yc+xd)(si1) */ 00738 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) - 00739 ((int32_t) (((q63_t) s1 * si1) >> 32))) >> 1u; 00740 00741 /* yb' = (ya-xb-yc+xd)co1 + (xa+yb-xc-yd)(si1) */ 00742 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) + 00743 ((int32_t) (((q63_t) r1 * si1) >> 32))) >> 1u; 00744 00745 /* xd' = (xa-yb-xc+yd)co3 - (ya+xb-yc-xd)(si3) */ 00746 pSrc[(2u * i3)] = (((int32_t) (((q63_t) r2 * co3) >> 32)) - 00747 ((int32_t) (((q63_t) s2 * si3) >> 32))) >> 1u; 00748 00749 /* yd' = (ya+xb-yc-xd)co3 + (xa-yb-xc+yd)(si3) */ 00750 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) + 00751 ((int32_t) (((q63_t) r2 * si3) >> 32))) >> 1u; 00752 } 00753 } 00754 twidCoefModifier <<= 2u; 00755 } 00756 00757 /* End of Middle stages process */ 00758 00759 /* data is in 11.21(q21) format for the 1024 point as there are 3 middle stages */ 00760 /* data is in 9.23(q23) format for the 256 point as there are 2 middle stages */ 00761 /* data is in 7.25(q25) format for the 64 point as there are 1 middle stage */ 00762 /* data is in 5.27(q27) format for the 16 point as there are no middle stages */ 00763 00764 00765 /* Start of last stage process */ 00766 00767 00768 /* Initializations for the last stage */ 00769 j = fftLen >> 2; 00770 ptr1 = &pSrc[0]; 00771 00772 /* Calculations of last stage */ 00773 do 00774 { 00775 #ifndef ARM_MATH_BIG_ENDIAN 00776 /* Read xa (real), ya(imag) input */ 00777 xaya = *__SIMD64(ptr1)++; xa = (q31_t)xaya; ya = (q31_t)(xaya >> 32); 00778 00779 /* Read xb (real), yb(imag) input */ 00780 xbyb = *__SIMD64(ptr1)++; xb = (q31_t)xbyb; yb = (q31_t)(xbyb >> 32); 00781 00782 /* Read xc (real), yc(imag) input */ 00783 xcyc = *__SIMD64(ptr1)++; xc = (q31_t)xcyc; yc = (q31_t)(xcyc >> 32); 00784 00785 /* Read xc (real), yc(imag) input */ 00786 xdyd = *__SIMD64(ptr1)++; xd = (q31_t)xdyd; yd = (q31_t)(xdyd >> 32); 00787 00788 #else 00789 00790 /* Read xa (real), ya(imag) input */ 00791 xaya = *__SIMD64(ptr1)++; ya = (q31_t)xaya; xa = (q31_t)(xaya >> 32); 00792 00793 /* Read xb (real), yb(imag) input */ 00794 xbyb = *__SIMD64(ptr1)++; yb = (q31_t)xbyb; xb = (q31_t)(xbyb >> 32); 00795 00796 /* Read xc (real), yc(imag) input */ 00797 xcyc = *__SIMD64(ptr1)++; yc = (q31_t)xcyc; xc = (q31_t)(xcyc >> 32); 00798 00799 /* Read xc (real), yc(imag) input */ 00800 xdyd = *__SIMD64(ptr1)++; yd = (q31_t)xdyd; xd = (q31_t)(xdyd >> 32); 00801 00802 00803 #endif 00804 00805 /* xa' = xa + xb + xc + xd */ 00806 xa_out = xa + xb + xc + xd; 00807 00808 /* ya' = ya + yb + yc + yd */ 00809 ya_out = ya + yb + yc + yd; 00810 00811 /* pointer updation for writing */ 00812 ptr1 = ptr1 - 8u; 00813 00814 /* writing xa' and ya' */ 00815 *ptr1++ = xa_out; 00816 *ptr1++ = ya_out; 00817 00818 xc_out = (xa-xb+xc-xd); 00819 yc_out = (ya-yb+yc-yd); 00820 00821 /* writing xc' and yc' */ 00822 *ptr1++ = xc_out; 00823 *ptr1++ = yc_out; 00824 00825 xb_out = (xa-yb-xc+yd); 00826 yb_out = (ya+xb-yc-xd); 00827 00828 /* writing xb' and yb' */ 00829 *ptr1++ = xb_out; 00830 *ptr1++ = yb_out; 00831 00832 xd_out = (xa+yb-xc-yd); 00833 yd_out = (ya-xb-yc+xd); 00834 00835 /* writing xd' and yd' */ 00836 *ptr1++ = xd_out; 00837 *ptr1++ = yd_out; 00838 00839 00840 }while(--j); 00841 00842 /* output is in 11.21(q21) format for the 1024 point */ 00843 /* output is in 9.23(q23) format for the 256 point */ 00844 /* output is in 7.25(q25) format for the 64 point */ 00845 /* output is in 5.27(q27) format for the 16 point */ 00846 00847 /* End of last stage process */ 00848 } 00849 00850 00851 /* 00852 * @brief In-place bit reversal function. 00853 * @param[in, out] *pSrc points to the in-place buffer of Q31 data type. 00854 * @param[in] fftLen length of the FFT. 00855 * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table 00856 * @param[in] *pBitRevTab points to bit reversal table. 00857 * @return none. 00858 */ 00859 00860 void arm_bitreversal_q31( 00861 q31_t * pSrc, 00862 uint32_t fftLen, 00863 uint16_t bitRevFactor, 00864 uint16_t * pBitRevTable) 00865 { 00866 uint32_t fftLenBy2, fftLenBy2p1, i, j; 00867 q31_t in; 00868 00869 /* Initializations */ 00870 j = 0u; 00871 fftLenBy2 = fftLen / 2u; 00872 fftLenBy2p1 = (fftLen / 2u) + 1u; 00873 00874 /* Bit Reversal Implementation */ 00875 for (i = 0u; i <= (fftLenBy2 - 2u); i += 2u) 00876 { 00877 if(i < j) 00878 { 00879 /* pSrc[i] <-> pSrc[j]; */ 00880 in = pSrc[2u * i]; 00881 pSrc[2u * i] = pSrc[2u * j]; 00882 pSrc[2u * j] = in; 00883 00884 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00885 in = pSrc[(2u * i) + 1u]; 00886 pSrc[(2u * i) + 1u] = pSrc[(2u * j) + 1u]; 00887 pSrc[(2u * j) + 1u] = in; 00888 00889 /* pSrc[i+fftLenBy2p1] <-> pSrc[j+fftLenBy2p1] */ 00890 in = pSrc[2u * (i + fftLenBy2p1)]; 00891 pSrc[2u * (i + fftLenBy2p1)] = pSrc[2u * (j + fftLenBy2p1)]; 00892 pSrc[2u * (j + fftLenBy2p1)] = in; 00893 00894 /* pSrc[i+fftLenBy2p1+1u] <-> pSrc[j+fftLenBy2p1+1u] */ 00895 in = pSrc[(2u * (i + fftLenBy2p1)) + 1u]; 00896 pSrc[(2u * (i + fftLenBy2p1)) + 1u] = 00897 pSrc[(2u * (j + fftLenBy2p1)) + 1u]; 00898 pSrc[(2u * (j + fftLenBy2p1)) + 1u] = in; 00899 00900 } 00901 00902 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00903 in = pSrc[2u * (i + 1u)]; 00904 pSrc[2u * (i + 1u)] = pSrc[2u * (j + fftLenBy2)]; 00905 pSrc[2u * (j + fftLenBy2)] = in; 00906 00907 /* pSrc[i+2u] <-> pSrc[j+2u] */ 00908 in = pSrc[(2u * (i + 1u)) + 1u]; 00909 pSrc[(2u * (i + 1u)) + 1u] = pSrc[(2u * (j + fftLenBy2)) + 1u]; 00910 pSrc[(2u * (j + fftLenBy2)) + 1u] = in; 00911 00912 /* Reading the index for the bit reversal */ 00913 j = *pBitRevTable; 00914 00915 /* Updating the bit reversal index depending on the fft length */ 00916 pBitRevTable += bitRevFactor; 00917 } 00918 }
