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_q15.c
10 * Description: Processing function for the
11 * Q15 Biquad cascade DirectFormI(DF1) 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 Q15 Biquad cascade filter.
55 * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
56 * @param[in] *pSrc points to the block of input data.
57 * @param[out] *pDst points to the location where the output result is written.
58 * @param[in] blockSize number of samples to process per call.
62 * <b>Scaling and Overflow Behavior:</b>
64 * The function is implemented using a 64-bit internal accumulator.
65 * Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
66 * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
67 * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
68 * The accumulator is then shifted by <code>postShift</code> bits to truncate the result to 1.15 format by discarding the low 16 bits.
69 * Finally, the result is saturated to 1.15 format.
72 * Refer to the function <code>arm_biquad_cascade_df1_fast_q15()</code> for a faster but less precise implementation of this filter for Cortex-M3 and Cortex-M4.
75 void arm_biquad_cascade_df1_q15(
76 const arm_biquad_casd_df1_inst_q15
* S
,
83 #ifndef ARM_MATH_CM0_FAMILY
85 /* Run the below code for Cortex-M4 and Cortex-M3 */
87 q15_t
*pIn
= pSrc
; /* Source pointer */
88 q15_t
*pOut
= pDst
; /* Destination pointer */
89 q31_t in
; /* Temporary variable to hold input value */
90 q31_t out
; /* Temporary variable to hold output value */
91 q31_t b0
; /* Temporary variable to hold bo value */
92 q31_t b1
, a1
; /* Filter coefficients */
93 q31_t state_in
, state_out
; /* Filter state variables */
95 q63_t acc
; /* Accumulator */
96 int32_t lShift
= (15 - (int32_t) S
->postShift
); /* Post shift */
97 q15_t
*pState
= S
->pState
; /* State pointer */
98 q15_t
*pCoeffs
= S
->pCoeffs
; /* Coefficient pointer */
99 uint32_t sample
, stage
= (uint32_t) S
->numStages
; /* Stage loop counter */
100 int32_t uShift
= (32 - lShift
);
104 /* Read the b0 and 0 coefficients using SIMD */
105 b0
= *__SIMD32(pCoeffs
)++;
107 /* Read the b1 and b2 coefficients using SIMD */
108 b1
= *__SIMD32(pCoeffs
)++;
110 /* Read the a1 and a2 coefficients using SIMD */
111 a1
= *__SIMD32(pCoeffs
)++;
113 /* Read the input state values from the state buffer: x[n-1], x[n-2] */
114 state_in
= *__SIMD32(pState
)++;
116 /* Read the output state values from the state buffer: y[n-1], y[n-2] */
117 state_out
= *__SIMD32(pState
)--;
119 /* Apply loop unrolling and compute 2 output values simultaneously. */
120 /* The variable acc hold output values that are being computed:
122 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
123 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
125 sample
= blockSize
>> 1u;
127 /* First part of the processing with loop unrolling. Compute 2 outputs at a time.
128 ** a second loop below computes the remaining 1 sample. */
133 in
= *__SIMD32(pIn
)++;
135 /* out = b0 * x[n] + 0 * 0 */
136 out
= __SMUAD(b0
, in
);
138 /* acc += b1 * x[n-1] + b2 * x[n-2] + out */
139 acc
= __SMLALD(b1
, state_in
, out
);
140 /* acc += a1 * y[n-1] + a2 * y[n-2] */
141 acc
= __SMLALD(a1
, state_out
, acc
);
143 /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
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 out
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
153 out
= __SSAT(out
, 16);
155 /* Every time after the output is computed state should be updated. */
156 /* The states should be updated as: */
161 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
162 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
164 #ifndef ARM_MATH_BIG_ENDIAN
166 state_in
= __PKHBT(in
, state_in
, 16);
167 state_out
= __PKHBT(out
, state_out
, 16);
171 state_in
= __PKHBT(state_in
>> 16, (in
>> 16), 16);
172 state_out
= __PKHBT(state_out
>> 16, (out
), 16);
174 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
176 /* out = b0 * x[n] + 0 * 0 */
177 out
= __SMUADX(b0
, in
);
178 /* acc += b1 * x[n-1] + b2 * x[n-2] + out */
179 acc
= __SMLALD(b1
, state_in
, out
);
180 /* acc += a1 * y[n-1] + a2 * y[n-2] */
181 acc
= __SMLALD(a1
, state_out
, acc
);
183 /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
184 /* Calc lower part of acc */
185 acc_l
= acc
& 0xffffffff;
187 /* Calc upper part of acc */
188 acc_h
= (acc
>> 32) & 0xffffffff;
190 /* Apply shift for lower part of acc and upper part of acc */
191 out
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
193 out
= __SSAT(out
, 16);
195 /* Store the output in the destination buffer. */
197 #ifndef ARM_MATH_BIG_ENDIAN
199 *__SIMD32(pOut
)++ = __PKHBT(state_out
, out
, 16);
203 *__SIMD32(pOut
)++ = __PKHBT(out
, state_out
>> 16, 16);
205 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
207 /* Every time after the output is computed state should be updated. */
208 /* The states should be updated as: */
213 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
214 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
215 #ifndef ARM_MATH_BIG_ENDIAN
217 state_in
= __PKHBT(in
>> 16, state_in
, 16);
218 state_out
= __PKHBT(out
, state_out
, 16);
222 state_in
= __PKHBT(state_in
>> 16, in
, 16);
223 state_out
= __PKHBT(state_out
>> 16, out
, 16);
225 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
228 /* Decrement the loop counter */
233 /* If the blockSize is not a multiple of 2, compute any remaining output samples here.
234 ** No loop unrolling is used. */
236 if((blockSize
& 0x1u
) != 0u)
241 /* out = b0 * x[n] + 0 * 0 */
243 #ifndef ARM_MATH_BIG_ENDIAN
245 out
= __SMUAD(b0
, in
);
249 out
= __SMUADX(b0
, in
);
251 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
253 /* acc = b1 * x[n-1] + b2 * x[n-2] + out */
254 acc
= __SMLALD(b1
, state_in
, out
);
255 /* acc += a1 * y[n-1] + a2 * y[n-2] */
256 acc
= __SMLALD(a1
, state_out
, acc
);
258 /* The result is converted from 3.29 to 1.31 if postShift = 1, and then saturation is applied */
259 /* Calc lower part of acc */
260 acc_l
= acc
& 0xffffffff;
262 /* Calc upper part of acc */
263 acc_h
= (acc
>> 32) & 0xffffffff;
265 /* Apply shift for lower part of acc and upper part of acc */
266 out
= (uint32_t) acc_l
>> lShift
| acc_h
<< uShift
;
268 out
= __SSAT(out
, 16);
270 /* Store the output in the destination buffer. */
271 *pOut
++ = (q15_t
) out
;
273 /* Every time after the output is computed state should be updated. */
274 /* The states should be updated as: */
279 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
280 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
282 #ifndef ARM_MATH_BIG_ENDIAN
284 state_in
= __PKHBT(in
, state_in
, 16);
285 state_out
= __PKHBT(out
, state_out
, 16);
289 state_in
= __PKHBT(state_in
>> 16, in
, 16);
290 state_out
= __PKHBT(state_out
>> 16, out
, 16);
292 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
296 /* The first stage goes from the input wire to the output wire. */
297 /* Subsequent numStages occur in-place in the output wire */
300 /* Reset the output pointer */
303 /* Store the updated state variables back into the state array */
304 *__SIMD32(pState
)++ = state_in
;
305 *__SIMD32(pState
)++ = state_out
;
308 /* Decrement the loop counter */
315 /* Run the below code for Cortex-M0 */
317 q15_t
*pIn
= pSrc
; /* Source pointer */
318 q15_t
*pOut
= pDst
; /* Destination pointer */
319 q15_t b0
, b1
, b2
, a1
, a2
; /* Filter coefficients */
320 q15_t Xn1
, Xn2
, Yn1
, Yn2
; /* Filter state variables */
321 q15_t Xn
; /* temporary input */
322 q63_t acc
; /* Accumulator */
323 int32_t shift
= (15 - (int32_t) S
->postShift
); /* Post shift */
324 q15_t
*pState
= S
->pState
; /* State pointer */
325 q15_t
*pCoeffs
= S
->pCoeffs
; /* Coefficient pointer */
326 uint32_t sample
, stage
= (uint32_t) S
->numStages
; /* Stage loop counter */
330 /* Reading the coefficients */
332 pCoeffs
++; // skip the 0 coefficient
338 /* Reading the state values */
344 /* The variables acc holds the output value that is computed:
345 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
355 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
356 /* acc = b0 * x[n] */
357 acc
= (q31_t
) b0
*Xn
;
359 /* acc += b1 * x[n-1] */
360 acc
+= (q31_t
) b1
*Xn1
;
361 /* acc += b[2] * x[n-2] */
362 acc
+= (q31_t
) b2
*Xn2
;
363 /* acc += a1 * y[n-1] */
364 acc
+= (q31_t
) a1
*Yn1
;
365 /* acc += a2 * y[n-2] */
366 acc
+= (q31_t
) a2
*Yn2
;
368 /* The result is converted to 1.31 */
369 acc
= __SSAT((acc
>> shift
), 16);
371 /* Every time after the output is computed state should be updated. */
372 /* The states should be updated as: */
382 /* Store the output in the destination buffer. */
383 *pOut
++ = (q15_t
) acc
;
385 /* decrement the loop counter */
389 /* The first stage goes from the input buffer to the output buffer. */
390 /* Subsequent stages occur in-place in the output buffer */
393 /* Reset to destination pointer */
396 /* Store the updated state variables back into the pState array */
404 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
410 * @} end of BiquadCascadeDF1 group