1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
8 * Title: arm_iir_lattice_f32.c
10 * Description: Floating-point IIR Lattice filter processing function.
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
17 * - Redistributions of source code must retain the above copyright
18 * notice, this list of conditions and the following disclaimer.
19 * - Redistributions in binary form must reproduce the above copyright
20 * notice, this list of conditions and the following disclaimer in
21 * the documentation and/or other materials provided with the
23 * - Neither the name of ARM LIMITED nor the names of its contributors
24 * may be used to endorse or promote products derived from this
25 * software without specific prior written permission.
27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
38 * POSSIBILITY OF SUCH DAMAGE.
39 * -------------------------------------------------------------------- */
44 * @ingroup groupFilters
48 * @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters
50 * This set of functions implements lattice filters
51 * for Q15, Q31 and floating-point data types. Lattice filters are used in a
52 * variety of adaptive filter applications. The filter structure has feedforward and
53 * feedback components and the net impulse response is infinite length.
54 * The functions operate on blocks
55 * of input and output data and each call to the function processes
56 * <code>blockSize</code> samples through the filter. <code>pSrc</code> and
57 * <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
60 * \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"
63 * fm-1(n) = fm(n) - km * gm-1(n-1) for m = N, N-1, ...1
64 * gm(n) = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ...1
65 * y(n) = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)
68 * <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.
69 * Reflection coefficients are stored in time-reversed order.
74 * <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.
75 * Ladder coefficients are stored in time-reversed order.
80 * <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.
81 * The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.
82 * The state variables are updated after each block of data is processed; the coefficients are untouched.
83 * \par Instance Structure
84 * The coefficients and state variables for a filter are stored together in an instance data structure.
85 * A separate instance structure must be defined for each filter.
86 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
87 * There are separate instance structure declarations for each of the 3 supported data types.
89 * \par Initialization Functions
90 * There is also an associated initialization function for each data type.
91 * The initialization function performs the following operations:
92 * - Sets the values of the internal structure fields.
93 * - Zeros out the values in the state buffer.
94 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
95 * numStages, pkCoeffs, pvCoeffs, pState. Also set all of the values in pState to zero.
98 * Use of the initialization function is optional.
99 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
100 * To place an instance structure into a const data section, the instance structure must be manually initialized.
101 * Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
103 *arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};
104 *arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};
105 *arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};
108 * where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;
109 * <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.
110 * \par Fixed-Point Behavior
111 * Care must be taken when using the fixed-point versions of the IIR lattice filter functions.
112 * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
113 * Refer to the function specific documentation below for usage guidelines.
117 * @addtogroup IIR_Lattice
122 * @brief Processing function for the floating-point IIR lattice filter.
123 * @param[in] *S points to an instance of the floating-point IIR lattice structure.
124 * @param[in] *pSrc points to the block of input data.
125 * @param[out] *pDst points to the block of output data.
126 * @param[in] blockSize number of samples to process.
130 #ifndef ARM_MATH_CM0_FAMILY
132 /* Run the below code for Cortex-M4 and Cortex-M3 */
134 void arm_iir_lattice_f32(
135 const arm_iir_lattice_instance_f32
* S
,
140 float32_t fnext1
, gcurr1
, gnext
; /* Temporary variables for lattice stages */
141 float32_t acc
; /* Accumlator */
142 uint32_t blkCnt
, tapCnt
; /* temporary variables for counts */
143 float32_t
*px1
, *px2
, *pk
, *pv
; /* temporary pointers for state and coef */
144 uint32_t numStages
= S
->numStages
; /* number of stages */
145 float32_t
*pState
; /* State pointer */
146 float32_t
*pStateCurnt
; /* State current pointer */
148 float32_t v1
, v2
, v3
, v4
;
152 /* initialise loop count */
155 /* initialise state pointer */
156 pState
= &S
->pState
[0];
158 /* Sample processing */
161 /* Read Sample from input buffer */
165 /* Initialize Ladder coeff pointer */
166 pv
= &S
->pvCoeffs
[0];
167 /* Initialize Reflection coeff pointer */
168 pk
= &S
->pkCoeffs
[0];
170 /* Initialize state read pointer */
172 /* Initialize state write pointer */
175 /* Set accumulator to zero */
178 /* Loop unrolling. Process 4 taps at a time. */
179 tapCnt
= (numStages
) >> 2;
183 /* Read gN-1(n-1) from state buffer */
186 /* read reflection coefficient kN */
189 /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
190 fnext1
= fnext2
- (k1
* gcurr1
);
192 /* read ladder coefficient vN */
195 /* read next reflection coefficient kN-1 */
198 /* Read gN-2(n-1) from state buffer */
199 gcurr2
= *(px1
+ 1u);
201 /* read next ladder coefficient vN-1 */
204 /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
205 fnext2
= fnext1
- (k2
* gcurr2
);
207 /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
208 gnext
= gcurr1
+ (k1
* fnext1
);
210 /* read reflection coefficient kN-2 */
213 /* write gN(n) into state for next sample processing */
216 /* Read gN-3(n-1) from state buffer */
217 gcurr1
= *(px1
+ 2u);
219 /* y(n) += gN(n) * vN */
222 /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
223 fnext1
= fnext2
- (k1
* gcurr1
);
225 /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
226 gnext
= gcurr2
+ (k2
* fnext2
);
228 /* Read gN-4(n-1) from state buffer */
229 gcurr2
= *(px1
+ 3u);
231 /* y(n) += gN-1(n) * vN-1 */
234 /* read reflection coefficient kN-3 */
237 /* write gN-1(n) into state for next sample processing */
240 /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
241 fnext2
= fnext1
- (k2
* gcurr2
);
243 /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
244 gnext
= gcurr1
+ (k1
* fnext1
);
246 /* read ladder coefficient vN-2 */
249 /* y(n) += gN-2(n) * vN-2 */
252 /* write gN-2(n) into state for next sample processing */
258 /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
259 gnext
= (fnext2
* k2
) + gcurr2
;
261 /* read next ladder coefficient vN-3 */
264 /* y(n) += gN-4(n) * vN-4 */
267 /* write gN-3(n) into state for next sample processing */
270 /* update pointers */
278 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
279 tapCnt
= (numStages
) % 0x4u
;
284 /* Process sample for last taps */
285 fnext1
= fnext2
- ((*pk
) * gcurr1
);
286 gnext
= (fnext1
* (*pk
++)) + gcurr1
;
287 /* Output samples for last taps */
288 acc
+= (gnext
* (*pv
++));
296 /* y(n) += g0(n) * v0 */
297 acc
+= (fnext2
* (*pv
));
301 /* write out into pDst */
304 /* Advance the state pointer by 4 to process the next group of 4 samples */
305 pState
= pState
+ 1u;
311 /* Processing is complete. Now copy last S->numStages samples to start of the buffer
312 for the preperation of next frame process */
314 /* Points to the start of the state buffer */
315 pStateCurnt
= &S
->pState
[0];
316 pState
= &S
->pState
[blockSize
];
318 tapCnt
= numStages
>> 2u;
323 *pStateCurnt
++ = *pState
++;
324 *pStateCurnt
++ = *pState
++;
325 *pStateCurnt
++ = *pState
++;
326 *pStateCurnt
++ = *pState
++;
328 /* Decrement the loop counter */
333 /* Calculate remaining number of copies */
334 tapCnt
= (numStages
) % 0x4u
;
336 /* Copy the remaining q31_t data */
339 *pStateCurnt
++ = *pState
++;
341 /* Decrement the loop counter */
348 void arm_iir_lattice_f32(
349 const arm_iir_lattice_instance_f32
* S
,
354 float32_t fcurr
, fnext
= 0, gcurr
, gnext
; /* Temporary variables for lattice stages */
355 float32_t acc
; /* Accumlator */
356 uint32_t blkCnt
, tapCnt
; /* temporary variables for counts */
357 float32_t
*px1
, *px2
, *pk
, *pv
; /* temporary pointers for state and coef */
358 uint32_t numStages
= S
->numStages
; /* number of stages */
359 float32_t
*pState
; /* State pointer */
360 float32_t
*pStateCurnt
; /* State current pointer */
363 /* Run the below code for Cortex-M0 */
367 pState
= &S
->pState
[0];
369 /* Sample processing */
372 /* Read Sample from input buffer */
376 /* Initialize state read pointer */
378 /* Initialize state write pointer */
380 /* Set accumulator to zero */
382 /* Initialize Ladder coeff pointer */
383 pv
= &S
->pvCoeffs
[0];
384 /* Initialize Reflection coeff pointer */
385 pk
= &S
->pkCoeffs
[0];
388 /* Process sample for numStages */
394 /* Process sample for last taps */
395 fnext
= fcurr
- ((*pk
) * gcurr
);
396 gnext
= (fnext
* (*pk
++)) + gcurr
;
398 /* Output samples for last taps */
399 acc
+= (gnext
* (*pv
++));
403 /* Decrementing loop counter */
408 /* y(n) += g0(n) * v0 */
409 acc
+= (fnext
* (*pv
));
413 /* write out into pDst */
416 /* Advance the state pointer by 1 to process the next group of samples */
417 pState
= pState
+ 1u;
422 /* Processing is complete. Now copy last S->numStages samples to start of the buffer
423 for the preperation of next frame process */
425 /* Points to the start of the state buffer */
426 pStateCurnt
= &S
->pState
[0];
427 pState
= &S
->pState
[blockSize
];
434 *pStateCurnt
++ = *pState
++;
436 /* Decrement the loop counter */
442 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
446 * @} end of IIR_Lattice group