F030C8xx_KLink - Gitblit

QuakeGod

2021-06-20 bfc108e6097eff2bec73050e261f3b9e5db447b7

提交 \| 用户 \| 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_fir_q31.c
	9	*
	10	* Description: Q31 FIR filter 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	/**
	44	* @ingroup groupFilters
	45	*/
	46
	47	/**
	48	* @addtogroup FIR
	49	* @{
	50	*/
	51
	52	/**
	53	* @param[in] *S points to an instance of the Q31 FIR filter structure.
	54	* @param[in] *pSrc points to the block of input data.
	55	* @param[out] *pDst points to the block of output data.
	56	* @param[in] blockSize number of samples to process per call.
	57	* @return none.
	58	*
	59	* @details
	60	* <b>Scaling and Overflow Behavior:</b>
	61	* \par
	62	* The function is implemented using an internal 64-bit accumulator.
	63	* The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
	64	* Thus, if the accumulator result overflows it wraps around rather than clip.
	65	* In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits.
	66	* After all multiply-accumulates are performed, the 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
	67	*
	68	* \par
	69	* Refer to the function <code>arm_fir_fast_q31()</code> for a faster but less precise implementation of this filter for Cortex-M3 and Cortex-M4.
	70	*/
	71
	72	void arm_fir_q31(
	73	const arm_fir_instance_q31 * S,
	74	q31_t * pSrc,
	75	q31_t * pDst,
	76	uint32_t blockSize)
	77	{
	78	q31_t pState = S->pState; / State pointer */
	79	q31_t pCoeffs = S->pCoeffs; / Coefficient pointer */
	80	q31_t pStateCurnt; / Points to the current sample of the state */
	81
	82
	83	#ifndef ARM_MATH_CM0_FAMILY
	84
	85	/* Run the below code for Cortex-M4 and Cortex-M3 */
	86
	87	q31_t x0, x1, x2; /* Temporary variables to hold state */
	88	q31_t c0; /* Temporary variable to hold coefficient value */
	89	q31_t px; / Temporary pointer for state */
	90	q31_t pb; / Temporary pointer for coefficient buffer */
	91	q63_t acc0, acc1, acc2; /* Accumulators */
	92	uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
	93	uint32_t i, tapCnt, blkCnt, tapCntN3; /* Loop counters */
	94
	95	/* S->pState points to state array which contains previous frame (numTaps - 1) samples */
	96	/* pStateCurnt points to the location where the new input data should be written */
	97	pStateCurnt = &(S->pState[(numTaps - 1u)]);
	98
	99	/* Apply loop unrolling and compute 4 output values simultaneously.
	100	* The variables acc0 ... acc3 hold output values that are being computed:
	101	*
	102	* acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
	103	* acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
	104	* acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
	105	* acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
	106	*/
	107	blkCnt = blockSize / 3;
	108	blockSize = blockSize - (3 * blkCnt);
	109
	110	tapCnt = numTaps / 3;
	111	tapCntN3 = numTaps - (3 * tapCnt);
	112
	113	/* First part of the processing with loop unrolling. Compute 4 outputs at a time.
	114	** a second loop below computes the remaining 1 to 3 samples. */
	115	while(blkCnt > 0u)
	116	{
	117	/* Copy three new input samples into the state buffer */
	118	pStateCurnt++ = pSrc++;
	119	pStateCurnt++ = pSrc++;
	120	pStateCurnt++ = pSrc++;
	121
	122	/* Set all accumulators to zero */
	123	acc0 = 0;
	124	acc1 = 0;
	125	acc2 = 0;
	126
	127	/* Initialize state pointer */
	128	px = pState;
	129
	130	/* Initialize coefficient pointer */
	131	pb = pCoeffs;
	132
	133	/* Read the first two samples from the state buffer:
	134	* x[n-numTaps], x[n-numTaps-1] */
	135	x0 = *(px++);
	136	x1 = *(px++);
	137
	138	/* Loop unrolling. Process 3 taps at a time. */
	139	i = tapCnt;
	140
	141	while(i > 0u)
	142	{
	143	/* Read the b[numTaps] coefficient */
	144	c0 = *pb;
	145
	146	/* Read x[n-numTaps-2] sample */
	147	x2 = *(px++);
	148
	149	/* Perform the multiply-accumulates */
	150	acc0 += ((q63_t) x0 * c0);
	151	acc1 += ((q63_t) x1 * c0);
	152	acc2 += ((q63_t) x2 * c0);
	153
	154	/* Read the coefficient and state */
	155	c0 = *(pb + 1u);
	156	x0 = *(px++);
	157
	158	/* Perform the multiply-accumulates */
	159	acc0 += ((q63_t) x1 * c0);
	160	acc1 += ((q63_t) x2 * c0);
	161	acc2 += ((q63_t) x0 * c0);
	162
	163	/* Read the coefficient and state */
	164	c0 = *(pb + 2u);
	165	x1 = *(px++);
	166
	167	/* update coefficient pointer */
	168	pb += 3u;
	169
	170	/* Perform the multiply-accumulates */
	171	acc0 += ((q63_t) x2 * c0);
	172	acc1 += ((q63_t) x0 * c0);
	173	acc2 += ((q63_t) x1 * c0);
	174
	175	/* Decrement the loop counter */
	176	i--;
	177	}
	178
	179	/* If the filter length is not a multiple of 3, compute the remaining filter taps */
	180
	181	i = tapCntN3;
	182
	183	while(i > 0u)
	184	{
	185	/* Read coefficients */
	186	c0 = *(pb++);
	187
	188	/* Fetch 1 state variable */
	189	x2 = *(px++);
	190
	191	/* Perform the multiply-accumulates */
	192	acc0 += ((q63_t) x0 * c0);
	193	acc1 += ((q63_t) x1 * c0);
	194	acc2 += ((q63_t) x2 * c0);
	195
	196	/* Reuse the present sample states for next sample */
	197	x0 = x1;
	198	x1 = x2;
	199
	200	/* Decrement the loop counter */
	201	i--;
	202	}
	203
	204	/* Advance the state pointer by 3 to process the next group of 3 samples */
	205	pState = pState + 3;
	206
	207	/* The results in the 3 accumulators are in 2.30 format. Convert to 1.31
	208	** Then store the 3 outputs in the destination buffer. */
	209	*pDst++ = (q31_t) (acc0 >> 31u);
	210	*pDst++ = (q31_t) (acc1 >> 31u);
	211	*pDst++ = (q31_t) (acc2 >> 31u);
	212
	213	/* Decrement the samples loop counter */
	214	blkCnt--;
	215	}
	216
	217	/* If the blockSize is not a multiple of 3, compute any remaining output samples here.
	218	** No loop unrolling is used. */
	219
	220	while(blockSize > 0u)
	221	{
	222	/* Copy one sample at a time into state buffer */
	223	pStateCurnt++ = pSrc++;
	224
	225	/* Set the accumulator to zero */
	226	acc0 = 0;
	227
	228	/* Initialize state pointer */
	229	px = pState;
	230
	231	/* Initialize Coefficient pointer */
	232	pb = (pCoeffs);
	233
	234	i = numTaps;
	235
	236	/* Perform the multiply-accumulates */
	237	do
	238	{
	239	acc0 += (q63_t) * (px++) * (*(pb++));
	240	i--;
	241	} while(i > 0u);
	242
	243	/* The result is in 2.62 format. Convert to 1.31
	244	** Then store the output in the destination buffer. */
	245	*pDst++ = (q31_t) (acc0 >> 31u);
	246
	247	/* Advance state pointer by 1 for the next sample */
	248	pState = pState + 1;
	249
	250	/* Decrement the samples loop counter */
	251	blockSize--;
	252	}
	253
	254	/* Processing is complete.
	255	** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
	256	** This prepares the state buffer for the next function call. */
	257
	258	/* Points to the start of the state buffer */
	259	pStateCurnt = S->pState;
	260
	261	tapCnt = (numTaps - 1u) >> 2u;
	262
	263	/* copy data */
	264	while(tapCnt > 0u)
	265	{
	266	pStateCurnt++ = pState++;
	267	pStateCurnt++ = pState++;
	268	pStateCurnt++ = pState++;
	269	pStateCurnt++ = pState++;
	270
	271	/* Decrement the loop counter */
	272	tapCnt--;
	273	}
	274
	275	/* Calculate remaining number of copies */
	276	tapCnt = (numTaps - 1u) % 0x4u;
	277
	278	/* Copy the remaining q31_t data */
	279	while(tapCnt > 0u)
	280	{
	281	pStateCurnt++ = pState++;
	282
	283	/* Decrement the loop counter */
	284	tapCnt--;
	285	}
	286
	287	#else
	288
	289	/* Run the below code for Cortex-M0 */
	290
	291	q31_t px; / Temporary pointer for state */
	292	q31_t pb; / Temporary pointer for coefficient buffer */
	293	q63_t acc; /* Accumulator */
	294	uint32_t numTaps = S->numTaps; /* Length of the filter */
	295	uint32_t i, tapCnt, blkCnt; /* Loop counters */
	296
	297	/* S->pState buffer contains previous frame (numTaps - 1) samples */
	298	/* pStateCurnt points to the location where the new input data should be written */
	299	pStateCurnt = &(S->pState[(numTaps - 1u)]);
	300
	301	/* Initialize blkCnt with blockSize */
	302	blkCnt = blockSize;
	303
	304	while(blkCnt > 0u)
	305	{
	306	/* Copy one sample at a time into state buffer */
	307	pStateCurnt++ = pSrc++;
	308
	309	/* Set the accumulator to zero */
	310	acc = 0;
	311
	312	/* Initialize state pointer */
	313	px = pState;
	314
	315	/* Initialize Coefficient pointer */
	316	pb = pCoeffs;
	317
	318	i = numTaps;
	319
	320	/* Perform the multiply-accumulates */
	321	do
	322	{
	323	/* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
	324	acc += (q63_t) * px++ * *pb++;
	325	i--;
	326	} while(i > 0u);
	327
	328	/* The result is in 2.62 format. Convert to 1.31
	329	** Then store the output in the destination buffer. */
	330	*pDst++ = (q31_t) (acc >> 31u);
	331
	332	/* Advance state pointer by 1 for the next sample */
	333	pState = pState + 1;
	334
	335	/* Decrement the samples loop counter */
	336	blkCnt--;
	337	}
	338
	339	/* Processing is complete.
	340	** Now copy the last numTaps - 1 samples to the starting of the state buffer.
	341	** This prepares the state buffer for the next function call. */
	342
	343	/* Points to the start of the state buffer */
	344	pStateCurnt = S->pState;
	345
	346	/* Copy numTaps number of values */
	347	tapCnt = numTaps - 1u;
	348
	349	/* Copy the data */
	350	while(tapCnt > 0u)
	351	{
	352	pStateCurnt++ = pState++;
	353
	354	/* Decrement the loop counter */
	355	tapCnt--;
	356	}
	357
	358
	359	#endif /* #ifndef ARM_MATH_CM0_FAMILY */
	360
	361	}
	362
	363	/**
	364	* @} end of FIR group
	365	*/