arm_cfft_mag_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_mag_q31.c 00009 * 00010 * Description: This file has function definition of Radix-4 FFT with magnitude 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 00056 void arm_cfft_mag_q31( 00057 const arm_cfft_radix4_instance_q31 * S, 00058 q31_t * pSrc) 00059 { 00060 /* Complex FFT Magnitude */ 00061 arm_cfft_mag_butterfly_q31(pSrc, S->fftLen, S->pTwiddle, 00062 S->twidCoefModifier); 00063 00064 00065 if(S->bitReverseFlag == 1u) 00066 { 00067 /* Bit Reversal */ 00068 arm_cfft_mag_bitreversal_q31(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); 00069 } 00070 00071 } 00072 00077 /* 00078 * Radix-4 FFT algorithm used is : 00079 * 00080 * Input real and imaginary data: 00081 * x(n) = xa + j * ya 00082 * x(n+N/4 ) = xb + j * yb 00083 * x(n+N/2 ) = xc + j * yc 00084 * x(n+3N 4) = xd + j * yd 00085 * 00086 * 00087 * Output real and imaginary data: 00088 * x(4r) = xa'+ j * ya' 00089 * x(4r+1) = xb'+ j * yb' 00090 * x(4r+2) = xc'+ j * yc' 00091 * x(4r+3) = xd'+ j * yd' 00092 * 00093 * 00094 * Twiddle factors for radix-4 FFT: 00095 * Wn = co1 + j * (- si1) 00096 * W2n = co2 + j * (- si2) 00097 * W3n = co3 + j * (- si3) 00098 * 00099 * Butterfly implementation: 00100 * xa' = xa + xb + xc + xd 00101 * ya' = ya + yb + yc + yd 00102 * xb' = (xa+yb-xc-yd)* co1 + (ya-xb-yc+xd)* (si1) 00103 * yb' = (ya-xb-yc+xd)* co1 - (xa+yb-xc-yd)* (si1) 00104 * xc' = (xa-xb+xc-xd)* co2 + (ya-yb+yc-yd)* (si2) 00105 * yc' = (ya-yb+yc-yd)* co2 - (xa-xb+xc-xd)* (si2) 00106 * xd' = (xa-yb-xc+yd)* co3 + (ya+xb-yc-xd)* (si3) 00107 * yd' = (ya+xb-yc-xd)* co3 - (xa-yb-xc+yd)* (si3) 00108 * 00109 */ 00110 00120 void arm_cfft_mag_butterfly_q31( 00121 q31_t * pSrc, 00122 uint32_t fftLen, 00123 q31_t * pCoef, 00124 uint32_t twidCoefModifier) 00125 { 00126 uint32_t n1, n2, ia1, ia2, ia3, i0, i1, i2, i3, j, k; 00127 q31_t t1, t2, r1, r2, s1, s2, co1, co2, co3, si1, si2, si3; 00128 00129 q31_t xa, xb, xc, xd; 00130 q31_t ya, yb, yc, yd; 00131 q31_t xa_out, xb_out, xc_out, xd_out; 00132 q31_t ya_out, yb_out, yc_out, yd_out; 00133 00134 q31_t *ptr1, *pDst = &pSrc[0]; 00135 q63_t xaya, xbyb, xcyc, xdyd; 00136 /* Total process is divided into three stages */ 00137 00138 /* process first stage, middle stages, & last stage */ 00139 00140 00141 /* start of first stage process */ 00142 00143 /* Initializations for the first stage */ 00144 n2 = fftLen; 00145 n1 = n2; 00146 /* n2 = fftLen/4 */ 00147 n2 >>= 2u; 00148 i0 = 0u; 00149 ia1 = 0u; 00150 00151 j = n2; 00152 00153 /* Calculation of first stage */ 00154 do 00155 { 00156 /* index calculation for the input as, */ 00157 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00158 i1 = i0 + n2; 00159 i2 = i1 + n2; 00160 i3 = i2 + n2; 00161 00162 /* input is in 1.31(q31) format and provide 4 guard bits for the input */ 00163 00164 /* Butterfly implementation */ 00165 /* xa + xc */ 00166 r1 = (pSrc[(2u * i0)] >> 4u) + (pSrc[(2u * i2)] >> 4u); 00167 /* xa - xc */ 00168 r2 = (pSrc[2u * i0] >> 4u) - (pSrc[2u * i2] >> 4u); 00169 00170 /* xb + xd */ 00171 t1 = (pSrc[2u * i1] >> 4u) + (pSrc[2u * i3] >> 4u); 00172 00173 /* ya + yc */ 00174 s1 = (pSrc[(2u * i0) + 1u] >> 4u) + (pSrc[(2u * i2) + 1u] >> 4u); 00175 /* ya - yc */ 00176 s2 = (pSrc[(2u * i0) + 1u] >> 4u) - (pSrc[(2u * i2) + 1u] >> 4u); 00177 00178 /* xa' = xa + xb + xc + xd */ 00179 pSrc[2u * i0] = (r1 + t1); 00180 /* (xa + xc) - (xb + xd) */ 00181 r1 = r1 - t1; 00182 /* yb + yd */ 00183 t2 = (pSrc[(2u * i1) + 1u] >> 4u) + (pSrc[(2u * i3) + 1u] >> 4u); 00184 00185 00186 /* ya' = ya + yb + yc + yd */ 00187 pSrc[(2u * i0) + 1u] = (s1 + t2); 00188 00189 /* (ya + yc) - (yb + yd) */ 00190 s1 = s1 - t2; 00191 00192 /* yb - yd */ 00193 t1 = (pSrc[(2u * i1) + 1u] >> 4u) - (pSrc[(2u * i3) + 1u] >> 4u); 00194 /* xb - xd */ 00195 t2 = (pSrc[2u * i1] >> 4u) - (pSrc[2u * i3] >> 4u); 00196 00197 /* index calculation for the coefficients */ 00198 ia2 = 2u * ia1; 00199 co2 = pCoef[ia2 * 2u]; 00200 si2 = pCoef[(ia2 * 2u) + 1u]; 00201 00202 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00203 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) + 00204 ((int32_t) (((q63_t) s1 * si2) >> 32))) << 1u; 00205 00206 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00207 pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) - 00208 ((int32_t) (((q63_t) r1 * si2) >> 32))) << 1u; 00209 00210 /* (xa - xc) + (yb - yd) */ 00211 r1 = r2 + t1; 00212 /* (xa - xc) - (yb - yd) */ 00213 r2 = r2 - t1; 00214 00215 /* (ya - yc) - (xb - xd) */ 00216 s1 = s2 - t2; 00217 /* (ya - yc) + (xb - xd) */ 00218 s2 = s2 + t2; 00219 00220 co1 = pCoef[ia1 * 2u]; 00221 si1 = pCoef[(ia1 * 2u) + 1u]; 00222 00223 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00224 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) + 00225 ((int32_t) (((q63_t) s1 * si1) >> 32))) << 1u; 00226 00227 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00228 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) - 00229 ((int32_t) (((q63_t) r1 * si1) >> 32))) << 1u; 00230 00231 /* index calculation for the coefficients */ 00232 ia3 = 3u * ia1; 00233 co3 = pCoef[ia3 * 2u]; 00234 si3 = pCoef[(ia3 * 2u) + 1u]; 00235 00236 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00237 pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) + 00238 ((int32_t) (((q63_t) s2 * si3) >> 32))) << 1u; 00239 00240 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00241 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) - 00242 ((int32_t) (((q63_t) r2 * si3) >> 32))) << 1u; 00243 00244 /* Twiddle coefficients index modifier */ 00245 ia1 = ia1 + twidCoefModifier; 00246 00247 /* Updating input index */ 00248 i0 = i0 + 1u; 00249 00250 } while(--j); 00251 00252 /* end of first stage process */ 00253 00254 /* data is in 5.27(q27) format */ 00255 00256 00257 /* start of Middle stages process */ 00258 00259 00260 /* each stage in middle stages provides two down scaling of the input */ 00261 00262 twidCoefModifier <<= 2u; 00263 00264 00265 for (k = fftLen / 4u; k > 4u; k >>= 2u) 00266 { 00267 /* Initializations for the first stage */ 00268 n1 = n2; 00269 n2 >>= 2u; 00270 ia1 = 0u; 00271 00272 /* Calculation of first stage */ 00273 for (j = 0u; j <= (n2 - 1u); j++) 00274 { 00275 /* index calculation for the coefficients */ 00276 ia2 = ia1 + ia1; 00277 ia3 = ia2 + ia1; 00278 co1 = pCoef[ia1 * 2u]; 00279 si1 = pCoef[(ia1 * 2u) + 1u]; 00280 co2 = pCoef[ia2 * 2u]; 00281 si2 = pCoef[(ia2 * 2u) + 1u]; 00282 co3 = pCoef[ia3 * 2u]; 00283 si3 = pCoef[(ia3 * 2u) + 1u]; 00284 00285 /* Twiddle coefficients index modifier */ 00286 ia1 = ia1 + twidCoefModifier; 00287 00288 for (i0 = j; i0 < fftLen; i0 += n1) 00289 { 00290 /* index calculation for the input as, */ 00291 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2u], pSrc[i0 + 3fftLen/4] */ 00292 i1 = i0 + n2; 00293 i2 = i1 + n2; 00294 i3 = i2 + n2; 00295 00296 /* Butterfly implementation */ 00297 /* xa + xc */ 00298 r1 = pSrc[2u * i0] + pSrc[2u * i2]; 00299 /* xa - xc */ 00300 r2 = pSrc[2u * i0] - pSrc[2u * i2]; 00301 00302 /* ya + yc */ 00303 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u]; 00304 /* ya - yc */ 00305 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u]; 00306 00307 /* xb + xd */ 00308 t1 = pSrc[2u * i1] + pSrc[2u * i3]; 00309 00310 /* xa' = xa + xb + xc + xd */ 00311 pSrc[2u * i0] = (r1 + t1) >> 2u; 00312 /* xa + xc -(xb + xd) */ 00313 r1 = r1 - t1; 00314 00315 /* yb + yd */ 00316 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u]; 00317 /* ya' = ya + yb + yc + yd */ 00318 pSrc[(2u * i0) + 1u] = (s1 + t2) >> 2u; 00319 00320 /* (ya + yc) - (yb + yd) */ 00321 s1 = s1 - t2; 00322 00323 /* (yb - yd) */ 00324 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u]; 00325 /* (xb - xd) */ 00326 t2 = pSrc[2u * i1] - pSrc[2u * i3]; 00327 00328 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00329 pSrc[2u * i1] = (((int32_t) (((q63_t) r1 * co2) >> 32)) + 00330 ((int32_t) (((q63_t) s1 * si2) >> 32))) >> 1u; 00331 00332 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00333 pSrc[(2u * i1) + 1u] = (((int32_t) (((q63_t) s1 * co2) >> 32)) - 00334 ((int32_t) (((q63_t) r1 * si2) >> 32))) >> 1u; 00335 00336 /* (xa - xc) + (yb - yd) */ 00337 r1 = r2 + t1; 00338 /* (xa - xc) - (yb - yd) */ 00339 r2 = r2 - t1; 00340 00341 /* (ya - yc) - (xb - xd) */ 00342 s1 = s2 - t2; 00343 /* (ya - yc) + (xb - xd) */ 00344 s2 = s2 + t2; 00345 00346 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00347 pSrc[2u * i2] = (((int32_t) (((q63_t) r1 * co1) >> 32)) + 00348 ((int32_t) (((q63_t) s1 * si1) >> 32))) >> 1u; 00349 00350 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00351 pSrc[(2u * i2) + 1u] = (((int32_t) (((q63_t) s1 * co1) >> 32)) - 00352 ((int32_t) (((q63_t) r1 * si1) >> 32))) >> 1u; 00353 00354 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00355 pSrc[2u * i3] = (((int32_t) (((q63_t) r2 * co3) >> 32)) + 00356 ((int32_t) (((q63_t) s2 * si3) >> 32))) >> 1u; 00357 00358 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00359 pSrc[(2u * i3) + 1u] = (((int32_t) (((q63_t) s2 * co3) >> 32)) - 00360 ((int32_t) (((q63_t) r2 * si3) >> 32))) >> 1u; 00361 } 00362 } 00363 twidCoefModifier <<= 2u; 00364 } 00365 00366 /* End of Middle stages process */ 00367 00368 /* data is in 11.21(q21) format for the 1024 point as there are 3 middle stages */ 00369 /* data is in 9.23(q23) format for the 256 point as there are 2 middle stages */ 00370 /* data is in 7.25(q25) format for the 64 point as there are 1 middle stage */ 00371 /* data is in 5.27(q27) format for the 16 point as there are no middle stages */ 00372 00373 /* start of Last stage process */ 00374 /* Initializations for the last stage */ 00375 j = fftLen >> 2; 00376 ptr1 = &pSrc[0]; 00377 00378 /* Calculations of last stage */ 00379 do 00380 { 00381 00382 #ifndef ARM_MATH_BIG_ENDIAN 00383 00384 /* Read xa (real), ya(imag) input */ 00385 xaya = *__SIMD64(ptr1)++; xa = (q31_t)xaya; ya = (q31_t)(xaya >> 32); 00386 00387 /* Read xb (real), yb(imag) input */ 00388 xbyb = *__SIMD64(ptr1)++; xb = (q31_t)xbyb; yb = (q31_t)(xbyb >> 32); 00389 00390 /* Read xc (real), yc(imag) input */ 00391 xcyc = *__SIMD64(ptr1)++; xc = (q31_t)xcyc; yc = (q31_t)(xcyc >> 32); 00392 00393 /* Read xc (real), yc(imag) input */ 00394 xdyd = *__SIMD64(ptr1)++; xd = (q31_t)xdyd; yd = (q31_t)(xdyd >> 32); 00395 00396 #else 00397 00398 /* Read xa (real), ya(imag) input */ 00399 xaya = *__SIMD64(ptr1)++; ya = (q31_t)xaya; xa = (q31_t)(xaya >> 32); 00400 00401 /* Read xb (real), yb(imag) input */ 00402 xbyb = *__SIMD64(ptr1)++; yb = (q31_t)xbyb; xb = (q31_t)(xbyb >> 32); 00403 00404 /* Read xc (real), yc(imag) input */ 00405 xcyc = *__SIMD64(ptr1)++; yc = (q31_t)xcyc; xc = (q31_t)(xcyc >> 32); 00406 00407 /* Read xc (real), yc(imag) input */ 00408 xdyd = *__SIMD64(ptr1)++; yd = (q31_t)xdyd; xd = (q31_t)(xdyd >> 32); 00409 00410 00411 #endif 00412 00413 /* xa' = xa + xb + xc + xd */ 00414 xa_out = xa + xb + xc + xd; 00415 00416 /* ya' = ya + yb + yc + yd */ 00417 ya_out = ya + yb + yc + yd; 00418 00419 xc_out = (xa-xb+xc-xd); 00420 yc_out = (ya-yb+yc-yd); 00421 00422 /* writing xa' and ya' */ 00423 xa_out = (q31_t) (((q63_t) xa_out * xa_out) >> 33); 00424 ya_out = (q31_t) (((q63_t) ya_out * ya_out) >> 33); 00425 00426 /* writing xc' and yc' */ 00427 xc_out = (q31_t) (((q63_t) xc_out * xc_out) >> 33); 00428 00429 yc_out = (q31_t) (((q63_t) yc_out * yc_out) >> 33); 00430 00431 arm_sqrt_q31(ya_out+xa_out, pDst++); 00432 arm_sqrt_q31(xc_out+yc_out, pDst++); 00433 00434 xb_out = (xa+yb-xc-yd); 00435 yb_out = (ya-xb-yc+xd); 00436 00437 xd_out = (xa-yb-xc+yd); 00438 yd_out = (ya+xb-yc-xd); 00439 00440 /* writing xb' and yb' */ 00441 xb_out = (q31_t) (((q63_t) xb_out * xb_out) >> 33); 00442 yb_out = (q31_t) (((q63_t) yb_out * yb_out) >> 33); 00443 00444 00445 /* writing xd' and yd' */ 00446 xd_out = (q31_t) (((q63_t) xd_out * xd_out) >> 33); 00447 yd_out = (q31_t) (((q63_t) yd_out * yd_out) >> 33); 00448 00449 arm_sqrt_q31(xb_out+yb_out, pDst++); 00450 arm_sqrt_q31(xd_out+yd_out, pDst++); 00451 00452 00453 }while(--j); 00454 00455 /* output is in 11.21(q21) format for the 1024 point */ 00456 /* output is in 9.23(q23) format for the 256 point */ 00457 /* output is in 7.25(q25) format for the 64 point */ 00458 /* output is in 5.27(q27) format for the 16 point */ 00459 00460 /* End of last stage process */ 00461 00462 } 00463 00464 00465 /* 00466 * @brief In-place bit reversal function. 00467 * @param[in, out] *pSrc points to the in-place buffer of Q31 data type. 00468 * @param[in] fftLen length of the FFT. 00469 * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table 00470 * @param[in] *pBitRevTab points to bit reversal table. 00471 * @return none. 00472 */ 00473 00474 void arm_cfft_mag_bitreversal_q31( 00475 q31_t * pSrc, 00476 uint32_t fftLen, 00477 uint16_t bitRevFactor, 00478 uint16_t * pBitRevTable) 00479 { 00480 uint32_t fftLenBy2, fftLenBy2p1, i, j; 00481 q31_t in; 00482 00483 /* Initializations */ 00484 j = 0u; 00485 fftLenBy2 = fftLen / 2u; 00486 fftLenBy2p1 = (fftLen / 2u) + 1u; 00487 00488 /* Bit Reversal Implementation */ 00489 for (i = 0u; i <= (fftLenBy2-2u); i+=2) 00490 { 00491 if(i < j) 00492 { 00493 /* pSrc[i] <-> pSrc[j]; */ 00494 in = pSrc[i]; 00495 pSrc[i] = pSrc[j]; 00496 pSrc[j] = in; 00497 00498 /* pSrc[i+fftLenBy2p1] <-> pSrc[j+fftLenBy2p1] */ 00499 in = pSrc[(i + fftLenBy2p1)]; 00500 pSrc[(i + fftLenBy2p1)] = pSrc[(j + fftLenBy2p1)]; 00501 pSrc[(j + fftLenBy2p1)] = in; 00502 00503 } 00504 00505 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00506 in = pSrc[(i + 1u)]; 00507 pSrc[(i + 1u)] = pSrc[(j + fftLenBy2)]; 00508 pSrc[(j + fftLenBy2)] = in; 00509 00510 /* Reading the index for the bit reversal */ 00511 j = *pBitRevTable; 00512 00513 /* Updating the bit reversal index depending on the fft length */ 00514 pBitRevTable += bitRevFactor; 00515 } 00516 }
