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