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_fast_q31.c
10 * Description: Processing function for the
11 * Q31 Fast Biquad cascade DirectFormI(DF1) filter.
13 * Target Processor: Cortex-M4/Cortex-M3
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
56 * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
57 * @param[in] *pSrc points to the block of input data.
58 * @param[out] *pDst points to the block of output data.
59 * @param[in] blockSize number of samples to process per call.
62 * <b>Scaling and Overflow Behavior:</b>
64 * This function is optimized for speed at the expense of fixed-point precision and overflow protection.
65 * The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
66 * These intermediate results are added to a 2.30 accumulator.
67 * Finally, the accumulator is saturated and converted to a 1.31 result.
68 * The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
69 * In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25). Use the intialization function
70 * arm_biquad_cascade_df1_init_q31() to initialize filter structure.
73 * Refer to the function <code>arm_biquad_cascade_df1_q31()</code> for a slower implementation of this function which uses 64-bit accumulation to provide higher precision. Both the slow and the fast versions use the same instance structure.
74 * Use the function <code>arm_biquad_cascade_df1_init_q31()</code> to initialize the filter structure.
77 void arm_biquad_cascade_df1_fast_q31(
78 const arm_biquad_casd_df1_inst_q31
* S
,
83 q31_t acc
= 0; /* accumulator */
84 q31_t Xn1
, Xn2
, Yn1
, Yn2
; /* Filter state variables */
85 q31_t b0
, b1
, b2
, a1
, a2
; /* Filter coefficients */
86 q31_t
*pIn
= pSrc
; /* input pointer initialization */
87 q31_t
*pOut
= pDst
; /* output pointer initialization */
88 q31_t
*pState
= S
->pState
; /* pState pointer initialization */
89 q31_t
*pCoeffs
= S
->pCoeffs
; /* coeff pointer initialization */
90 q31_t Xn
; /* temporary input */
91 int32_t shift
= (int32_t) S
->postShift
+ 1; /* Shift to be applied to the output */
92 uint32_t sample
, stage
= S
->numStages
; /* loop counters */
97 /* Reading the coefficients */
104 /* Reading the state values */
110 /* Apply loop unrolling and compute 4 output values simultaneously. */
111 /* The variables acc ... acc3 hold output values that are being computed:
113 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
116 sample
= blockSize
>> 2u;
118 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
119 ** a second loop below computes the remaining 1 to 3 samples. */
125 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
126 /* acc = b0 * x[n] */
127 //acc = (q31_t) (((q63_t) b1 * Xn1) >> 32);
128 mult_32x32_keep32_R(acc
, b1
, Xn1
);
129 /* acc += b1 * x[n-1] */
130 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b0 * (Xn))) >> 32);
131 multAcc_32x32_keep32_R(acc
, b0
, Xn
);
132 /* acc += b[2] * x[n-2] */
133 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
134 multAcc_32x32_keep32_R(acc
, b2
, Xn2
);
135 /* acc += a1 * y[n-1] */
136 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
137 multAcc_32x32_keep32_R(acc
, a1
, Yn1
);
138 /* acc += a2 * y[n-2] */
139 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
140 multAcc_32x32_keep32_R(acc
, a2
, Yn2
);
142 /* The result is converted to 1.31 , Yn2 variable is reused */
145 /* Read the second input */
148 /* Store the output in the destination buffer. */
151 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
152 /* acc = b0 * x[n] */
153 //acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);
154 mult_32x32_keep32_R(acc
, b0
, Xn2
);
155 /* acc += b1 * x[n-1] */
156 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);
157 multAcc_32x32_keep32_R(acc
, b1
, Xn
);
158 /* acc += b[2] * x[n-2] */
159 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);
160 multAcc_32x32_keep32_R(acc
, b2
, Xn1
);
161 /* acc += a1 * y[n-1] */
162 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
163 multAcc_32x32_keep32_R(acc
, a1
, Yn2
);
164 /* acc += a2 * y[n-2] */
165 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
166 multAcc_32x32_keep32_R(acc
, a2
, Yn1
);
168 /* The result is converted to 1.31, Yn1 variable is reused */
171 /* Read the third input */
174 /* Store the output in the destination buffer. */
177 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
178 /* acc = b0 * x[n] */
179 //acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);
180 mult_32x32_keep32_R(acc
, b0
, Xn1
);
181 /* acc += b1 * x[n-1] */
182 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);
183 multAcc_32x32_keep32_R(acc
, b1
, Xn2
);
184 /* acc += b[2] * x[n-2] */
185 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);
186 multAcc_32x32_keep32_R(acc
, b2
, Xn
);
187 /* acc += a1 * y[n-1] */
188 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
189 multAcc_32x32_keep32_R(acc
, a1
, Yn1
);
190 /* acc += a2 * y[n-2] */
191 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
192 multAcc_32x32_keep32_R(acc
, a2
, Yn2
);
194 /* The result is converted to 1.31, Yn2 variable is reused */
197 /* Read the forth input */
200 /* Store the output in the destination buffer. */
204 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
205 /* acc = b0 * x[n] */
206 //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
207 mult_32x32_keep32_R(acc
, b0
, Xn
);
208 /* acc += b1 * x[n-1] */
209 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
210 multAcc_32x32_keep32_R(acc
, b1
, Xn1
);
211 /* acc += b[2] * x[n-2] */
212 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
213 multAcc_32x32_keep32_R(acc
, b2
, Xn2
);
214 /* acc += a1 * y[n-1] */
215 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
216 multAcc_32x32_keep32_R(acc
, a1
, Yn2
);
217 /* acc += a2 * y[n-2] */
218 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
219 multAcc_32x32_keep32_R(acc
, a2
, Yn1
);
221 /* Every time after the output is computed state should be updated. */
222 /* The states should be updated as: */
226 /* The result is converted to 1.31, Yn1 variable is reused */
232 /* Store the output in the destination buffer. */
236 /* decrement the loop counter */
240 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
241 ** No loop unrolling is used. */
242 sample
= (blockSize
& 0x3u
);
249 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
250 /* acc = b0 * x[n] */
251 //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
252 mult_32x32_keep32_R(acc
, b0
, Xn
);
253 /* acc += b1 * x[n-1] */
254 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
255 multAcc_32x32_keep32_R(acc
, b1
, Xn1
);
256 /* acc += b[2] * x[n-2] */
257 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
258 multAcc_32x32_keep32_R(acc
, b2
, Xn2
);
259 /* acc += a1 * y[n-1] */
260 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
261 multAcc_32x32_keep32_R(acc
, a1
, Yn1
);
262 /* acc += a2 * y[n-2] */
263 //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
264 multAcc_32x32_keep32_R(acc
, a2
, Yn2
);
266 /* The result is converted to 1.31 */
269 /* Every time after the output is computed state should be updated. */
270 /* The states should be updated as: */
280 /* Store the output in the destination buffer. */
283 /* decrement the loop counter */
287 /* The first stage goes from the input buffer to the output buffer. */
288 /* Subsequent stages occur in-place in the output buffer */
291 /* Reset to destination pointer */
294 /* Store the updated state variables back into the pState array */
304 * @} end of BiquadCascadeDF1 group