1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_iir_lattice_f32.c
4 * Description: Floating-point IIR Lattice filter processing function
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
13 *
14 * SPDX-License-Identifier: Apache-2.0
15 *
16 * Licensed under the Apache License, Version 2.0 (the License); you may
17 * not use this file except in compliance with the License.
18 * You may obtain a copy of the License at
19 *
20 * www.apache.org/licenses/LICENSE-2.0
21 *
22 * Unless required by applicable law or agreed to in writing, software
23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
25 * See the License for the specific language governing permissions and
26 * limitations under the License.
27 */
28
29 #include "dsp/filtering_functions.h"
30
31 /**
32 @ingroup groupFilters
33 */
34
35 /**
36 @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters
37
38 This set of functions implements lattice filters
39 for Q15, Q31 and floating-point data types. Lattice filters are used in a
40 variety of adaptive filter applications. The filter structure has feedforward and
41 feedback components and the net impulse response is infinite length.
42 The functions operate on blocks
43 of input and output data and each call to the function processes
44 <code>blockSize</code> samples through the filter. <code>pSrc</code> and
45 <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
46
47 @par Algorithm
48 \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"
49 @par
50 <pre>
51 fN(n) = x(n)
52 fm-1(n) = fm(n) - km * gm-1(n-1) for m = N, N-1, ..., 1
53 gm(n) = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ..., 1
54 y(n) = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)
55 </pre>
56 @par
57 <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.
58 Reflection Coefficients are stored in time-reversed order.
59 @par
60 <pre>
61 {kN, kN-1, ..., k1}
62 </pre>
63 @par
64 <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.
65 Ladder coefficients are stored in time-reversed order.
66 <pre>
67 {vN, vN-1, ..., v0}
68 </pre>
69 @par
70 <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.
71 The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.
72 The state variables are updated after each block of data is processed; the coefficients are untouched.
73
74 @par Instance Structure
75 The coefficients and state variables for a filter are stored together in an instance data structure.
76 A separate instance structure must be defined for each filter.
77 Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
78 There are separate instance structure declarations for each of the 3 supported data types.
79
80 @par Initialization Functions
81 There is also an associated initialization function for each data type.
82 The initialization function performs the following operations:
83 - Sets the values of the internal structure fields.
84 - Zeros out the values in the state buffer.
85 To do this manually without calling the init function, assign the follow subfields of the instance structure:
86 numStages, pkCoeffs, pvCoeffs, pState. Also set all of the values in pState to zero.
87 @par
88 Use of the initialization function is optional.
89 However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
90 To place an instance structure into a const data section, the instance structure must be manually initialized.
91 Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
92 <pre>
93 arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};
94 arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};
95 arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};
96 </pre>
97 @par
98 where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;
99 <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.
100
101 @par Fixed-Point Behavior
102 Care must be taken when using the fixed-point versions of the IIR lattice filter functions.
103 In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
104 Refer to the function specific documentation below for usage guidelines.
105 */
106
107 /**
108 @addtogroup IIR_Lattice
109 @{
110 */
111
112 /**
113 @brief Processing function for the floating-point IIR lattice filter.
114 @param[in] S points to an instance of the floating-point IIR lattice structure
115 @param[in] pSrc points to the block of input data
116 @param[out] pDst points to the block of output data
117 @param[in] blockSize number of samples to process
118 */
119
arm_iir_lattice_f32(const arm_iir_lattice_instance_f32 * S,const float32_t * pSrc,float32_t * pDst,uint32_t blockSize)120 void arm_iir_lattice_f32(
121 const arm_iir_lattice_instance_f32 * S,
122 const float32_t * pSrc,
123 float32_t * pDst,
124 uint32_t blockSize)
125 {
126 float32_t *pState = S->pState; /* State pointer */
127 float32_t *pStateCur; /* State current pointer */
128 float32_t acc; /* Accumlator */
129 float32_t fnext1, fnext2, gcurr1, gnext; /* Temporary variables for lattice stages */
130 float32_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
131 uint32_t numStages = S->numStages; /* Number of stages */
132 uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
133
134 #if defined (ARM_MATH_LOOPUNROLL)
135 float32_t gcurr2; /* Temporary variables for lattice stages */
136 float32_t k1, k2;
137 float32_t v1, v2, v3, v4;
138 #endif
139
140 /* initialise loop count */
141 blkCnt = blockSize;
142
143 /* Sample processing */
144 while (blkCnt > 0U)
145 {
146 /* Read Sample from input buffer */
147 /* fN(n) = x(n) */
148 fnext2 = *pSrc++;
149
150 /* Initialize Ladder coeff pointer */
151 pv = &S->pvCoeffs[0];
152
153 /* Initialize Reflection coeff pointer */
154 pk = &S->pkCoeffs[0];
155
156 /* Initialize state read pointer */
157 px1 = pState;
158
159 /* Initialize state write pointer */
160 px2 = pState;
161
162 /* Set accumulator to zero */
163 acc = 0.0;
164
165 #if defined (ARM_MATH_LOOPUNROLL)
166
167 /* Loop unrolling: Compute 4 taps at a time. */
168 tapCnt = (numStages) >> 2U;
169
170 while (tapCnt > 0U)
171 {
172 /* Read gN-1(n-1) from state buffer */
173 gcurr1 = *px1;
174
175 /* read reflection coefficient kN */
176 k1 = *pk;
177
178 /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
179 fnext1 = fnext2 - (k1 * gcurr1);
180
181 /* read ladder coefficient vN */
182 v1 = *pv;
183
184 /* read next reflection coefficient kN-1 */
185 k2 = *(pk + 1U);
186
187 /* Read gN-2(n-1) from state buffer */
188 gcurr2 = *(px1 + 1U);
189
190 /* read next ladder coefficient vN-1 */
191 v2 = *(pv + 1U);
192
193 /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
194 fnext2 = fnext1 - (k2 * gcurr2);
195
196 /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
197 gnext = gcurr1 + (k1 * fnext1);
198
199 /* read reflection coefficient kN-2 */
200 k1 = *(pk + 2U);
201
202 /* write gN(n) into state for next sample processing */
203 *px2++ = gnext;
204
205 /* Read gN-3(n-1) from state buffer */
206 gcurr1 = *(px1 + 2U);
207
208 /* y(n) += gN(n) * vN */
209 acc += (gnext * v1);
210
211 /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
212 fnext1 = fnext2 - (k1 * gcurr1);
213
214 /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
215 gnext = gcurr2 + (k2 * fnext2);
216
217 /* Read gN-4(n-1) from state buffer */
218 gcurr2 = *(px1 + 3U);
219
220 /* y(n) += gN-1(n) * vN-1 */
221 acc += (gnext * v2);
222
223 /* read reflection coefficient kN-3 */
224 k2 = *(pk + 3U);
225
226 /* write gN-1(n) into state for next sample processing */
227 *px2++ = gnext;
228
229 /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
230 fnext2 = fnext1 - (k2 * gcurr2);
231
232 /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
233 gnext = gcurr1 + (k1 * fnext1);
234
235 /* read ladder coefficient vN-2 */
236 v3 = *(pv + 2U);
237
238 /* y(n) += gN-2(n) * vN-2 */
239 acc += (gnext * v3);
240
241 /* write gN-2(n) into state for next sample processing */
242 *px2++ = gnext;
243
244 /* update pointer */
245 pk += 4U;
246
247 /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
248 gnext = (fnext2 * k2) + gcurr2;
249
250 /* read next ladder coefficient vN-3 */
251 v4 = *(pv + 3U);
252
253 /* y(n) += gN-4(n) * vN-4 */
254 acc += (gnext * v4);
255
256 /* write gN-3(n) into state for next sample processing */
257 *px2++ = gnext;
258
259 /* update pointers */
260 px1 += 4U;
261 pv += 4U;
262
263 /* Decrement loop counter */
264 tapCnt--;
265 }
266
267 /* Loop unrolling: Compute remaining taps */
268 tapCnt = numStages % 0x4U;
269
270 #else
271
272 /* Initialize tapCnt with number of samples */
273 tapCnt = numStages;
274
275 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
276
277 while (tapCnt > 0U)
278 {
279 gcurr1 = *px1++;
280 /* Process sample for last taps */
281 fnext1 = fnext2 - ((*pk) * gcurr1);
282 gnext = (fnext1 * (*pk++)) + gcurr1;
283 /* Output samples for last taps */
284 acc += (gnext * (*pv++));
285 *px2++ = gnext;
286 fnext2 = fnext1;
287
288 /* Decrement loop counter */
289 tapCnt--;
290 }
291
292 /* y(n) += g0(n) * v0 */
293 acc += (fnext2 * (*pv));
294
295 *px2++ = fnext2;
296
297 /* write out into pDst */
298 *pDst++ = acc;
299
300 /* Advance the state pointer by 4 to process the next group of 4 samples */
301 pState = pState + 1U;
302
303 /* Decrement loop counter */
304 blkCnt--;
305 }
306
307 /* Processing is complete. Now copy last S->numStages samples to start of the buffer
308 for the preperation of next frame process */
309
310 /* Points to the start of the state buffer */
311 pStateCur = &S->pState[0];
312 pState = &S->pState[blockSize];
313
314 /* Copy data */
315 #if defined (ARM_MATH_LOOPUNROLL)
316
317 /* Loop unrolling: Compute 4 taps at a time. */
318 tapCnt = numStages >> 2U;
319
320 while (tapCnt > 0U)
321 {
322 *pStateCur++ = *pState++;
323 *pStateCur++ = *pState++;
324 *pStateCur++ = *pState++;
325 *pStateCur++ = *pState++;
326
327 /* Decrement loop counter */
328 tapCnt--;
329 }
330
331 /* Loop unrolling: Compute remaining taps */
332 tapCnt = numStages % 0x4U;
333
334 #else
335
336 /* Initialize blkCnt with number of samples */
337 tapCnt = numStages;
338
339 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
340
341 while (tapCnt > 0U)
342 {
343 *pStateCur++ = *pState++;
344
345 /* Decrement loop counter */
346 tapCnt--;
347 }
348
349 }
350
351 /**
352 @} end of IIR_Lattice group
353 */
354
