1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
8 * Title: arm_cfft_radix2_q15.c
10 * Description: Radix-2 Decimation in Frequency CFFT & CIFFT Fixed point processing function
13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
15 * Redistribution and use in source and binary forms, with or without
16 * modification, are permitted provided that the following conditions
18 * - Redistributions of source code must retain the above copyright
19 * notice, this list of conditions and the following disclaimer.
20 * - Redistributions in binary form must reproduce the above copyright
21 * notice, this list of conditions and the following disclaimer in
22 * the documentation and/or other materials provided with the
24 * - Neither the name of ARM LIMITED nor the names of its contributors
25 * may be used to endorse or promote products derived from this
26 * software without specific prior written permission.
28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
39 * POSSIBILITY OF SUCH DAMAGE.
40 * -------------------------------------------------------------------- */
44 void arm_radix2_butterfly_q15(
48 uint16_t twidCoefModifier
);
50 void arm_radix2_butterfly_inverse_q15(
54 uint16_t twidCoefModifier
);
56 void arm_bitreversal_q15(
59 uint16_t bitRevFactor
,
60 uint16_t * pBitRevTab
);
63 * @ingroup groupTransforms
67 * @addtogroup ComplexFFT
73 * @brief Processing function for the fixed-point CFFT/CIFFT.
74 * @param[in] *S points to an instance of the fixed-point CFFT/CIFFT structure.
75 * @param[in, out] *pSrc points to the complex data buffer of size <code>2*fftLen</code>. Processing occurs in-place.
79 void arm_cfft_radix2_q15(
80 const arm_cfft_radix2_instance_q15
* S
,
86 arm_radix2_butterfly_inverse_q15(pSrc
, S
->fftLen
,
87 S
->pTwiddle
, S
->twidCoefModifier
);
91 arm_radix2_butterfly_q15(pSrc
, S
->fftLen
,
92 S
->pTwiddle
, S
->twidCoefModifier
);
95 arm_bitreversal_q15(pSrc
, S
->fftLen
, S
->bitRevFactor
, S
->pBitRevTable
);
99 * @} end of ComplexFFT group
102 void arm_radix2_butterfly_q15(
106 uint16_t twidCoefModifier
)
108 #ifndef ARM_MATH_CM0_FAMILY
114 q31_t coeff
, out1
, out2
;
124 for (i
= 0; i
< n2
; i
++)
126 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
128 ia
= ia
+ twidCoefModifier
;
132 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
133 in
= ((int16_t) (T
& 0xFFFF)) >> 2;
134 T
= ((T
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
136 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
137 in
= ((int16_t) (S
& 0xFFFF)) >> 2;
138 S
= ((S
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
142 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
144 #ifndef ARM_MATH_BIG_ENDIAN
146 out1
= __SMUAD(coeff
, R
) >> 16;
147 out2
= __SMUSDX(coeff
, R
);
151 out1
= __SMUSDX(R
, coeff
) >> 16u;
152 out2
= __SMUAD(coeff
, R
);
154 #endif // #ifndef ARM_MATH_BIG_ENDIAN
156 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
157 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
159 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
161 ia
= ia
+ twidCoefModifier
;
163 // loop for butterfly
167 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
168 in
= ((int16_t) (T
& 0xFFFF)) >> 2;
169 T
= ((T
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
171 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
172 in
= ((int16_t) (S
& 0xFFFF)) >> 2;
173 S
= ((S
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
177 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
179 #ifndef ARM_MATH_BIG_ENDIAN
181 out1
= __SMUAD(coeff
, R
) >> 16;
182 out2
= __SMUSDX(coeff
, R
);
186 out1
= __SMUSDX(R
, coeff
) >> 16u;
187 out2
= __SMUAD(coeff
, R
);
189 #endif // #ifndef ARM_MATH_BIG_ENDIAN
191 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
192 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
196 twidCoefModifier
= twidCoefModifier
<< 1u;
199 for (k
= fftLen
/ 2; k
> 2; k
= k
>> 1)
206 for (j
= 0; j
< n2
; j
++)
208 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
210 ia
= ia
+ twidCoefModifier
;
212 // loop for butterfly
213 for (i
= j
; i
< fftLen
; i
+= n1
)
217 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
219 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
223 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
225 #ifndef ARM_MATH_BIG_ENDIAN
227 out1
= __SMUAD(coeff
, R
) >> 16;
228 out2
= __SMUSDX(coeff
, R
);
232 out1
= __SMUSDX(R
, coeff
) >> 16u;
233 out2
= __SMUAD(coeff
, R
);
235 #endif // #ifndef ARM_MATH_BIG_ENDIAN
237 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
238 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
244 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
246 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
250 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
252 #ifndef ARM_MATH_BIG_ENDIAN
254 out1
= __SMUAD(coeff
, R
) >> 16;
255 out2
= __SMUSDX(coeff
, R
);
259 out1
= __SMUSDX(R
, coeff
) >> 16u;
260 out2
= __SMUAD(coeff
, R
);
262 #endif // #ifndef ARM_MATH_BIG_ENDIAN
264 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
265 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
267 } // butterfly loop end
271 twidCoefModifier
= twidCoefModifier
<< 1u;
278 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
280 ia
= ia
+ twidCoefModifier
;
282 // loop for butterfly
283 for (i
= 0; i
< fftLen
; i
+= n1
)
287 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
289 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
293 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __QADD16(T
, S
);
295 _SIMD32_OFFSET(pSrc
+ (2u * l
)) = R
;
300 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
302 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
306 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __QADD16(T
, S
);
308 _SIMD32_OFFSET(pSrc
+ (2u * l
)) = R
;
317 q15_t xt
, yt
, cosVal
, sinVal
;
328 for (j
= 0; j
< n2
; j
++)
330 cosVal
= pCoef
[ia
* 2];
331 sinVal
= pCoef
[(ia
* 2) + 1];
332 ia
= ia
+ twidCoefModifier
;
334 // loop for butterfly
335 for (i
= j
; i
< fftLen
; i
+= n1
)
338 xt
= (pSrc
[2 * i
] >> 2u) - (pSrc
[2 * l
] >> 2u);
339 pSrc
[2 * i
] = ((pSrc
[2 * i
] >> 2u) + (pSrc
[2 * l
] >> 2u)) >> 1u;
341 yt
= (pSrc
[2 * i
+ 1] >> 2u) - (pSrc
[2 * l
+ 1] >> 2u);
343 ((pSrc
[2 * l
+ 1] >> 2u) + (pSrc
[2 * i
+ 1] >> 2u)) >> 1u;
345 pSrc
[2u * l
] = (((int16_t) (((q31_t
) xt
* cosVal
) >> 16)) +
346 ((int16_t) (((q31_t
) yt
* sinVal
) >> 16)));
348 pSrc
[2u * l
+ 1u] = (((int16_t) (((q31_t
) yt
* cosVal
) >> 16)) -
349 ((int16_t) (((q31_t
) xt
* sinVal
) >> 16)));
351 } // butterfly loop end
355 twidCoefModifier
= twidCoefModifier
<< 1u;
358 for (k
= fftLen
/ 2; k
> 2; k
= k
>> 1)
365 for (j
= 0; j
< n2
; j
++)
367 cosVal
= pCoef
[ia
* 2];
368 sinVal
= pCoef
[(ia
* 2) + 1];
369 ia
= ia
+ twidCoefModifier
;
371 // loop for butterfly
372 for (i
= j
; i
< fftLen
; i
+= n1
)
375 xt
= pSrc
[2 * i
] - pSrc
[2 * l
];
376 pSrc
[2 * i
] = (pSrc
[2 * i
] + pSrc
[2 * l
]) >> 1u;
378 yt
= pSrc
[2 * i
+ 1] - pSrc
[2 * l
+ 1];
379 pSrc
[2 * i
+ 1] = (pSrc
[2 * l
+ 1] + pSrc
[2 * i
+ 1]) >> 1u;
381 pSrc
[2u * l
] = (((int16_t) (((q31_t
) xt
* cosVal
) >> 16)) +
382 ((int16_t) (((q31_t
) yt
* sinVal
) >> 16)));
384 pSrc
[2u * l
+ 1u] = (((int16_t) (((q31_t
) yt
* cosVal
) >> 16)) -
385 ((int16_t) (((q31_t
) xt
* sinVal
) >> 16)));
387 } // butterfly loop end
391 twidCoefModifier
= twidCoefModifier
<< 1u;
399 for (j
= 0; j
< n2
; j
++)
401 cosVal
= pCoef
[ia
* 2];
402 sinVal
= pCoef
[(ia
* 2) + 1];
404 ia
= ia
+ twidCoefModifier
;
406 // loop for butterfly
407 for (i
= j
; i
< fftLen
; i
+= n1
)
410 xt
= pSrc
[2 * i
] - pSrc
[2 * l
];
411 pSrc
[2 * i
] = (pSrc
[2 * i
] + pSrc
[2 * l
]);
413 yt
= pSrc
[2 * i
+ 1] - pSrc
[2 * l
+ 1];
414 pSrc
[2 * i
+ 1] = (pSrc
[2 * l
+ 1] + pSrc
[2 * i
+ 1]);
418 pSrc
[2u * l
+ 1u] = yt
;
420 } // butterfly loop end
424 twidCoefModifier
= twidCoefModifier
<< 1u;
426 #endif // #ifndef ARM_MATH_CM0_FAMILY
431 void arm_radix2_butterfly_inverse_q15(
435 uint16_t twidCoefModifier
)
437 #ifndef ARM_MATH_CM0_FAMILY
443 q31_t coeff
, out1
, out2
;
453 for (i
= 0; i
< n2
; i
++)
455 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
457 ia
= ia
+ twidCoefModifier
;
461 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
462 in
= ((int16_t) (T
& 0xFFFF)) >> 2;
463 T
= ((T
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
465 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
466 in
= ((int16_t) (S
& 0xFFFF)) >> 2;
467 S
= ((S
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
471 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
473 #ifndef ARM_MATH_BIG_ENDIAN
475 out1
= __SMUSD(coeff
, R
) >> 16;
476 out2
= __SMUADX(coeff
, R
);
479 out1
= __SMUADX(R
, coeff
) >> 16u;
480 out2
= __SMUSD(__QSUB(0, coeff
), R
);
482 #endif // #ifndef ARM_MATH_BIG_ENDIAN
484 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
485 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
487 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
489 ia
= ia
+ twidCoefModifier
;
491 // loop for butterfly
495 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
496 in
= ((int16_t) (T
& 0xFFFF)) >> 2;
497 T
= ((T
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
499 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
500 in
= ((int16_t) (S
& 0xFFFF)) >> 2;
501 S
= ((S
>> 2) & 0xFFFF0000) | (in
& 0xFFFF);
505 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
507 #ifndef ARM_MATH_BIG_ENDIAN
509 out1
= __SMUSD(coeff
, R
) >> 16;
510 out2
= __SMUADX(coeff
, R
);
513 out1
= __SMUADX(R
, coeff
) >> 16u;
514 out2
= __SMUSD(__QSUB(0, coeff
), R
);
516 #endif // #ifndef ARM_MATH_BIG_ENDIAN
518 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
519 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
523 twidCoefModifier
= twidCoefModifier
<< 1u;
526 for (k
= fftLen
/ 2; k
> 2; k
= k
>> 1)
533 for (j
= 0; j
< n2
; j
++)
535 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
537 ia
= ia
+ twidCoefModifier
;
539 // loop for butterfly
540 for (i
= j
; i
< fftLen
; i
+= n1
)
544 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
546 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
550 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
552 #ifndef ARM_MATH_BIG_ENDIAN
554 out1
= __SMUSD(coeff
, R
) >> 16;
555 out2
= __SMUADX(coeff
, R
);
559 out1
= __SMUADX(R
, coeff
) >> 16u;
560 out2
= __SMUSD(__QSUB(0, coeff
), R
);
562 #endif // #ifndef ARM_MATH_BIG_ENDIAN
564 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
565 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
571 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
573 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
577 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __SHADD16(T
, S
);
579 #ifndef ARM_MATH_BIG_ENDIAN
581 out1
= __SMUSD(coeff
, R
) >> 16;
582 out2
= __SMUADX(coeff
, R
);
585 out1
= __SMUADX(R
, coeff
) >> 16u;
586 out2
= __SMUSD(__QSUB(0, coeff
), R
);
588 #endif // #ifndef ARM_MATH_BIG_ENDIAN
590 _SIMD32_OFFSET(pSrc
+ (2u * l
)) =
591 (q31_t
) ((out2
) & 0xFFFF0000) | (out1
& 0x0000FFFF);
593 } // butterfly loop end
597 twidCoefModifier
= twidCoefModifier
<< 1u;
605 for (j
= 0; j
< n2
; j
++)
607 coeff
= _SIMD32_OFFSET(pCoef
+ (ia
* 2u));
609 ia
= ia
+ twidCoefModifier
;
611 // loop for butterfly
612 for (i
= j
; i
< fftLen
; i
+= n1
)
616 T
= _SIMD32_OFFSET(pSrc
+ (2 * i
));
618 S
= _SIMD32_OFFSET(pSrc
+ (2 * l
));
622 _SIMD32_OFFSET(pSrc
+ (2 * i
)) = __QADD16(T
, S
);
624 _SIMD32_OFFSET(pSrc
+ (2u * l
)) = R
;
626 } // butterfly loop end
630 twidCoefModifier
= twidCoefModifier
<< 1u;
637 q15_t xt
, yt
, cosVal
, sinVal
;
647 for (j
= 0; j
< n2
; j
++)
649 cosVal
= pCoef
[ia
* 2];
650 sinVal
= pCoef
[(ia
* 2) + 1];
651 ia
= ia
+ twidCoefModifier
;
653 // loop for butterfly
654 for (i
= j
; i
< fftLen
; i
+= n1
)
657 xt
= (pSrc
[2 * i
] >> 2u) - (pSrc
[2 * l
] >> 2u);
658 pSrc
[2 * i
] = ((pSrc
[2 * i
] >> 2u) + (pSrc
[2 * l
] >> 2u)) >> 1u;
660 yt
= (pSrc
[2 * i
+ 1] >> 2u) - (pSrc
[2 * l
+ 1] >> 2u);
662 ((pSrc
[2 * l
+ 1] >> 2u) + (pSrc
[2 * i
+ 1] >> 2u)) >> 1u;
664 pSrc
[2u * l
] = (((int16_t) (((q31_t
) xt
* cosVal
) >> 16)) -
665 ((int16_t) (((q31_t
) yt
* sinVal
) >> 16)));
667 pSrc
[2u * l
+ 1u] = (((int16_t) (((q31_t
) yt
* cosVal
) >> 16)) +
668 ((int16_t) (((q31_t
) xt
* sinVal
) >> 16)));
670 } // butterfly loop end
674 twidCoefModifier
= twidCoefModifier
<< 1u;
677 for (k
= fftLen
/ 2; k
> 2; k
= k
>> 1)
684 for (j
= 0; j
< n2
; j
++)
686 cosVal
= pCoef
[ia
* 2];
687 sinVal
= pCoef
[(ia
* 2) + 1];
688 ia
= ia
+ twidCoefModifier
;
690 // loop for butterfly
691 for (i
= j
; i
< fftLen
; i
+= n1
)
694 xt
= pSrc
[2 * i
] - pSrc
[2 * l
];
695 pSrc
[2 * i
] = (pSrc
[2 * i
] + pSrc
[2 * l
]) >> 1u;
697 yt
= pSrc
[2 * i
+ 1] - pSrc
[2 * l
+ 1];
698 pSrc
[2 * i
+ 1] = (pSrc
[2 * l
+ 1] + pSrc
[2 * i
+ 1]) >> 1u;
700 pSrc
[2u * l
] = (((int16_t) (((q31_t
) xt
* cosVal
) >> 16)) -
701 ((int16_t) (((q31_t
) yt
* sinVal
) >> 16)));
703 pSrc
[2u * l
+ 1u] = (((int16_t) (((q31_t
) yt
* cosVal
) >> 16)) +
704 ((int16_t) (((q31_t
) xt
* sinVal
) >> 16)));
706 } // butterfly loop end
710 twidCoefModifier
= twidCoefModifier
<< 1u;
717 cosVal
= pCoef
[ia
* 2];
718 sinVal
= pCoef
[(ia
* 2) + 1];
720 ia
= ia
+ twidCoefModifier
;
722 // loop for butterfly
723 for (i
= 0; i
< fftLen
; i
+= n1
)
726 xt
= pSrc
[2 * i
] - pSrc
[2 * l
];
727 pSrc
[2 * i
] = (pSrc
[2 * i
] + pSrc
[2 * l
]);
729 yt
= pSrc
[2 * i
+ 1] - pSrc
[2 * l
+ 1];
730 pSrc
[2 * i
+ 1] = (pSrc
[2 * l
+ 1] + pSrc
[2 * i
+ 1]);
734 pSrc
[2u * l
+ 1u] = yt
;
739 #endif // #ifndef ARM_MATH_CM0_FAMILY