1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
8 * Title: arm_biquad_cascade_df1_q31.c
10 * Description: Processing function for the
11 * Q31 Biquad cascade filter
13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
15 * Redistribution and use in source and binary forms, with or without
16 * modification, are permitted provided that the following conditions
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
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.
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 * -------------------------------------------------------------------- */
45 * @ingroup groupFilters
49 * @addtogroup BiquadCascadeDF1
54 * @brief Processing function for the Q31 Biquad cascade filter.
55 * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
56 * @param[in] *pSrc points to the block of input data.
57 * @param[out] *pDst points to the block of output data.
58 * @param[in] blockSize number of samples to process per call.
61 * <b>Scaling and Overflow Behavior:</b>
63 * The function is implemented using an internal 64-bit accumulator.
64 * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
65 * Thus, if the accumulator result overflows it wraps around rather than clip.
66 * In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).
67 * After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to
68 * 1.31 format by discarding the low 32 bits.
71 * Refer to the function <code>arm_biquad_cascade_df1_fast_q31()</code> for a faster but less precise implementation of this filter for Cortex-M3 and Cortex-M4.
74 void arm_biquad_cascade_df1_q31(
75 const arm_biquad_casd_df1_inst_q31
* S
,
80 q63_t acc
; /* accumulator */
81 uint32_t uShift
= ((uint32_t) S
->postShift
+ 1u);
82 uint32_t lShift
= 32u - uShift
; /* Shift to be applied to the output */
83 q31_t
*pIn
= pSrc
; /* input pointer initialization */
84 q31_t
*pOut
= pDst
; /* output pointer initialization */
85 q31_t
*pState
= S
->pState
; /* pState pointer initialization */
86 q31_t
*pCoeffs
= S
->pCoeffs
; /* coeff pointer initialization */
87 q31_t Xn1
, Xn2
, Yn1
, Yn2
; /* Filter state variables */
88 q31_t b0
, b1
, b2
, a1
, a2
; /* Filter coefficients */
89 q31_t Xn
; /* temporary input */
90 uint32_t sample
, stage
= S
->numStages
; /* loop counters */
93 #ifndef ARM_MATH_CM0_FAMILY
95 q31_t acc_l
, acc_h
; /* temporary output variables */
97 /* Run the below code for Cortex-M4 and Cortex-M3 */
101 /* Reading the coefficients */
108 /* Reading the state values */
114 /* Apply loop unrolling and compute 4 output values simultaneously. */
115 /* The variable acc hold output values that are being computed:
117 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
120 sample
= blockSize
>> 2u;
122 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
123 ** a second loop below computes the remaining 1 to 3 samples. */
129 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
131 /* acc = b0 * x[n] */
132 acc
= (q63_t
) b0
*Xn
;
133 /* acc += b1 * x[n-1] */
134 acc
+= (q63_t
) b1
*Xn1
;
135 /* acc += b[2] * x[n-2] */
136 acc
+= (q63_t
) b2
*Xn2
;
137 /* acc += a1 * y[n-1] */
138 acc
+= (q63_t
) a1
*Yn1
;
139 /* acc += a2 * y[n-2] */
140 acc
+= (q63_t
) a2
*Yn2
;
142 /* The result is converted to 1.31 , Yn2 variable is reused */
144 /* Calc lower part of acc */
145 acc_l
= acc
& 0xffffffff;
147 /* Calc upper part of acc */
148 acc_h
= (acc
>> 32) & 0xffffffff;
150 /* Apply shift for lower part of acc and upper part of acc */
151 Yn2
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
153 /* Store the output in the destination buffer. */
156 /* Read the second input */
159 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
161 /* acc = b0 * x[n] */
162 acc
= (q63_t
) b0
*Xn2
;
163 /* acc += b1 * x[n-1] */
164 acc
+= (q63_t
) b1
*Xn
;
165 /* acc += b[2] * x[n-2] */
166 acc
+= (q63_t
) b2
*Xn1
;
167 /* acc += a1 * y[n-1] */
168 acc
+= (q63_t
) a1
*Yn2
;
169 /* acc += a2 * y[n-2] */
170 acc
+= (q63_t
) a2
*Yn1
;
173 /* The result is converted to 1.31, Yn1 variable is reused */
175 /* Calc lower part of acc */
176 acc_l
= acc
& 0xffffffff;
178 /* Calc upper part of acc */
179 acc_h
= (acc
>> 32) & 0xffffffff;
182 /* Apply shift for lower part of acc and upper part of acc */
183 Yn1
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
185 /* Store the output in the destination buffer. */
188 /* Read the third input */
191 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
193 /* acc = b0 * x[n] */
194 acc
= (q63_t
) b0
*Xn1
;
195 /* acc += b1 * x[n-1] */
196 acc
+= (q63_t
) b1
*Xn2
;
197 /* acc += b[2] * x[n-2] */
198 acc
+= (q63_t
) b2
*Xn
;
199 /* acc += a1 * y[n-1] */
200 acc
+= (q63_t
) a1
*Yn1
;
201 /* acc += a2 * y[n-2] */
202 acc
+= (q63_t
) a2
*Yn2
;
204 /* The result is converted to 1.31, Yn2 variable is reused */
205 /* Calc lower part of acc */
206 acc_l
= acc
& 0xffffffff;
208 /* Calc upper part of acc */
209 acc_h
= (acc
>> 32) & 0xffffffff;
212 /* Apply shift for lower part of acc and upper part of acc */
213 Yn2
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
215 /* Store the output in the destination buffer. */
218 /* Read the forth input */
221 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
223 /* acc = b0 * x[n] */
224 acc
= (q63_t
) b0
*Xn
;
225 /* acc += b1 * x[n-1] */
226 acc
+= (q63_t
) b1
*Xn1
;
227 /* acc += b[2] * x[n-2] */
228 acc
+= (q63_t
) b2
*Xn2
;
229 /* acc += a1 * y[n-1] */
230 acc
+= (q63_t
) a1
*Yn2
;
231 /* acc += a2 * y[n-2] */
232 acc
+= (q63_t
) a2
*Yn1
;
234 /* The result is converted to 1.31, Yn1 variable is reused */
235 /* Calc lower part of acc */
236 acc_l
= acc
& 0xffffffff;
238 /* Calc upper part of acc */
239 acc_h
= (acc
>> 32) & 0xffffffff;
241 /* Apply shift for lower part of acc and upper part of acc */
242 Yn1
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
244 /* Every time after the output is computed state should be updated. */
245 /* The states should be updated as: */
253 /* Store the output in the destination buffer. */
256 /* decrement the loop counter */
260 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
261 ** No loop unrolling is used. */
262 sample
= (blockSize
& 0x3u
);
269 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
271 /* acc = b0 * x[n] */
272 acc
= (q63_t
) b0
*Xn
;
273 /* acc += b1 * x[n-1] */
274 acc
+= (q63_t
) b1
*Xn1
;
275 /* acc += b[2] * x[n-2] */
276 acc
+= (q63_t
) b2
*Xn2
;
277 /* acc += a1 * y[n-1] */
278 acc
+= (q63_t
) a1
*Yn1
;
279 /* acc += a2 * y[n-2] */
280 acc
+= (q63_t
) a2
*Yn2
;
282 /* The result is converted to 1.31 */
285 /* Every time after the output is computed state should be updated. */
286 /* The states should be updated as: */
296 /* Store the output in the destination buffer. */
297 *pOut
++ = (q31_t
) acc
;
299 /* decrement the loop counter */
303 /* The first stage goes from the input buffer to the output buffer. */
304 /* Subsequent stages occur in-place in the output buffer */
307 /* Reset to destination pointer */
310 /* Store the updated state variables back into the pState array */
320 /* Run the below code for Cortex-M0 */
324 /* Reading the coefficients */
331 /* Reading the state values */
337 /* The variables acc holds the output value that is computed:
338 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
348 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
349 /* acc = b0 * x[n] */
350 acc
= (q63_t
) b0
*Xn
;
352 /* acc += b1 * x[n-1] */
353 acc
+= (q63_t
) b1
*Xn1
;
354 /* acc += b[2] * x[n-2] */
355 acc
+= (q63_t
) b2
*Xn2
;
356 /* acc += a1 * y[n-1] */
357 acc
+= (q63_t
) a1
*Yn1
;
358 /* acc += a2 * y[n-2] */
359 acc
+= (q63_t
) a2
*Yn2
;
361 /* The result is converted to 1.31 */
364 /* Every time after the output is computed state should be updated. */
365 /* The states should be updated as: */
375 /* Store the output in the destination buffer. */
376 *pOut
++ = (q31_t
) acc
;
378 /* decrement the loop counter */
382 /* The first stage goes from the input buffer to the output buffer. */
383 /* Subsequent stages occur in-place in the output buffer */
386 /* Reset to destination pointer */
389 /* Store the updated state variables back into the pState array */
397 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
401 * @} end of BiquadCascadeDF1 group