arm_cfft_mag_f32.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_f32.c 00009 * 00010 * Description: Floating point processing function for Magnitude calculation of CFFT Output 00011 * 00012 * Target Processor: Cortex-R4/R5 00013 * 00014 * Version 1.0.0 2011/03/08 00015 * Alpha release. 00016 * 00017 * Version 1.0.1 2011/09/30 00018 * Beta release. 00019 * 00020 * Version 2.0.0 2011/12/15 00021 * Final release. 00022 * 00023 * -------------------------------------------------------------------- */ 00024 00025 #include "arm_math.h" 00026 00077 void arm_cfft_mag_f32( 00078 const arm_cfft_radix4_instance_f32 * S, 00079 float32_t * pSrc) 00080 { 00081 00082 /* Complex FFT Magnituderadix-4 */ 00083 arm_cfft_mag_butterfly_f32(pSrc, S->fftLen, S->pTwiddle, 00084 S->twidCoefModifier); 00085 00086 if(S->bitReverseFlag == 1u) 00087 { 00088 /* Bit Reversal */ 00089 arm_cfft_mag_bitreversal_f32(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable); 00090 } 00091 00092 } 00093 00094 00101 /* ---------------------------------------------------------------------- 00102 ** Internal helper function used by the FFTs 00103 ** ------------------------------------------------------------------- */ 00104 00105 /* 00106 * @brief Core function for the floating-point CFFT butterfly process. 00107 * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. 00108 * @param[in] fftLen length of the FFT. 00109 * @param[in] *pCoef points to the twiddle coefficient buffer. 00110 * @param[in] twidCoefModifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. 00111 * @return none. 00112 */ 00113 00114 void arm_cfft_mag_butterfly_f32( 00115 float32_t * pSrc, 00116 uint16_t fftLen, 00117 float32_t * pCoef, 00118 uint16_t twidCoefModifier) 00119 { 00120 00121 float32_t co1, co2, co3, si1, si2, si3; 00122 uint32_t ia1, ia2, ia3; 00123 uint32_t i0, i1, i2, i3; 00124 uint32_t n1, n2, j, k; 00125 00126 float32_t xaIn, yaIn, xbIn, ybIn, xcIn, ycIn, xdIn, ydIn; 00127 float32_t Xaplusc, Xbplusd, Yaplusc, Ybplusd, Xaminusc, Xbminusd, Yaminusc, Ybminusd; 00128 float32_t Xb12C_out, Yb12C_out, Xc12C_out, Yc12C_out, Xd12C_out, Yd12C_out; 00129 float32_t Xb12_out, Yb12_out, Xc12_out, Yc12_out, Xd12_out, Yd12_out; 00130 float32_t Xa12_out, Ya12_out; 00131 float32_t *ptr1; 00132 00133 /* Initializations for the first stage */ 00134 n2 = fftLen; 00135 n1 = n2; 00136 00137 /* n2 = fftLen/4 */ 00138 n2 >>= 2u; 00139 i0 = 0u; 00140 ia1 = 0u; 00141 00142 j = n2; 00143 00144 /* Calculation of first stage */ 00145 do 00146 { 00147 /* index calculation for the input as, */ 00148 /* pSrc[i0 + 0], pSrc[i0 + fftLen/4], pSrc[i0 + fftLen/2], pSrc[i0 + 3fftLen/4] */ 00149 i1 = i0 + n2; 00150 i2 = i1 + n2; 00151 i3 = i2 + n2; 00152 00153 xaIn = pSrc[(2u * i0)]; 00154 yaIn = pSrc[(2u * i0) + 1u]; 00155 00156 xcIn = pSrc[(2u * i2)]; 00157 ycIn = pSrc[(2u * i2) + 1u]; 00158 00159 xbIn = pSrc[(2u * i1)]; 00160 ybIn = pSrc[(2u * i1) + 1u]; 00161 00162 xdIn = pSrc[(2u * i3)]; 00163 ydIn = pSrc[(2u * i3) + 1u]; 00164 00165 /* xa + xc */ 00166 Xaplusc = xaIn + xcIn; 00167 /* xb + xd */ 00168 Xbplusd = xbIn + xdIn; 00169 /* ya + yc */ 00170 Yaplusc = yaIn + ycIn; 00171 /* yb + yd */ 00172 Ybplusd = ybIn + ydIn; 00173 00174 /* index calculation for the coefficients */ 00175 ia2 = ia1 + ia1; 00176 co2 = pCoef[ia2 * 2u]; 00177 si2 = pCoef[(ia2 * 2u) + 1u]; 00178 00179 /* xa - xc */ 00180 Xaminusc = xaIn - xcIn; 00181 /* xb - xd */ 00182 Xbminusd = xbIn - xdIn; 00183 /* ya - yc */ 00184 Yaminusc = yaIn - ycIn; 00185 /* yb + yd */ 00186 Ybminusd = ybIn - ydIn; 00187 00188 /* xa' = xa + xb + xc + xd */ 00189 pSrc[(2u * i0)] = Xaplusc + Xbplusd; 00190 /* ya' = ya + yb + yc + yd */ 00191 pSrc[(2u * i0) + 1u] = Yaplusc + Ybplusd; 00192 00193 /* (xa - xc) + (yb - yd) */ 00194 Xb12C_out = (Xaminusc + Ybminusd); 00195 /* (ya - yc) + (xb - xd) */ 00196 Yb12C_out = (Yaminusc - Xbminusd); 00197 /* (xa + xc) - (xb + xd) */ 00198 Xc12C_out = (Xaplusc - Xbplusd); 00199 /* (ya + yc) - (yb + yd) */ 00200 Yc12C_out = (Yaplusc - Ybplusd); 00201 /* (xa - xc) - (yb - yd) */ 00202 Xd12C_out = (Xaminusc - Ybminusd); 00203 /* (ya - yc) + (xb - xd) */ 00204 Yd12C_out = (Xbminusd + Yaminusc); 00205 00206 co1 = pCoef[ia1 * 2u]; 00207 si1 = pCoef[(ia1 * 2u) + 1u]; 00208 00209 /* index calculation for the coefficients */ 00210 ia3 = ia2 + ia1; 00211 co3 = pCoef[ia3 * 2u]; 00212 si3 = pCoef[(ia3 * 2u) + 1u]; 00213 00214 Xb12_out = Xb12C_out * co1; 00215 Yb12_out = Yb12C_out * co1; 00216 Xc12_out = Xc12C_out * co2; 00217 Yc12_out = Yc12C_out * co2; 00218 Xd12_out = Xd12C_out * co3; 00219 Yd12_out = Yd12C_out * co3; 00220 00221 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00222 Xb12_out += Yb12C_out * si1; 00223 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00224 Yb12_out -= Xb12C_out * si1; 00225 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00226 Xc12_out += Yc12C_out * si2; 00227 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00228 Yc12_out -= Xc12C_out * si2; 00229 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00230 Xd12_out += Yd12C_out * si3; 00231 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00232 Yd12_out -= Xd12C_out * si3; 00233 00234 00235 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00236 pSrc[2u * i1] = Xc12_out; 00237 00238 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00239 pSrc[(2u * i1) + 1u] = Yc12_out; 00240 00241 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00242 pSrc[2u * i2] = Xb12_out; 00243 00244 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00245 pSrc[(2u * i2) + 1u] = Yb12_out; 00246 00247 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00248 pSrc[2u * i3] = Xd12_out; 00249 00250 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00251 pSrc[(2u * i3) + 1u] = Yd12_out; 00252 00253 /* Twiddle coefficients index modifier */ 00254 ia1 = ia1 + twidCoefModifier; 00255 00256 /* Updating input index */ 00257 i0 = i0 + 1u; 00258 00259 } 00260 while(--j); 00261 00262 twidCoefModifier <<= 2u; 00263 00264 /* Calculation of second stage to excluding last stage */ 00265 for (k = fftLen / 4; 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/2], pSrc[i0 + 3fftLen/4] */ 00292 i1 = i0 + n2; 00293 i2 = i1 + n2; 00294 i3 = i2 + n2; 00295 00296 xaIn = pSrc[(2u * i0)]; 00297 yaIn = pSrc[(2u * i0) + 1u]; 00298 00299 xbIn = pSrc[(2u * i1)]; 00300 ybIn = pSrc[(2u * i1) + 1u]; 00301 00302 xcIn = pSrc[(2u * i2)]; 00303 ycIn = pSrc[(2u * i2) + 1u]; 00304 00305 xdIn = pSrc[(2u * i3)]; 00306 ydIn = pSrc[(2u * i3) + 1u]; 00307 00308 /* xa - xc */ 00309 Xaminusc = xaIn - xcIn; 00310 /* (xb - xd) */ 00311 Xbminusd = xbIn - xdIn; 00312 /* ya - yc */ 00313 Yaminusc = yaIn - ycIn; 00314 /* (yb - yd) */ 00315 Ybminusd = ybIn - ydIn; 00316 00317 /* xa + xc */ 00318 Xaplusc = xaIn + xcIn; 00319 /* xb + xd */ 00320 Xbplusd = xbIn + xdIn; 00321 /* ya + yc */ 00322 Yaplusc = yaIn + ycIn; 00323 /* yb + yd */ 00324 Ybplusd = ybIn + ydIn; 00325 00326 /* (xa - xc) + (yb - yd) */ 00327 Xb12C_out = (Xaminusc + Ybminusd); 00328 /* (ya - yc) - (xb - xd) */ 00329 Yb12C_out = (Yaminusc - Xbminusd); 00330 /* xa + xc -(xb + xd) */ 00331 Xc12C_out = (Xaplusc - Xbplusd); 00332 /* (ya + yc) - (yb + yd) */ 00333 Yc12C_out = (Yaplusc - Ybplusd); 00334 /* (xa - xc) - (yb - yd) */ 00335 Xd12C_out = (Xaminusc - Ybminusd); 00336 /* (ya - yc) + (xb - xd) */ 00337 Yd12C_out = (Xbminusd + Yaminusc); 00338 00339 pSrc[(2u * i0)] = Xaplusc + Xbplusd; 00340 pSrc[(2u * i0) + 1u] = Yaplusc + Ybplusd; 00341 00342 Xb12_out = Xb12C_out * co1; 00343 Yb12_out = Yb12C_out * co1; 00344 Xc12_out = Xc12C_out * co2; 00345 Yc12_out = Yc12C_out * co2; 00346 Xd12_out = Xd12C_out * co3; 00347 Yd12_out = Yd12C_out * co3; 00348 00349 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00350 Xb12_out += Yb12C_out * si1; 00351 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00352 Yb12_out -= Xb12C_out * si1; 00353 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00354 Xc12_out += Yc12C_out * si2; 00355 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00356 Yc12_out -= Xc12C_out * si2; 00357 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00358 Xd12_out += Yd12C_out * si3; 00359 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00360 Yd12_out -= Xd12C_out * si3; 00361 00362 /* xc' = (xa-xb+xc-xd)co2 + (ya-yb+yc-yd)(si2) */ 00363 pSrc[2u * i1] = Xc12_out; 00364 00365 /* yc' = (ya-yb+yc-yd)co2 - (xa-xb+xc-xd)(si2) */ 00366 pSrc[(2u * i1) + 1u] = Yc12_out; 00367 00368 /* xb' = (xa+yb-xc-yd)co1 + (ya-xb-yc+xd)(si1) */ 00369 pSrc[2u * i2] = Xb12_out; 00370 00371 /* yb' = (ya-xb-yc+xd)co1 - (xa+yb-xc-yd)(si1) */ 00372 pSrc[(2u * i2) + 1u] = Yb12_out; 00373 00374 /* xd' = (xa-yb-xc+yd)co3 + (ya+xb-yc-xd)(si3) */ 00375 pSrc[2u * i3] = Xd12_out; 00376 00377 /* yd' = (ya+xb-yc-xd)co3 - (xa-yb-xc+yd)(si3) */ 00378 pSrc[(2u * i3) + 1u] = Yd12_out; 00379 00380 } 00381 } 00382 twidCoefModifier <<= 2u; 00383 } 00384 00385 j = fftLen >> 2; 00386 ptr1 = &pSrc[0]; 00387 00388 /* Calculations of last stage */ 00389 do 00390 { 00391 00392 xaIn = ptr1[0]; 00393 xcIn = ptr1[4]; 00394 yaIn = ptr1[1]; 00395 ycIn = ptr1[5]; 00396 00397 /* xa + xc */ 00398 Xaplusc = xaIn + xcIn; 00399 00400 xbIn = ptr1[2]; 00401 00402 /* xa - xc */ 00403 Xaminusc = xaIn - xcIn; 00404 00405 xdIn = ptr1[6]; 00406 00407 /* ya + yc */ 00408 Yaplusc = yaIn + ycIn; 00409 00410 ybIn = ptr1[3]; 00411 00412 /* ya - yc */ 00413 Yaminusc = yaIn - ycIn; 00414 00415 ydIn = ptr1[7]; 00416 00417 /* xb + xd */ 00418 Xbplusd = xbIn + xdIn; 00419 00420 /* yb + yd */ 00421 Ybplusd = ybIn + ydIn; 00422 00423 /* xa' = xa + xb + xc + xd */ 00424 Xa12_out = (Xaplusc + Xbplusd); 00425 00426 /* (xb-xd) */ 00427 Xbminusd = xbIn - xdIn; 00428 00429 /* ya' = ya + yb + yc + yd */ 00430 Ya12_out = (Yaplusc + Ybplusd); 00431 00432 /* (yb-yd) */ 00433 Ybminusd = ybIn - ydIn; 00434 00435 /* xc' = (xa-xb+xc-xd) */ 00436 Xc12_out = (Xaplusc - Xbplusd); 00437 /* yc' = (ya-yb+yc-yd) */ 00438 Yc12_out = (Yaplusc - Ybplusd); 00439 /* xb' = (xa+yb-xc-yd) */ 00440 Xb12_out = (Xaminusc + Ybminusd); 00441 /* yb' = (ya-xb-yc+xd) */ 00442 Yb12_out = (Yaminusc - Xbminusd); 00443 /* xd' = (xa-yb-xc+yd)) */ 00444 Xd12_out = (Xaminusc - Ybminusd); 00445 /* yd' = (ya+xb-yc-xd) */ 00446 Yd12_out = (Xbminusd + Yaminusc); 00447 00448 /* calculating magnitude to output */ 00449 Xa12_out = Xa12_out * Xa12_out; 00450 Ya12_out = Ya12_out * Ya12_out; 00451 00452 Xc12_out = Xc12_out * Xc12_out; 00453 Yc12_out = Yc12_out * Yc12_out; 00454 00455 Xb12_out = Xb12_out * Xb12_out; 00456 Yb12_out = Yb12_out * Yb12_out; 00457 00458 Xd12_out = Xd12_out * Xd12_out; 00459 Yd12_out = Yd12_out * Yd12_out; 00460 00461 arm_sqrt_f32(Xa12_out + Ya12_out, pSrc++); 00462 arm_sqrt_f32(Xc12_out + Yc12_out, pSrc++); 00463 arm_sqrt_f32(Xb12_out + Yb12_out, pSrc++); 00464 arm_sqrt_f32(Xd12_out + Yd12_out, pSrc++); 00465 00466 00467 /* increment pointer by 8 */ 00468 ptr1 = ptr1 + 8u; 00469 00470 }while(--j); 00471 00472 } 00473 00474 /* 00475 * @brief In-place bit reversal function. 00476 * @param[in, out] *pSrc points to the in-place buffer of floating-point data type. 00477 * @param[in] fftSize length of the FFT. 00478 * @param[in] bitRevFactor bit reversal modifier that supports different size FFTs with the same bit reversal table. 00479 * @param[in] *pBitRevTab points to the bit reversal table. 00480 * @return none. 00481 */ 00482 00483 void arm_cfft_mag_bitreversal_f32( 00484 float32_t * pSrc, 00485 uint16_t fftSize, 00486 uint16_t bitRevFactor, 00487 uint16_t * pBitRevTab) 00488 { 00489 uint16_t fftLenBy2, fftLenBy2p1; 00490 uint16_t i, j; 00491 float32_t in; 00492 00493 /* Initializations */ 00494 j = 0u; 00495 fftLenBy2 = fftSize >> 1u; 00496 fftLenBy2p1 = (fftSize >> 1u) + 1u; 00497 00498 /* Bit Reversal Implementation */ 00499 for (i = 0u; i <= (fftLenBy2 - 2u); i += 2u) 00500 { 00501 if(i < j) 00502 { 00503 /* pSrc[i] <-> pSrc[j]; */ 00504 in = pSrc[i]; 00505 pSrc[i] = pSrc[j]; 00506 pSrc[j] = in; 00507 00508 /* pSrc[i+fftLenBy2p1] <-> pSrc[j+fftLenBy2p1] */ 00509 in = pSrc[(i + fftLenBy2p1)]; 00510 pSrc[(i + fftLenBy2p1)] = pSrc[(j + fftLenBy2p1)]; 00511 pSrc[(j + fftLenBy2p1)] = in; 00512 00513 } 00514 00515 /* pSrc[i+1u] <-> pSrc[j+1u] */ 00516 in = pSrc[ (i + 1u)]; 00517 pSrc[(i + 1u)] = pSrc[(j + fftLenBy2)]; 00518 pSrc[(j + fftLenBy2)] = in; 00519 00520 /* Reading the index for the bit reversal */ 00521 j = *pBitRevTab; 00522 00523 /* Updating the bit reversal index depending on the fft length */ 00524 pBitRevTab += bitRevFactor; 00525 } 00526 }
