]>
git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_q31.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
10 * Description: Q31 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 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.
60 * <b>Scaling and Overflow Behavior:</b>
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.
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.
73 const arm_fir_instance_q31
* S
,
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 */
83 #ifndef ARM_MATH_CM0_FAMILY
85 /* Run the below code for Cortex-M4 and Cortex-M3 */
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 */
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 )]);
99 /* Apply loop unrolling and compute 4 output values simultaneously.
100 * The variables acc0 ... acc3 hold output values that are being computed:
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]
107 blkCnt
= blockSize
/ 3 ;
108 blockSize
= blockSize
- ( 3 * blkCnt
);
110 tapCnt
= numTaps
/ 3 ;
111 tapCntN3
= numTaps
- ( 3 * tapCnt
);
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. */
117 /* Copy three new input samples into the state buffer */
118 * pStateCurnt
++ = * pSrc
++;
119 * pStateCurnt
++ = * pSrc
++;
120 * pStateCurnt
++ = * pSrc
++;
122 /* Set all accumulators to zero */
127 /* Initialize state pointer */
130 /* Initialize coefficient pointer */
133 /* Read the first two samples from the state buffer:
134 * x[n-numTaps], x[n-numTaps-1] */
138 /* Loop unrolling. Process 3 taps at a time. */
143 /* Read the b[numTaps] coefficient */
146 /* Read x[n-numTaps-2] sample */
149 /* Perform the multiply-accumulates */
150 acc0
+= (( q63_t
) x0
* c0
);
151 acc1
+= (( q63_t
) x1
* c0
);
152 acc2
+= (( q63_t
) x2
* c0
);
154 /* Read the coefficient and state */
158 /* Perform the multiply-accumulates */
159 acc0
+= (( q63_t
) x1
* c0
);
160 acc1
+= (( q63_t
) x2
* c0
);
161 acc2
+= (( q63_t
) x0
* c0
);
163 /* Read the coefficient and state */
167 /* update coefficient pointer */
170 /* Perform the multiply-accumulates */
171 acc0
+= (( q63_t
) x2
* c0
);
172 acc1
+= (( q63_t
) x0
* c0
);
173 acc2
+= (( q63_t
) x1
* c0
);
175 /* Decrement the loop counter */
179 /* If the filter length is not a multiple of 3, compute the remaining filter taps */
185 /* Read coefficients */
188 /* Fetch 1 state variable */
191 /* Perform the multiply-accumulates */
192 acc0
+= (( q63_t
) x0
* c0
);
193 acc1
+= (( q63_t
) x1
* c0
);
194 acc2
+= (( q63_t
) x2
* c0
);
196 /* Reuse the present sample states for next sample */
200 /* Decrement the loop counter */
204 /* Advance the state pointer by 3 to process the next group of 3 samples */
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 );
213 /* Decrement the samples loop counter */
217 /* If the blockSize is not a multiple of 3, compute any remaining output samples here.
218 ** No loop unrolling is used. */
220 while ( blockSize
> 0u )
222 /* Copy one sample at a time into state buffer */
223 * pStateCurnt
++ = * pSrc
++;
225 /* Set the accumulator to zero */
228 /* Initialize state pointer */
231 /* Initialize Coefficient pointer */
236 /* Perform the multiply-accumulates */
239 acc0
+= ( q63_t
) * ( px
++) * (*( pb
++));
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 );
247 /* Advance state pointer by 1 for the next sample */
250 /* Decrement the samples loop counter */
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. */
258 /* Points to the start of the state buffer */
259 pStateCurnt
= S
-> pState
;
261 tapCnt
= ( numTaps
- 1u ) >> 2u ;
266 * pStateCurnt
++ = * pState
++;
267 * pStateCurnt
++ = * pState
++;
268 * pStateCurnt
++ = * pState
++;
269 * pStateCurnt
++ = * pState
++;
271 /* Decrement the loop counter */
275 /* Calculate remaining number of copies */
276 tapCnt
= ( numTaps
- 1u ) % 0x4 u
;
278 /* Copy the remaining q31_t data */
281 * pStateCurnt
++ = * pState
++;
283 /* Decrement the loop counter */
289 /* Run the below code for Cortex-M0 */
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 */
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 )]);
301 /* Initialize blkCnt with blockSize */
306 /* Copy one sample at a time into state buffer */
307 * pStateCurnt
++ = * pSrc
++;
309 /* Set the accumulator to zero */
312 /* Initialize state pointer */
315 /* Initialize Coefficient pointer */
320 /* Perform the multiply-accumulates */
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
++;
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 );
332 /* Advance state pointer by 1 for the next sample */
335 /* Decrement the samples loop counter */
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. */
343 /* Points to the start of the state buffer */
344 pStateCurnt
= S
-> pState
;
346 /* Copy numTaps number of values */
347 tapCnt
= numTaps
- 1u ;
352 * pStateCurnt
++ = * pState
++;
354 /* Decrement the loop counter */
359 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
364 * @} end of FIR group