F030C8xx_KLink - Gitblit

QuakeGod

2022-10-17 eb4ffba9f16634e6f8144b9e85a0291e1cb8dfba

提交 \| 用户 \| 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_rfft_q15.c
	9	*
	10	* Description: RFFT & RIFFT Q15 process function
	11	*
	12	*
	13	* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
	14	*
	15	* Redistribution and use in source and binary forms, with or without
	16	* modification, are permitted provided that the following conditions
	17	* are met:
	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
	23	* distribution.
	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.
	27	*
	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	* -------------------------------------------------------------------- */
	41
	42	#include "arm_math.h"
	43
	44	/*--------------------------------------------------------------------
	45	* Internal functions prototypes
	46	--------------------------------------------------------------------*/
	47
	48	void arm_split_rfft_q15(
	49	q15_t * pSrc,
	50	uint32_t fftLen,
	51	q15_t * pATable,
	52	q15_t * pBTable,
	53	q15_t * pDst,
	54	uint32_t modifier);
	55
	56	void arm_split_rifft_q15(
	57	q15_t * pSrc,
	58	uint32_t fftLen,
	59	q15_t * pATable,
	60	q15_t * pBTable,
	61	q15_t * pDst,
	62	uint32_t modifier);
	63
	64	/**
	65	* @addtogroup RealFFT
	66	* @{
	67	*/
	68
	69	/**
	70	* @brief Processing function for the Q15 RFFT/RIFFT.
	71	* @param[in] *S points to an instance of the Q15 RFFT/RIFFT structure.
	72	* @param[in] *pSrc points to the input buffer.
	73	* @param[out] *pDst points to the output buffer.
	74	* @return none.
	75	*
	76	* \par Input an output formats:
	77	* \par
	78	* Internally input is downscaled by 2 for every stage to avoid saturations inside CFFT/CIFFT process.
	79	* Hence the output format is different for different RFFT sizes.
	80	* The input and output formats for different RFFT sizes and number of bits to upscale are mentioned in the tables below for RFFT and RIFFT:
	81	* \par
	82	* \image html RFFTQ15.gif "Input and Output Formats for Q15 RFFT"
	83	* \par
	84	* \image html RIFFTQ15.gif "Input and Output Formats for Q15 RIFFT"
	85	*/
	86
	87	void arm_rfft_q15(
	88	const arm_rfft_instance_q15 * S,
	89	q15_t * pSrc,
	90	q15_t * pDst)
	91	{
	92	const arm_cfft_instance_q15 *S_CFFT = S->pCfft;
	93	uint32_t i;
	94	uint32_t L2 = S->fftLenReal >> 1;
	95
	96	/* Calculation of RIFFT of input */
	97	if(S->ifftFlagR == 1u)
	98	{
	99	/* Real IFFT core process */
	100	arm_split_rifft_q15(pSrc, L2, S->pTwiddleAReal,
	101	S->pTwiddleBReal, pDst, S->twidCoefRModifier);
	102
	103	/* Complex IFFT process */
	104	arm_cfft_q15(S_CFFT, pDst, S->ifftFlagR, S->bitReverseFlagR);
	105
	106	for(i=0;i<S->fftLenReal;i++)
	107	{
	108	pDst[i] = pDst[i] << 1;
	109	}
	110	}
	111	else
	112	{
	113	/* Calculation of RFFT of input */
	114
	115	/* Complex FFT process */
	116	arm_cfft_q15(S_CFFT, pSrc, S->ifftFlagR, S->bitReverseFlagR);
	117
	118	/* Real FFT core process */
	119	arm_split_rfft_q15(pSrc, L2, S->pTwiddleAReal,
	120	S->pTwiddleBReal, pDst, S->twidCoefRModifier);
	121	}
	122	}
	123
	124	/**
	125	* @} end of RealFFT group
	126	*/
	127
	128	/**
	129	* @brief Core Real FFT process
	130	* @param *pSrc points to the input buffer.
	131	* @param fftLen length of FFT.
	132	* @param *pATable points to the A twiddle Coef buffer.
	133	* @param *pBTable points to the B twiddle Coef buffer.
	134	* @param *pDst points to the output buffer.
	135	* @param modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
	136	* @return none.
	137	* The function implements a Real FFT
	138	*/
	139
	140	void arm_split_rfft_q15(
	141	q15_t * pSrc,
	142	uint32_t fftLen,
	143	q15_t * pATable,
	144	q15_t * pBTable,
	145	q15_t * pDst,
	146	uint32_t modifier)
	147	{
	148	uint32_t i; /* Loop Counter */
	149	q31_t outR, outI; /* Temporary variables for output */
	150	q15_t pCoefA, pCoefB; /* Temporary pointers for twiddle factors */
	151	q15_t pSrc1, pSrc2;
	152	#ifndef ARM_MATH_CM0_FAMILY
	153	q15_t pD1, pD2;
	154	#endif
	155
	156	// pSrc[2u * fftLen] = pSrc[0];
	157	// pSrc[(2u * fftLen) + 1u] = pSrc[1];
	158
	159	pCoefA = &pATable[modifier * 2u];
	160	pCoefB = &pBTable[modifier * 2u];
	161
	162	pSrc1 = &pSrc[2];
	163	pSrc2 = &pSrc[(2u * fftLen) - 2u];
	164
	165	#ifndef ARM_MATH_CM0_FAMILY
	166
	167	/* Run the below code for Cortex-M4 and Cortex-M3 */
	168	i = 1u;
	169	pD1 = pDst + 2;
	170	pD2 = pDst + (4u * fftLen) - 2;
	171
	172	for(i = fftLen - 1; i > 0; i--)
	173	{
	174	/*
	175	outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
	176	+ pSrc[2 * n - 2 * i] * pBTable[2 * i] +
	177	pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
	178	*/
	179
	180	/* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
	181	pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
	182	pIn[2 * n - 2 * i + 1] * pBTable[2 * i]); */
	183
	184
	185	#ifndef ARM_MATH_BIG_ENDIAN
	186
	187	/* pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1] */
	188	outR = __SMUSD(__SIMD32(pSrc1), __SIMD32(pCoefA));
	189
	190	#else
	191
	192	/* -(pSrc[2 * i + 1] * pATable[2 * i + 1] - pSrc[2 * i] * pATable[2 * i]) */
	193	outR = -(__SMUSD(__SIMD32(pSrc1), __SIMD32(pCoefA)));
	194
	195	#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
	196
	197	/* pSrc[2 * n - 2 * i] * pBTable[2 * i] +
	198	pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
	199	outR = __SMLAD(__SIMD32(pSrc2), __SIMD32(pCoefB), outR) >> 16u;
	200
	201	/* pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
	202	pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
	203
	204	#ifndef ARM_MATH_BIG_ENDIAN
	205
	206	outI = __SMUSDX(__SIMD32(pSrc2)--, __SIMD32(pCoefB));
	207
	208	#else
	209
	210	outI = __SMUSDX(__SIMD32(pCoefB), __SIMD32(pSrc2)--);
	211
	212	#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
	213
	214	/* (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] */
	215	outI = __SMLADX(__SIMD32(pSrc1)++, __SIMD32(pCoefA), outI);
	216
	217	/* write output */
	218	*pD1++ = (q15_t) outR;
	219	*pD1++ = outI >> 16u;
	220
	221	/* write complex conjugate output */
	222	pD2[0] = (q15_t) outR;
	223	pD2[1] = -(outI >> 16u);
	224	pD2 -= 2;
	225
	226	/* update coefficient pointer */
	227	pCoefB = pCoefB + (2u * modifier);
	228	pCoefA = pCoefA + (2u * modifier);
	229	}
	230
	231	pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
	232	pDst[(2u * fftLen) + 1u] = 0;
	233
	234	pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
	235	pDst[1] = 0;
	236
	237	#else
	238
	239	/* Run the below code for Cortex-M0 */
	240	i = 1u;
	241
	242	while(i < fftLen)
	243	{
	244	/*
	245	outR = (pSrc[2 * i] * pATable[2 * i] - pSrc[2 * i + 1] * pATable[2 * i + 1]
	246	+ pSrc[2 * n - 2 * i] * pBTable[2 * i] +
	247	pSrc[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
	248	*/
	249
	250	outR = pSrc1 *pCoefA;
	251	outR = outR - ((pSrc1 + 1) *(pCoefA + 1));
	252	outR = outR + (pSrc2 *pCoefB);
	253	outR = (outR + ((pSrc2 + 1) *(pCoefB + 1))) >> 16;
	254
	255
	256	/* outI = (pIn[2 * i + 1] * pATable[2 * i] + pIn[2 * i] * pATable[2 * i + 1] +
	257	pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
	258	pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
	259	*/
	260
	261	outI = pSrc2 *(pCoefB + 1);
	262	outI = outI - ((pSrc2 + 1) *pCoefB);
	263	outI = outI + ((pSrc1 + 1) *pCoefA);
	264	outI = outI + (pSrc1 *(pCoefA + 1));
	265
	266	/* update input pointers */
	267	pSrc1 += 2u;
	268	pSrc2 -= 2u;
	269
	270	/* write output */
	271	pDst[2u * i] = (q15_t) outR;
	272	pDst[(2u * i) + 1u] = outI >> 16u;
	273
	274	/* write complex conjugate output */
	275	pDst[(4u * fftLen) - (2u * i)] = (q15_t) outR;
	276	pDst[((4u * fftLen) - (2u * i)) + 1u] = -(outI >> 16u);
	277
	278	/* update coefficient pointer */
	279	pCoefB = pCoefB + (2u * modifier);
	280	pCoefA = pCoefA + (2u * modifier);
	281
	282	i++;
	283	}
	284
	285	pDst[2u * fftLen] = (pSrc[0] - pSrc[1]) >> 1;
	286	pDst[(2u * fftLen) + 1u] = 0;
	287
	288	pDst[0] = (pSrc[0] + pSrc[1]) >> 1;
	289	pDst[1] = 0;
	290
	291	#endif /* #ifndef ARM_MATH_CM0_FAMILY */
	292	}
	293
	294
	295	/**
	296	* @brief Core Real IFFT process
	297	* @param[in] *pSrc points to the input buffer.
	298	* @param[in] fftLen length of FFT.
	299	* @param[in] *pATable points to the twiddle Coef A buffer.
	300	* @param[in] *pBTable points to the twiddle Coef B buffer.
	301	* @param[out] *pDst points to the output buffer.
	302	* @param[in] modifier twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table.
	303	* @return none.
	304	* The function implements a Real IFFT
	305	*/
	306	void arm_split_rifft_q15(
	307	q15_t * pSrc,
	308	uint32_t fftLen,
	309	q15_t * pATable,
	310	q15_t * pBTable,
	311	q15_t * pDst,
	312	uint32_t modifier)
	313	{
	314	uint32_t i; /* Loop Counter */
	315	q31_t outR, outI; /* Temporary variables for output */
	316	q15_t pCoefA, pCoefB; /* Temporary pointers for twiddle factors */
	317	q15_t pSrc1, pSrc2;
	318	q15_t *pDst1 = &pDst[0];
	319
	320	pCoefA = &pATable[0];
	321	pCoefB = &pBTable[0];
	322
	323	pSrc1 = &pSrc[0];
	324	pSrc2 = &pSrc[2u * fftLen];
	325
	326	#ifndef ARM_MATH_CM0_FAMILY
	327
	328	/* Run the below code for Cortex-M4 and Cortex-M3 */
	329	i = fftLen;
	330
	331	while(i > 0u)
	332	{
	333	/*
	334	outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
	335	pIn[2 * n - 2 * i] * pBTable[2 * i] -
	336	pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
	337
	338	outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
	339	pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
	340	pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
	341	*/
	342
	343
	344	#ifndef ARM_MATH_BIG_ENDIAN
	345
	346	/* pIn[2 * n - 2 * i] * pBTable[2 * i] -
	347	pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]) */
	348	outR = __SMUSD(__SIMD32(pSrc2), __SIMD32(pCoefB));
	349
	350	#else
	351
	352	/* -(-pIn[2 * n - 2 * i] * pBTable[2 * i] +
	353	pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1])) */
	354	outR = -(__SMUSD(__SIMD32(pSrc2), __SIMD32(pCoefB)));
	355
	356	#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
	357
	358	/* pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
	359	pIn[2 * n - 2 * i] * pBTable[2 * i] */
	360	outR = __SMLAD(__SIMD32(pSrc1), __SIMD32(pCoefA), outR) >> 16u;
	361
	362	/*
	363	-pIn[2 * n - 2 * i] * pBTable[2 * i + 1] +
	364	pIn[2 * n - 2 * i + 1] * pBTable[2 * i] */
	365	outI = __SMUADX(__SIMD32(pSrc2)--, __SIMD32(pCoefB));
	366
	367	/* pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] */
	368
	369	#ifndef ARM_MATH_BIG_ENDIAN
	370
	371	outI = __SMLSDX(__SIMD32(pCoefA), __SIMD32(pSrc1)++, -outI);
	372
	373	#else
	374
	375	outI = __SMLSDX(__SIMD32(pSrc1)++, __SIMD32(pCoefA), -outI);
	376
	377	#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
	378	/* write output */
	379
	380	#ifndef ARM_MATH_BIG_ENDIAN
	381
	382	*__SIMD32(pDst1)++ = __PKHBT(outR, (outI >> 16u), 16);
	383
	384	#else
	385
	386	*__SIMD32(pDst1)++ = __PKHBT((outI >> 16u), outR, 16);
	387
	388	#endif /* #ifndef ARM_MATH_BIG_ENDIAN */
	389
	390	/* update coefficient pointer */
	391	pCoefB = pCoefB + (2u * modifier);
	392	pCoefA = pCoefA + (2u * modifier);
	393
	394	i--;
	395	}
	396	#else
	397	/* Run the below code for Cortex-M0 */
	398	i = fftLen;
	399
	400	while(i > 0u)
	401	{
	402	/*
	403	outR = (pIn[2 * i] * pATable[2 * i] + pIn[2 * i + 1] * pATable[2 * i + 1] +
	404	pIn[2 * n - 2 * i] * pBTable[2 * i] -
	405	pIn[2 * n - 2 * i + 1] * pBTable[2 * i + 1]);
	406	*/
	407
	408	outR = pSrc2 *pCoefB;
	409	outR = outR - ((pSrc2 + 1) *(pCoefB + 1));
	410	outR = outR + (pSrc1 *pCoefA);
	411	outR = (outR + ((pSrc1 + 1) *(pCoefA + 1))) >> 16;
	412
	413	/*
	414	outI = (pIn[2 * i + 1] * pATable[2 * i] - pIn[2 * i] * pATable[2 * i + 1] -
	415	pIn[2 * n - 2 * i] * pBTable[2 * i + 1] -
	416	pIn[2 * n - 2 * i + 1] * pBTable[2 * i]);
	417	*/
	418
	419	outI = (pSrc1 + 1) *pCoefA;
	420	outI = outI - (pSrc1 *(pCoefA + 1));
	421	outI = outI - (pSrc2 *(pCoefB + 1));
	422	outI = outI - ((pSrc2 + 1) *(pCoefB));
	423
	424	/* update input pointers */
	425	pSrc1 += 2u;
	426	pSrc2 -= 2u;
	427
	428	/* write output */
	429	*pDst1++ = (q15_t) outR;
	430	*pDst1++ = (q15_t) (outI >> 16);
	431
	432	/* update coefficient pointer */
	433	pCoefB = pCoefB + (2u * modifier);
	434	pCoefA = pCoefA + (2u * modifier);
	435
	436	i--;
	437	}
	438	#endif /* #ifndef ARM_MATH_CM0_FAMILY */
	439	}