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 @return none
119 */
120
arm_iir_lattice_f32(const arm_iir_lattice_instance_f32 * S,const float32_t * pSrc,float32_t * pDst,uint32_t blockSize)121 void arm_iir_lattice_f32(
122 const arm_iir_lattice_instance_f32 * S,
123 const float32_t * pSrc,
124 float32_t * pDst,
125 uint32_t blockSize)
126 {
127 float32_t *pState = S->pState; /* State pointer */
128 float32_t *pStateCur; /* State current pointer */
129 float32_t acc; /* Accumlator */
130 float32_t fnext1, fnext2, gcurr1, gnext; /* Temporary variables for lattice stages */
131 float32_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */
132 uint32_t numStages = S->numStages; /* Number of stages */
133 uint32_t blkCnt, tapCnt; /* Temporary variables for counts */
134
135 #if defined (ARM_MATH_LOOPUNROLL)
136 float32_t gcurr2; /* Temporary variables for lattice stages */
137 float32_t k1, k2;
138 float32_t v1, v2, v3, v4;
139 #endif
140
141 /* initialise loop count */
142 blkCnt = blockSize;
143
144 /* Sample processing */
145 while (blkCnt > 0U)
146 {
147 /* Read Sample from input buffer */
148 /* fN(n) = x(n) */
149 fnext2 = *pSrc++;
150
151 /* Initialize Ladder coeff pointer */
152 pv = &S->pvCoeffs[0];
153
154 /* Initialize Reflection coeff pointer */
155 pk = &S->pkCoeffs[0];
156
157 /* Initialize state read pointer */
158 px1 = pState;
159
160 /* Initialize state write pointer */
161 px2 = pState;
162
163 /* Set accumulator to zero */
164 acc = 0.0;
165
166 #if defined (ARM_MATH_LOOPUNROLL)
167
168 /* Loop unrolling: Compute 4 taps at a time. */
169 tapCnt = (numStages) >> 2U;
170
171 while (tapCnt > 0U)
172 {
173 /* Read gN-1(n-1) from state buffer */
174 gcurr1 = *px1;
175
176 /* read reflection coefficient kN */
177 k1 = *pk;
178
179 /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
180 fnext1 = fnext2 - (k1 * gcurr1);
181
182 /* read ladder coefficient vN */
183 v1 = *pv;
184
185 /* read next reflection coefficient kN-1 */
186 k2 = *(pk + 1U);
187
188 /* Read gN-2(n-1) from state buffer */
189 gcurr2 = *(px1 + 1U);
190
191 /* read next ladder coefficient vN-1 */
192 v2 = *(pv + 1U);
193
194 /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
195 fnext2 = fnext1 - (k2 * gcurr2);
196
197 /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
198 gnext = gcurr1 + (k1 * fnext1);
199
200 /* read reflection coefficient kN-2 */
201 k1 = *(pk + 2U);
202
203 /* write gN(n) into state for next sample processing */
204 *px2++ = gnext;
205
206 /* Read gN-3(n-1) from state buffer */
207 gcurr1 = *(px1 + 2U);
208
209 /* y(n) += gN(n) * vN */
210 acc += (gnext * v1);
211
212 /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
213 fnext1 = fnext2 - (k1 * gcurr1);
214
215 /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
216 gnext = gcurr2 + (k2 * fnext2);
217
218 /* Read gN-4(n-1) from state buffer */
219 gcurr2 = *(px1 + 3U);
220
221 /* y(n) += gN-1(n) * vN-1 */
222 acc += (gnext * v2);
223
224 /* read reflection coefficient kN-3 */
225 k2 = *(pk + 3U);
226
227 /* write gN-1(n) into state for next sample processing */
228 *px2++ = gnext;
229
230 /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
231 fnext2 = fnext1 - (k2 * gcurr2);
232
233 /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
234 gnext = gcurr1 + (k1 * fnext1);
235
236 /* read ladder coefficient vN-2 */
237 v3 = *(pv + 2U);
238
239 /* y(n) += gN-2(n) * vN-2 */
240 acc += (gnext * v3);
241
242 /* write gN-2(n) into state for next sample processing */
243 *px2++ = gnext;
244
245 /* update pointer */
246 pk += 4U;
247
248 /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
249 gnext = (fnext2 * k2) + gcurr2;
250
251 /* read next ladder coefficient vN-3 */
252 v4 = *(pv + 3U);
253
254 /* y(n) += gN-4(n) * vN-4 */
255 acc += (gnext * v4);
256
257 /* write gN-3(n) into state for next sample processing */
258 *px2++ = gnext;
259
260 /* update pointers */
261 px1 += 4U;
262 pv += 4U;
263
264 /* Decrement loop counter */
265 tapCnt--;
266 }
267
268 /* Loop unrolling: Compute remaining taps */
269 tapCnt = numStages % 0x4U;
270
271 #else
272
273 /* Initialize tapCnt with number of samples */
274 tapCnt = numStages;
275
276 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
277
278 while (tapCnt > 0U)
279 {
280 gcurr1 = *px1++;
281 /* Process sample for last taps */
282 fnext1 = fnext2 - ((*pk) * gcurr1);
283 gnext = (fnext1 * (*pk++)) + gcurr1;
284 /* Output samples for last taps */
285 acc += (gnext * (*pv++));
286 *px2++ = gnext;
287 fnext2 = fnext1;
288
289 /* Decrement loop counter */
290 tapCnt--;
291 }
292
293 /* y(n) += g0(n) * v0 */
294 acc += (fnext2 * (*pv));
295
296 *px2++ = fnext2;
297
298 /* write out into pDst */
299 *pDst++ = acc;
300
301 /* Advance the state pointer by 4 to process the next group of 4 samples */
302 pState = pState + 1U;
303
304 /* Decrement loop counter */
305 blkCnt--;
306 }
307
308 /* Processing is complete. Now copy last S->numStages samples to start of the buffer
309 for the preperation of next frame process */
310
311 /* Points to the start of the state buffer */
312 pStateCur = &S->pState[0];
313 pState = &S->pState[blockSize];
314
315 /* Copy data */
316 #if defined (ARM_MATH_LOOPUNROLL)
317
318 /* Loop unrolling: Compute 4 taps at a time. */
319 tapCnt = numStages >> 2U;
320
321 while (tapCnt > 0U)
322 {
323 *pStateCur++ = *pState++;
324 *pStateCur++ = *pState++;
325 *pStateCur++ = *pState++;
326 *pStateCur++ = *pState++;
327
328 /* Decrement loop counter */
329 tapCnt--;
330 }
331
332 /* Loop unrolling: Compute remaining taps */
333 tapCnt = numStages % 0x4U;
334
335 #else
336
337 /* Initialize blkCnt with number of samples */
338 tapCnt = numStages;
339
340 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
341
342 while (tapCnt > 0U)
343 {
344 *pStateCur++ = *pState++;
345
346 /* Decrement loop counter */
347 tapCnt--;
348 }
349
350 }
351
352 /**
353 @} end of IIR_Lattice group
354 */
355
