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