]>
git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_q7.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
10 * Description: Q7 FIR filter processing function.
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
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
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.
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 * -------------------------------------------------------------------- */
44 * @ingroup groupFilters
53 * @param[in] *S points to an instance of the Q7 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.
59 * <b>Scaling and Overflow Behavior:</b>
61 * The function is implemented using a 32-bit internal accumulator.
62 * Both coefficients and state variables are represented in 1.7 format and multiplications yield a 2.14 result.
63 * The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
64 * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
65 * The accumulator is converted to 18.7 format by discarding the low 7 bits.
66 * Finally, the result is truncated to 1.7 format.
70 const arm_fir_instance_q7
* S
,
76 #ifndef ARM_MATH_CM0_FAMILY
78 /* Run the below code for Cortex-M4 and Cortex-M3 */
80 q7_t
*pState
= S
->pState
; /* State pointer */
81 q7_t
*pCoeffs
= S
->pCoeffs
; /* Coefficient pointer */
82 q7_t
*pStateCurnt
; /* Points to the current sample of the state */
83 q7_t x0
, x1
, x2
, x3
; /* Temporary variables to hold state */
84 q7_t c0
; /* Temporary variable to hold coefficient value */
85 q7_t
*px
; /* Temporary pointer for state */
86 q7_t
*pb
; /* Temporary pointer for coefficient buffer */
87 q31_t acc0
, acc1
, acc2
, acc3
; /* Accumulators */
88 uint32_t numTaps
= S
->numTaps
; /* Number of filter coefficients in the filter */
89 uint32_t i
, tapCnt
, blkCnt
; /* Loop counters */
91 /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
92 /* pStateCurnt points to the location where the new input data should be written */
93 pStateCurnt
= &(S
->pState
[(numTaps
- 1u)]);
95 /* Apply loop unrolling and compute 4 output values simultaneously.
96 * The variables acc0 ... acc3 hold output values that are being computed:
98 * 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]
99 * 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]
100 * 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]
101 * 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]
103 blkCnt
= blockSize
>> 2;
105 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
106 ** a second loop below computes the remaining 1 to 3 samples. */
109 /* Copy four new input samples into the state buffer */
110 *pStateCurnt
++ = *pSrc
++;
111 *pStateCurnt
++ = *pSrc
++;
112 *pStateCurnt
++ = *pSrc
++;
113 *pStateCurnt
++ = *pSrc
++;
115 /* Set all accumulators to zero */
121 /* Initialize state pointer */
124 /* Initialize coefficient pointer */
127 /* Read the first three samples from the state buffer:
128 * x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2] */
133 /* Loop unrolling. Process 4 taps at a time. */
134 tapCnt
= numTaps
>> 2;
139 /* Read the b[numTaps] coefficient */
142 /* Read x[n-numTaps-3] sample */
145 /* acc0 += b[numTaps] * x[n-numTaps] */
146 acc0
+= ((q15_t
) x0
* c0
);
148 /* acc1 += b[numTaps] * x[n-numTaps-1] */
149 acc1
+= ((q15_t
) x1
* c0
);
151 /* acc2 += b[numTaps] * x[n-numTaps-2] */
152 acc2
+= ((q15_t
) x2
* c0
);
154 /* acc3 += b[numTaps] * x[n-numTaps-3] */
155 acc3
+= ((q15_t
) x3
* c0
);
157 /* Read the b[numTaps-1] coefficient */
160 /* Read x[n-numTaps-4] sample */
163 /* Perform the multiply-accumulates */
164 acc0
+= ((q15_t
) x1
* c0
);
165 acc1
+= ((q15_t
) x2
* c0
);
166 acc2
+= ((q15_t
) x3
* c0
);
167 acc3
+= ((q15_t
) x0
* c0
);
169 /* Read the b[numTaps-2] coefficient */
172 /* Read x[n-numTaps-5] sample */
175 /* Perform the multiply-accumulates */
176 acc0
+= ((q15_t
) x2
* c0
);
177 acc1
+= ((q15_t
) x3
* c0
);
178 acc2
+= ((q15_t
) x0
* c0
);
179 acc3
+= ((q15_t
) x1
* c0
);
180 /* Read the b[numTaps-3] coefficients */
183 /* Read x[n-numTaps-6] sample */
186 /* Perform the multiply-accumulates */
187 acc0
+= ((q15_t
) x3
* c0
);
188 acc1
+= ((q15_t
) x0
* c0
);
189 acc2
+= ((q15_t
) x1
* c0
);
190 acc3
+= ((q15_t
) x2
* c0
);
194 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
196 i
= numTaps
- (tapCnt
* 4u);
199 /* Read coefficients */
202 /* Fetch 1 state variable */
205 /* Perform the multiply-accumulates */
206 acc0
+= ((q15_t
) x0
* c0
);
207 acc1
+= ((q15_t
) x1
* c0
);
208 acc2
+= ((q15_t
) x2
* c0
);
209 acc3
+= ((q15_t
) x3
* c0
);
211 /* Reuse the present sample states for next sample */
216 /* Decrement the loop counter */
220 /* Advance the state pointer by 4 to process the next group of 4 samples */
223 /* The results in the 4 accumulators are in 2.62 format. Convert to 1.31
224 ** Then store the 4 outputs in the destination buffer. */
225 acc0
= __SSAT((acc0
>> 7u), 8);
227 acc1
= __SSAT((acc1
>> 7u), 8);
229 acc2
= __SSAT((acc2
>> 7u), 8);
231 acc3
= __SSAT((acc3
>> 7u), 8);
234 /* Decrement the samples loop counter */
239 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
240 ** No loop unrolling is used. */
241 blkCnt
= blockSize
% 4u;
245 /* Copy one sample at a time into state buffer */
246 *pStateCurnt
++ = *pSrc
++;
248 /* Set the accumulator to zero */
251 /* Initialize state pointer */
254 /* Initialize Coefficient pointer */
259 /* Perform the multiply-accumulates */
262 acc0
+= (q15_t
) * (px
++) * (*(pb
++));
266 /* The result is in 2.14 format. Convert to 1.7
267 ** Then store the output in the destination buffer. */
268 *pDst
++ = __SSAT((acc0
>> 7u), 8);
270 /* Advance state pointer by 1 for the next sample */
273 /* Decrement the samples loop counter */
277 /* Processing is complete.
278 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
279 ** This prepares the state buffer for the next function call. */
281 /* Points to the start of the state buffer */
282 pStateCurnt
= S
->pState
;
284 tapCnt
= (numTaps
- 1u) >> 2u;
289 *pStateCurnt
++ = *pState
++;
290 *pStateCurnt
++ = *pState
++;
291 *pStateCurnt
++ = *pState
++;
292 *pStateCurnt
++ = *pState
++;
294 /* Decrement the loop counter */
298 /* Calculate remaining number of copies */
299 tapCnt
= (numTaps
- 1u) % 0x4u
;
301 /* Copy the remaining q31_t data */
304 *pStateCurnt
++ = *pState
++;
306 /* Decrement the loop counter */
312 /* Run the below code for Cortex-M0 */
314 uint32_t numTaps
= S
->numTaps
; /* Number of taps in the filter */
315 uint32_t i
, blkCnt
; /* Loop counters */
316 q7_t
*pState
= S
->pState
; /* State pointer */
317 q7_t
*pCoeffs
= S
->pCoeffs
; /* Coefficient pointer */
318 q7_t
*px
, *pb
; /* Temporary pointers to state and coeff */
319 q31_t acc
= 0; /* Accumlator */
320 q7_t
*pStateCurnt
; /* Points to the current sample of the state */
323 /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
324 /* pStateCurnt points to the location where the new input data should be written */
325 pStateCurnt
= S
->pState
+ (numTaps
- 1u);
327 /* Initialize blkCnt with blockSize */
330 /* Perform filtering upto BlockSize - BlockSize%4 */
333 /* Copy one sample at a time into state buffer */
334 *pStateCurnt
++ = *pSrc
++;
336 /* Set accumulator to zero */
339 /* Initialize state pointer of type q7 */
342 /* Initialize coeff pointer of type q7 */
350 /* 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] */
351 acc
+= (q15_t
) * px
++ * *pb
++;
355 /* Store the 1.7 format filter output in destination buffer */
356 *pDst
++ = (q7_t
) __SSAT((acc
>> 7), 8);
358 /* Advance the state pointer by 1 to process the next sample */
361 /* Decrement the loop counter */
365 /* Processing is complete.
366 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
367 ** This prepares the state buffer for the next function call. */
370 /* Points to the start of the state buffer */
371 pStateCurnt
= S
->pState
;
374 /* Copy numTaps number of values */
380 *pStateCurnt
++ = *pState
++;
384 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
389 * @} end of FIR group