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