1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_lms_norm_q31.c
4 * Description: Processing function for the Q31 NLMS filter
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 @addtogroup LMS_NORM
37 @{
38 */
39
40 /**
41 @brief Processing function for Q31 normalized LMS filter.
42 @param[in] S points to an instance of the Q31 normalized LMS filter structure
43 @param[in] pSrc points to the block of input data
44 @param[in] pRef points to the block of reference data
45 @param[out] pOut points to the block of output data
46 @param[out] pErr points to the block of error data
47 @param[in] blockSize number of samples to process
48
49 @par Scaling and Overflow Behavior
50 The function is implemented using an internal 64-bit accumulator.
51 The accumulator has a 2.62 format and maintains full precision of the intermediate
52 multiplication results but provides only a single guard bit.
53 Thus, if the accumulator result overflows it wraps around rather than clip.
54 In order to avoid overflows completely the input signal must be scaled down by
55 log2(numTaps) bits. The reference signal should not be scaled down.
56 After all multiply-accumulates are performed, the 2.62 accumulator is shifted
57 and saturated to 1.31 format to yield the final result.
58 The output signal and error signal are in 1.31 format.
59 @par
60 In this filter, filter coefficients are updated for each sample and the
61 updation of filter cofficients are saturted.
62 */
63
arm_lms_norm_q31(arm_lms_norm_instance_q31 * S,const q31_t * pSrc,q31_t * pRef,q31_t * pOut,q31_t * pErr,uint32_t blockSize)64 ARM_DSP_ATTRIBUTE void arm_lms_norm_q31(
65 arm_lms_norm_instance_q31 * S,
66 const q31_t * pSrc,
67 q31_t * pRef,
68 q31_t * pOut,
69 q31_t * pErr,
70 uint32_t blockSize)
71 {
72 q31_t *pState = S->pState; /* State pointer */
73 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
74 q31_t *pStateCurnt; /* Points to the current sample of the state */
75 q31_t *px, *pb; /* Temporary pointers for state and coefficient buffers */
76 q31_t mu = S->mu; /* Adaptive factor */
77 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
78 uint32_t tapCnt, blkCnt; /* Loop counters */
79 q63_t acc; /* Accumulator */
80 q63_t energy; /* Energy of the input */
81 q31_t e = 0; /* Error data sample */
82 q31_t w = 0, in; /* Weight factor and state */
83 q31_t x0; /* Temporary variable to hold input sample */
84 q31_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
85 q31_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
86 q31_t coef; /* Temporary variable for coef */
87 q31_t acc_l, acc_h; /* Temporary input */
88 uint32_t uShift = ((uint32_t) S->postShift + 1U);
89 uint32_t lShift = 32U - uShift; /* Shift to be applied to the output */
90
91 energy = S->energy;
92 x0 = S->x0;
93
94 /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
95 /* pStateCurnt points to the location where the new input data should be written */
96 pStateCurnt = &(S->pState[(numTaps - 1U)]);
97
98 /* initialise loop count */
99 blkCnt = blockSize;
100
101 while (blkCnt > 0U)
102 {
103 /* Copy the new input sample into the state buffer */
104 *pStateCurnt++ = *pSrc;
105
106 /* Initialize pState pointer */
107 px = pState;
108
109 /* Initialize coefficient pointer */
110 pb = pCoeffs;
111
112 /* Read the sample from input buffer */
113 in = *pSrc++;
114
115 /* Update the energy calculation */
116 energy = (q31_t) ((((q63_t) energy << 32) - (((q63_t) x0 * x0) << 1)) >> 32);
117 energy = ((((q63_t) in * in) << 1) + (energy << 32)) >> 32;
118 energy = clip_q63_to_q31(energy);
119
120 /* Set the accumulator to zero */
121 acc = 0;
122
123 #if defined (ARM_MATH_LOOPUNROLL)
124
125 /* Loop unrolling: Compute 4 taps at a time. */
126 tapCnt = numTaps >> 2U;
127
128 while (tapCnt > 0U)
129 {
130 /* Perform the multiply-accumulate */
131 /* acc += b[N] * x[n-N] */
132 acc += ((q63_t) (*px++)) * (*pb++);
133
134 /* acc += b[N-1] * x[n-N-1] */
135 acc += ((q63_t) (*px++)) * (*pb++);
136
137 /* acc += b[N-2] * x[n-N-2] */
138 acc += ((q63_t) (*px++)) * (*pb++);
139
140 /* acc += b[N-3] * x[n-N-3] */
141 acc += ((q63_t) (*px++)) * (*pb++);
142
143 /* Decrement loop counter */
144 tapCnt--;
145 }
146
147 /* Loop unrolling: Compute remaining taps */
148 tapCnt = numTaps % 0x4U;
149
150 #else
151
152 /* Initialize tapCnt with number of samples */
153 tapCnt = numTaps;
154
155 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
156
157 while (tapCnt > 0U)
158 {
159 /* Perform the multiply-accumulate */
160 acc += ((q63_t) (*px++)) * (*pb++);
161
162 /* Decrement the loop counter */
163 tapCnt--;
164 }
165
166 /* Converting the result to 1.31 format */
167 /* Calc lower part of acc */
168 acc_l = acc & 0xffffffff;
169
170 /* Calc upper part of acc */
171 acc_h = (acc >> 32) & 0xffffffff;
172
173 acc = (uint32_t) acc_l >> lShift | acc_h << uShift;
174
175 /* Store the result from accumulator into the destination buffer. */
176 *pOut++ = (q31_t) acc;
177
178 /* Compute and store error */
179 e = *pRef++ - (q31_t) acc;
180 *pErr++ = e;
181
182 /* Calculates the reciprocal of energy */
183 postShift = arm_recip_q31(energy + DELTA_Q31, &oneByEnergy, &S->recipTable[0]);
184
185 /* Calculation of product of (e * mu) */
186 errorXmu = (q31_t) (((q63_t) e * mu) >> 31);
187
188 /* Weighting factor for the normalized version */
189 w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift));
190
191 /* Initialize pState pointer */
192 px = pState;
193
194 /* Initialize coefficient pointer */
195 pb = pCoeffs;
196
197 #if defined (ARM_MATH_LOOPUNROLL)
198
199 /* Loop unrolling: Compute 4 taps at a time. */
200 tapCnt = numTaps >> 2U;
201
202 /* Update filter coefficients */
203 while (tapCnt > 0U)
204 {
205 /* Perform the multiply-accumulate */
206
207 /* coef is in 2.30 format */
208 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
209 /* get coef in 1.31 format by left shifting */
210 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
211 /* update coefficient buffer to next coefficient */
212 pb++;
213
214 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
215 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
216 pb++;
217
218 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
219 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
220 pb++;
221
222 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
223 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
224 pb++;
225
226 /* Decrement loop counter */
227 tapCnt--;
228 }
229
230 /* Loop unrolling: Compute remaining taps */
231 tapCnt = numTaps % 0x4U;
232
233 #else
234
235 /* Initialize tapCnt with number of samples */
236 tapCnt = numTaps;
237
238 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
239
240 while (tapCnt > 0U)
241 {
242 /* Perform the multiply-accumulate */
243 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
244 *pb = clip_q63_to_q31((q63_t) * pb + (coef << 1U));
245 pb++;
246
247 /* Decrement loop counter */
248 tapCnt--;
249 }
250
251 /* Read the sample from state buffer */
252 x0 = *pState;
253
254 /* Advance state pointer by 1 for the next sample */
255 pState = pState + 1;
256
257 /* Decrement loop counter */
258 blkCnt--;
259 }
260
261 /* Save energy and x0 values for the next frame */
262 S->energy = (q31_t) energy;
263 S->x0 = x0;
264
265 /* Processing is complete.
266 Now copy the last numTaps - 1 samples to the start of the state buffer.
267 This prepares the state buffer for the next function call. */
268
269 /* Points to the start of the pState buffer */
270 pStateCurnt = S->pState;
271
272 /* copy data */
273 #if defined (ARM_MATH_LOOPUNROLL)
274
275 /* Loop unrolling: Compute 4 taps at a time. */
276 tapCnt = (numTaps - 1U) >> 2U;
277
278 while (tapCnt > 0U)
279 {
280 *pStateCurnt++ = *pState++;
281 *pStateCurnt++ = *pState++;
282 *pStateCurnt++ = *pState++;
283 *pStateCurnt++ = *pState++;
284
285 /* Decrement loop counter */
286 tapCnt--;
287 }
288
289 /* Loop unrolling: Compute remaining taps */
290 tapCnt = (numTaps - 1U) % 0x4U;
291
292 #else
293
294 /* Initialize tapCnt with number of samples */
295 tapCnt = (numTaps - 1U);
296
297 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
298
299 while (tapCnt > 0U)
300 {
301 *pStateCurnt++ = *pState++;
302
303 /* Decrement loop counter */
304 tapCnt--;
305 }
306
307 }
308
309 /**
310 @} end of LMS_NORM group
311 */
312
