00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "arm_math.h"
00035
00174 void arm_cfft_radix4_f32(
00175 const arm_cfft_radix4_instance_f32 * S,
00176 float32_t * pSrc)
00177 {
00178
00179 if(S->ifftFlag == 1u)
00180 {
00181
00182 arm_radix4_butterfly_inverse_f32(pSrc, S->fftLen, S->pTwiddle,
00183 S->twidCoefModifier, S->onebyfftLen);
00184 }
00185 else
00186 {
00187
00188 arm_radix4_butterfly_f32(pSrc, S->fftLen, S->pTwiddle,
00189 S->twidCoefModifier);
00190 }
00191
00192 if(S->bitReverseFlag == 1u)
00193 {
00194
00195 arm_bitreversal_f32(pSrc, S->fftLen, S->bitRevFactor, S->pBitRevTable);
00196 }
00197
00198 }
00199
00200
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220 void arm_radix4_butterfly_f32(
00221 float32_t * pSrc,
00222 uint16_t fftLen,
00223 float32_t * pCoef,
00224 uint16_t twidCoefModifier)
00225 {
00226
00227 float32_t co1, co2, co3, si1, si2, si3;
00228 float32_t t1, t2, r1, r2, s1, s2;
00229 uint32_t ia1, ia2, ia3;
00230 uint32_t i0, i1, i2, i3;
00231 uint32_t n1, n2, j, k;
00232
00233
00234 n2 = fftLen;
00235 n1 = n2;
00236
00237
00238 n2 >>= 2u;
00239 i0 = 0u;
00240 ia1 = 0u;
00241
00242 j = n2;
00243
00244
00245 do
00246 {
00247
00248
00249 i1 = i0 + n2;
00250 i2 = i1 + n2;
00251 i3 = i2 + n2;
00252
00253
00254
00255
00256 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)];
00257
00258
00259 r2 = pSrc[2u * i0] - pSrc[2u * i2];
00260
00261
00262 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00263
00264
00265 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00266
00267
00268 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00269
00270
00271 pSrc[2u * i0] = r1 + t1;
00272
00273
00274 r1 = r1 - t1;
00275
00276
00277 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00278
00279
00280 pSrc[(2u * i0) + 1u] = s1 + t2;
00281
00282
00283 s1 = s1 - t2;
00284
00285
00286 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00287
00288
00289 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00290
00291
00292 ia2 = ia1 + ia1;
00293 co2 = pCoef[ia2 * 2u];
00294 si2 = pCoef[(ia2 * 2u) + 1u];
00295
00296
00297 pSrc[2u * i1] = (r1 * co2) + (s1 * si2);
00298
00299
00300 pSrc[(2u * i1) + 1u] = (s1 * co2) - (r1 * si2);
00301
00302
00303 r1 = r2 + t1;
00304
00305
00306 r2 = r2 - t1;
00307
00308
00309 s1 = s2 - t2;
00310
00311
00312 s2 = s2 + t2;
00313
00314 co1 = pCoef[ia1 * 2u];
00315 si1 = pCoef[(ia1 * 2u) + 1u];
00316
00317
00318 pSrc[2u * i2] = (r1 * co1) + (s1 * si1);
00319
00320
00321 pSrc[(2u * i2) + 1u] = (s1 * co1) - (r1 * si1);
00322
00323
00324 ia3 = ia2 + ia1;
00325 co3 = pCoef[ia3 * 2u];
00326 si3 = pCoef[(ia3 * 2u) + 1u];
00327
00328
00329
00330 pSrc[2u * i3] = (r2 * co3) + (s2 * si3);
00331
00332
00333 pSrc[(2u * i3) + 1u] = (s2 * co3) - (r2 * si3);
00334
00335
00336 ia1 = ia1 + twidCoefModifier;
00337
00338
00339 i0 = i0 + 1u;
00340
00341 }
00342 while(--j);
00343
00344 twidCoefModifier <<= 2u;
00345
00346
00347 for (k = fftLen / 4; k > 4u; k >>= 2u)
00348 {
00349
00350 n1 = n2;
00351 n2 >>= 2u;
00352 ia1 = 0u;
00353
00354
00355 for (j = 0u; j <= (n2 - 1u); j++)
00356 {
00357
00358 ia2 = ia1 + ia1;
00359 ia3 = ia2 + ia1;
00360 co1 = pCoef[ia1 * 2u];
00361 si1 = pCoef[(ia1 * 2u) + 1u];
00362 co2 = pCoef[ia2 * 2u];
00363 si2 = pCoef[(ia2 * 2u) + 1u];
00364 co3 = pCoef[ia3 * 2u];
00365 si3 = pCoef[(ia3 * 2u) + 1u];
00366
00367
00368 ia1 = ia1 + twidCoefModifier;
00369
00370 for (i0 = j; i0 < fftLen; i0 += n1)
00371 {
00372
00373
00374 i1 = i0 + n2;
00375 i2 = i1 + n2;
00376 i3 = i2 + n2;
00377
00378
00379 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)];
00380
00381
00382 r2 = pSrc[(2u * i0)] - pSrc[(2u * i2)];
00383
00384
00385 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00386
00387
00388 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00389
00390
00391 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00392
00393
00394 pSrc[2u * i0] = r1 + t1;
00395
00396
00397 r1 = r1 - t1;
00398
00399
00400 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00401
00402
00403 pSrc[(2u * i0) + 1u] = s1 + t2;
00404
00405
00406 s1 = s1 - t2;
00407
00408
00409 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00410
00411
00412 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00413
00414
00415 pSrc[2u * i1] = (r1 * co2) + (s1 * si2);
00416
00417
00418 pSrc[(2u * i1) + 1u] = (s1 * co2) - (r1 * si2);
00419
00420
00421 r1 = r2 + t1;
00422
00423
00424 r2 = r2 - t1;
00425
00426
00427 s1 = s2 - t2;
00428
00429
00430 s2 = s2 + t2;
00431
00432
00433 pSrc[2u * i2] = (r1 * co1) + (s1 * si1);
00434
00435
00436 pSrc[(2u * i2) + 1u] = (s1 * co1) - (r1 * si1);
00437
00438
00439 pSrc[2u * i3] = (r2 * co3) + (s2 * si3);
00440
00441
00442 pSrc[(2u * i3) + 1u] = (s2 * co3) - (r2 * si3);
00443 }
00444 }
00445 twidCoefModifier <<= 2u;
00446 }
00447
00448
00449 n1 = n2;
00450 n2 >>= 2u;
00451
00452
00453 for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1)
00454 {
00455
00456
00457 i1 = i0 + n2;
00458 i2 = i1 + n2;
00459 i3 = i2 + n2;
00460
00461
00462
00463
00464 r1 = pSrc[2u * i0] + pSrc[2u * i2];
00465
00466
00467 r2 = pSrc[2u * i0] - pSrc[2u * i2];
00468
00469
00470 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00471
00472
00473 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00474
00475
00476 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00477
00478
00479 pSrc[2u * i0] = r1 + t1;
00480
00481
00482 r1 = r1 - t1;
00483
00484
00485 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00486
00487
00488 pSrc[(2u * i0) + 1u] = s1 + t2;
00489
00490
00491 s1 = s1 - t2;
00492
00493
00494 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00495
00496
00497 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00498
00499
00500 pSrc[2u * i1] = r1;
00501
00502
00503 pSrc[(2u * i1) + 1u] = s1;
00504
00505
00506 r1 = r2 + t1;
00507
00508
00509 r2 = r2 - t1;
00510
00511
00512 s1 = s2 - t2;
00513
00514
00515 s2 = s2 + t2;
00516
00517
00518 pSrc[2u * i2] = r1;
00519
00520
00521 pSrc[(2u * i2) + 1u] = s1;
00522
00523
00524 pSrc[2u * i3] = r2;
00525
00526
00527 pSrc[(2u * i3) + 1u] = s2;
00528 }
00529 }
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541 void arm_radix4_butterfly_inverse_f32(
00542 float32_t * pSrc,
00543 uint16_t fftLen,
00544 float32_t * pCoef,
00545 uint16_t twidCoefModifier,
00546 float32_t onebyfftLen)
00547 {
00548 float32_t co1, co2, co3, si1, si2, si3;
00549 float32_t t1, t2, r1, r2, s1, s2;
00550 uint32_t ia1, ia2, ia3;
00551 uint32_t i0, i1, i2, i3;
00552 uint32_t n1, n2, j, k;
00553
00554
00555 n2 = fftLen;
00556 n1 = n2;
00557
00558
00559 n2 >>= 2u;
00560 i0 = 0u;
00561 ia1 = 0u;
00562
00563 j = n2;
00564
00565
00566 do
00567 {
00568
00569
00570 i1 = i0 + n2;
00571 i2 = i1 + n2;
00572 i3 = i2 + n2;
00573
00574
00575
00576 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)];
00577
00578
00579 r2 = pSrc[2u * i0] - pSrc[2u * i2];
00580
00581
00582 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00583
00584
00585 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00586
00587
00588 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00589
00590
00591 pSrc[2u * i0] = r1 + t1;
00592
00593
00594 r1 = r1 - t1;
00595
00596
00597 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00598
00599
00600 pSrc[(2u * i0) + 1u] = s1 + t2;
00601
00602
00603 s1 = s1 - t2;
00604
00605
00606 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00607
00608
00609 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00610
00611
00612 ia2 = ia1 + ia1;
00613 co2 = pCoef[ia2 * 2u];
00614 si2 = pCoef[(ia2 * 2u) + 1u];
00615
00616
00617 pSrc[2u * i1] = (r1 * co2) - (s1 * si2);
00618
00619
00620 pSrc[(2u * i1) + 1u] = (s1 * co2) + (r1 * si2);
00621
00622
00623 r1 = r2 - t1;
00624
00625
00626 r2 = r2 + t1;
00627
00628
00629 s1 = s2 + t2;
00630
00631
00632 s2 = s2 - t2;
00633
00634 co1 = pCoef[ia1 * 2u];
00635 si1 = pCoef[(ia1 * 2u) + 1u];
00636
00637
00638 pSrc[2u * i2] = (r1 * co1) - (s1 * si1);
00639
00640
00641 pSrc[(2u * i2) + 1u] = (s1 * co1) + (r1 * si1);
00642
00643
00644 ia3 = ia2 + ia1;
00645 co3 = pCoef[ia3 * 2u];
00646 si3 = pCoef[(ia3 * 2u) + 1u];
00647
00648
00649 pSrc[2u * i3] = (r2 * co3) - (s2 * si3);
00650
00651
00652 pSrc[(2u * i3) + 1u] = (s2 * co3) + (r2 * si3);
00653
00654
00655 ia1 = ia1 + twidCoefModifier;
00656
00657
00658 i0 = i0 + 1u;
00659
00660 }
00661 while(--j);
00662
00663 twidCoefModifier <<= 2u;
00664
00665
00666 for (k = fftLen / 4; k > 4u; k >>= 2u)
00667 {
00668
00669 n1 = n2;
00670 n2 >>= 2u;
00671 ia1 = 0u;
00672
00673
00674 for (j = 0u; j <= (n2 - 1u); j++)
00675 {
00676
00677 ia2 = ia1 + ia1;
00678 ia3 = ia2 + ia1;
00679 co1 = pCoef[ia1 * 2u];
00680 si1 = pCoef[(ia1 * 2u) + 1u];
00681 co2 = pCoef[ia2 * 2u];
00682 si2 = pCoef[(ia2 * 2u) + 1u];
00683 co3 = pCoef[ia3 * 2u];
00684 si3 = pCoef[(ia3 * 2u) + 1u];
00685
00686
00687 ia1 = ia1 + twidCoefModifier;
00688
00689 for (i0 = j; i0 < fftLen; i0 += n1)
00690 {
00691
00692
00693 i1 = i0 + n2;
00694 i2 = i1 + n2;
00695 i3 = i2 + n2;
00696
00697
00698 r1 = pSrc[(2u * i0)] + pSrc[(2u * i2)];
00699
00700
00701 r2 = pSrc[(2u * i0)] - pSrc[(2u * i2)];
00702
00703
00704 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00705
00706
00707 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00708
00709
00710 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00711
00712
00713 pSrc[2u * i0] = r1 + t1;
00714
00715
00716 r1 = r1 - t1;
00717
00718
00719 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00720
00721
00722 pSrc[(2u * i0) + 1u] = s1 + t2;
00723
00724
00725 s1 = s1 - t2;
00726
00727
00728 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00729
00730
00731 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00732
00733
00734 pSrc[2u * i1] = (r1 * co2) - (s1 * si2);
00735
00736
00737 pSrc[(2u * i1) + 1u] = (s1 * co2) + (r1 * si2);
00738
00739
00740 r1 = r2 - t1;
00741
00742
00743 r2 = r2 + t1;
00744
00745
00746 s1 = s2 + t2;
00747
00748
00749 s2 = s2 - t2;
00750
00751
00752 pSrc[2u * i2] = (r1 * co1) - (s1 * si1);
00753
00754
00755 pSrc[(2u * i2) + 1u] = (s1 * co1) + (r1 * si1);
00756
00757
00758 pSrc[2u * i3] = (r2 * co3) - (s2 * si3);
00759
00760
00761 pSrc[(2u * i3) + 1u] = (s2 * co3) + (r2 * si3);
00762 }
00763 }
00764 twidCoefModifier <<= 2u;
00765 }
00766
00767
00768 n1 = n2;
00769 n2 >>= 2u;
00770
00771
00772 for (i0 = 0u; i0 <= (fftLen - n1); i0 += n1)
00773 {
00774
00775
00776 i1 = i0 + n2;
00777 i2 = i1 + n2;
00778 i3 = i2 + n2;
00779
00780
00781
00782 r1 = pSrc[2u * i0] + pSrc[2u * i2];
00783
00784
00785 r2 = pSrc[2u * i0] - pSrc[2u * i2];
00786
00787
00788 s1 = pSrc[(2u * i0) + 1u] + pSrc[(2u * i2) + 1u];
00789
00790
00791 s2 = pSrc[(2u * i0) + 1u] - pSrc[(2u * i2) + 1u];
00792
00793
00794 t1 = pSrc[2u * i1] + pSrc[2u * i3];
00795
00796
00797 pSrc[2u * i0] = (r1 + t1) * onebyfftLen;
00798
00799
00800 r1 = r1 - t1;
00801
00802
00803 t2 = pSrc[(2u * i1) + 1u] + pSrc[(2u * i3) + 1u];
00804
00805
00806 pSrc[(2u * i0) + 1u] = (s1 + t2) * onebyfftLen;
00807
00808
00809 s1 = s1 - t2;
00810
00811
00812 t1 = pSrc[(2u * i1) + 1u] - pSrc[(2u * i3) + 1u];
00813
00814
00815 t2 = pSrc[2u * i1] - pSrc[2u * i3];
00816
00817
00818 pSrc[2u * i1] = r1 * onebyfftLen;
00819
00820
00821 pSrc[(2u * i1) + 1u] = s1 * onebyfftLen;
00822
00823
00824
00825 r1 = r2 - t1;
00826
00827
00828 r2 = r2 + t1;
00829
00830
00831 s1 = s2 + t2;
00832
00833
00834 s2 = s2 - t2;
00835
00836
00837 pSrc[2u * i2] = r1 * onebyfftLen;
00838
00839
00840 pSrc[(2u * i2) + 1u] = s1 * onebyfftLen;
00841
00842
00843 pSrc[2u * i3] = r2 * onebyfftLen;
00844
00845
00846 pSrc[(2u * i3) + 1u] = s2 * onebyfftLen;
00847 }
00848 }
00849
00850
00851
00852
00853
00854
00855
00856
00857
00858
00859 void arm_bitreversal_f32(
00860 float32_t * pSrc,
00861 uint16_t fftSize,
00862 uint16_t bitRevFactor,
00863 uint16_t * pBitRevTab)
00864 {
00865 uint16_t fftLenBy2, fftLenBy2p1;
00866 uint16_t i, j;
00867 float32_t in;
00868
00869
00870 j = 0u;
00871 fftLenBy2 = fftSize >> 1u;
00872 fftLenBy2p1 = (fftSize >> 1u) + 1u;
00873
00874
00875 for (i = 0u; i <= (fftLenBy2 - 2u); i += 2u)
00876 {
00877 if(i < j)
00878 {
00879
00880 in = pSrc[2u * i];
00881 pSrc[2u * i] = pSrc[2u * j];
00882 pSrc[2u * j] = in;
00883
00884
00885 in = pSrc[(2u * i) + 1u];
00886 pSrc[(2u * i) + 1u] = pSrc[(2u * j) + 1u];
00887 pSrc[(2u * j) + 1u] = in;
00888
00889
00890 in = pSrc[2u * (i + fftLenBy2p1)];
00891 pSrc[2u * (i + fftLenBy2p1)] = pSrc[2u * (j + fftLenBy2p1)];
00892 pSrc[2u * (j + fftLenBy2p1)] = in;
00893
00894
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
00903 in = pSrc[2u * (i + 1u)];
00904 pSrc[2u * (i + 1u)] = pSrc[2u * (j + fftLenBy2)];
00905 pSrc[2u * (j + fftLenBy2)] = in;
00906
00907
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
00913 j = *pBitRevTab;
00914
00915
00916 pBitRevTab += bitRevFactor;
00917 }
00918 }