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