F030C8xx_KBus.git

QuakeGod

2022-10-17 6a62617e99d79314d1e10995fbc29fbb00419a6e

提交 \| 用户 \| age
bfc108	1	/* ----------------------------------------------------------------------
Q	2	* Copyright (C) 2010-2014 ARM Limited. All rights reserved.
	3	*
	4	* $Date: 19. March 2015
	5	* $Revision: V.1.4.5
	6	*
	7	* Project: CMSIS DSP Library
	8	* Title: arm_cfft_q31.c
	9	*
	10	* Description: Combined Radix Decimation in Frequency CFFT fixed point processing function
	11	*
	12	* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
	13	*
	14	* Redistribution and use in source and binary forms, with or without
	15	* modification, are permitted provided that the following conditions
	16	* are met:
	17	* - Redistributions of source code must retain the above copyright
	18	* notice, this list of conditions and the following disclaimer.
	19	* - Redistributions in binary form must reproduce the above copyright
	20	* notice, this list of conditions and the following disclaimer in
	21	* the documentation and/or other materials provided with the
	22	* distribution.
	23	* - Neither the name of ARM LIMITED nor the names of its contributors
	24	* may be used to endorse or promote products derived from this
	25	* software without specific prior written permission.
	26	*
	27	* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
	28	* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
	29	* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
	30	* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
	31	* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
	32	* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
	33	* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
	34	* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
	35	* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
	36	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
	37	* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	38	* POSSIBILITY OF SUCH DAMAGE.
	39	* -------------------------------------------------------------------- */
	40
	41	#include "arm_math.h"
	42
	43	extern void arm_radix4_butterfly_q31(
	44	q31_t * pSrc,
	45	uint32_t fftLen,
	46	q31_t * pCoef,
	47	uint32_t twidCoefModifier);
	48
	49	extern void arm_radix4_butterfly_inverse_q31(
	50	q31_t * pSrc,
	51	uint32_t fftLen,
	52	q31_t * pCoef,
	53	uint32_t twidCoefModifier);
	54
	55	extern void arm_bitreversal_32(
	56	uint32_t * pSrc,
	57	const uint16_t bitRevLen,
	58	const uint16_t * pBitRevTable);
	59
	60	void arm_cfft_radix4by2_q31(
	61	q31_t * pSrc,
	62	uint32_t fftLen,
	63	const q31_t * pCoef);
	64
	65	void arm_cfft_radix4by2_inverse_q31(
	66	q31_t * pSrc,
	67	uint32_t fftLen,
	68	const q31_t * pCoef);
	69
	70	/**
	71	* @ingroup groupTransforms
	72	*/
	73
	74	/**
	75	* @addtogroup ComplexFFT
	76	* @{
	77	*/
	78
	79	/**
	80	* @details
	81	* @brief Processing function for the fixed-point complex FFT in Q31 format.
	82	* @param[in] *S points to an instance of the fixed-point CFFT structure.
	83	* @param[in, out] p1 points to the complex data buffer of size <code>2fftLen</code>. Processing occurs in-place.
	84	* @param[in] ifftFlag flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform.
	85	* @param[in] bitReverseFlag flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output.
	86	* @return none.
	87	*/
	88
	89	void arm_cfft_q31(
	90	const arm_cfft_instance_q31 * S,
	91	q31_t * p1,
	92	uint8_t ifftFlag,
	93	uint8_t bitReverseFlag)
	94	{
	95	uint32_t L = S->fftLen;
	96
	97	if(ifftFlag == 1u)
	98	{
	99	switch (L)
	100	{
	101	case 16:
	102	case 64:
	103	case 256:
	104	case 1024:
	105	case 4096:
	106	arm_radix4_butterfly_inverse_q31 ( p1, L, (q31_t*)S->pTwiddle, 1 );
	107	break;
	108
	109	case 32:
	110	case 128:
	111	case 512:
	112	case 2048:
	113	arm_cfft_radix4by2_inverse_q31 ( p1, L, S->pTwiddle );
	114	break;
	115	}
	116	}
	117	else
	118	{
	119	switch (L)
	120	{
	121	case 16:
	122	case 64:
	123	case 256:
	124	case 1024:
	125	case 4096:
	126	arm_radix4_butterfly_q31 ( p1, L, (q31_t*)S->pTwiddle, 1 );
	127	break;
	128
	129	case 32:
	130	case 128:
	131	case 512:
	132	case 2048:
	133	arm_cfft_radix4by2_q31 ( p1, L, S->pTwiddle );
	134	break;
	135	}
	136	}
	137
	138	if( bitReverseFlag )
	139	arm_bitreversal_32((uint32_t*)p1,S->bitRevLength,S->pBitRevTable);
	140	}
	141
	142	/**
	143	* @} end of ComplexFFT group
	144	*/
	145
	146	void arm_cfft_radix4by2_q31(
	147	q31_t * pSrc,
	148	uint32_t fftLen,
	149	const q31_t * pCoef)
	150	{
	151	uint32_t i, l;
	152	uint32_t n2, ia;
	153	q31_t xt, yt, cosVal, sinVal;
	154	q31_t p0, p1;
	155
	156	n2 = fftLen >> 1;
	157	ia = 0;
	158	for (i = 0; i < n2; i++)
	159	{
	160	cosVal = pCoef[2*ia];
	161	sinVal = pCoef[2*ia + 1];
	162	ia++;
	163
	164	l = i + n2;
	165	xt = (pSrc[2 * i] >> 2) - (pSrc[2 * l] >> 2);
	166	pSrc[2 * i] = (pSrc[2 * i] >> 2) + (pSrc[2 * l] >> 2);
	167
	168	yt = (pSrc[2 * i + 1] >> 2) - (pSrc[2 * l + 1] >> 2);
	169	pSrc[2 * i + 1] = (pSrc[2 * l + 1] >> 2) + (pSrc[2 * i + 1] >> 2);
	170
	171	mult_32x32_keep32_R(p0, xt, cosVal);
	172	mult_32x32_keep32_R(p1, yt, cosVal);
	173	multAcc_32x32_keep32_R(p0, yt, sinVal);
	174	multSub_32x32_keep32_R(p1, xt, sinVal);
	175
	176	pSrc[2u * l] = p0 << 1;
	177	pSrc[2u * l + 1u] = p1 << 1;
	178
	179	}
	180
	181	// first col
	182	arm_radix4_butterfly_q31( pSrc, n2, (q31_t*)pCoef, 2u);
	183	// second col
	184	arm_radix4_butterfly_q31( pSrc + fftLen, n2, (q31_t*)pCoef, 2u);
	185
	186	for (i = 0; i < fftLen >> 1; i++)
	187	{
	188	p0 = pSrc[4*i+0];
	189	p1 = pSrc[4*i+1];
	190	xt = pSrc[4*i+2];
	191	yt = pSrc[4*i+3];
	192
	193	p0 <<= 1;
	194	p1 <<= 1;
	195	xt <<= 1;
	196	yt <<= 1;
	197
	198	pSrc[4*i+0] = p0;
	199	pSrc[4*i+1] = p1;
	200	pSrc[4*i+2] = xt;
	201	pSrc[4*i+3] = yt;
	202	}
	203
	204	}
	205
	206	void arm_cfft_radix4by2_inverse_q31(
	207	q31_t * pSrc,
	208	uint32_t fftLen,
	209	const q31_t * pCoef)
	210	{
	211	uint32_t i, l;
	212	uint32_t n2, ia;
	213	q31_t xt, yt, cosVal, sinVal;
	214	q31_t p0, p1;
	215
	216	n2 = fftLen >> 1;
	217	ia = 0;
	218	for (i = 0; i < n2; i++)
	219	{
	220	cosVal = pCoef[2*ia];
	221	sinVal = pCoef[2*ia + 1];
	222	ia++;
	223
	224	l = i + n2;
	225	xt = (pSrc[2 * i] >> 2) - (pSrc[2 * l] >> 2);
	226	pSrc[2 * i] = (pSrc[2 * i] >> 2) + (pSrc[2 * l] >> 2);
	227
	228	yt = (pSrc[2 * i + 1] >> 2) - (pSrc[2 * l + 1] >> 2);
	229	pSrc[2 * i + 1] = (pSrc[2 * l + 1] >> 2) + (pSrc[2 * i + 1] >> 2);
	230
	231	mult_32x32_keep32_R(p0, xt, cosVal);
	232	mult_32x32_keep32_R(p1, yt, cosVal);
	233	multSub_32x32_keep32_R(p0, yt, sinVal);
	234	multAcc_32x32_keep32_R(p1, xt, sinVal);
	235
	236	pSrc[2u * l] = p0 << 1;
	237	pSrc[2u * l + 1u] = p1 << 1;
	238
	239	}
	240
	241	// first col
	242	arm_radix4_butterfly_inverse_q31( pSrc, n2, (q31_t*)pCoef, 2u);
	243	// second col
	244	arm_radix4_butterfly_inverse_q31( pSrc + fftLen, n2, (q31_t*)pCoef, 2u);
	245
	246	for (i = 0; i < fftLen >> 1; i++)
	247	{
	248	p0 = pSrc[4*i+0];
	249	p1 = pSrc[4*i+1];
	250	xt = pSrc[4*i+2];
	251	yt = pSrc[4*i+3];
	252
	253	p0 <<= 1;
	254	p1 <<= 1;
	255	xt <<= 1;
	256	yt <<= 1;
	257
	258	pSrc[4*i+0] = p0;
	259	pSrc[4*i+1] = p1;
	260	pSrc[4*i+2] = xt;
	261	pSrc[4*i+3] = yt;
	262	}
	263	}
	264