arm_lms_norm_q31.c
Go to the documentation of this file.
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2011 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 15. December 2011 00005 * $Revision: V2.0.0 00006 * 00007 * Project: Cortex-R DSP Library 00008 * Title: arm_lms_norm_q31.c 00009 * 00010 * Description: Processing function for the Q31 NLMS filter. 00011 * 00012 * Target Processor: Cortex-R4/R5 00013 * 00014 * Version 1.0.0 2011/03/08 00015 * Alpha release. 00016 * 00017 * Version 1.0.1 2011/09/30 00018 * Beta release. 00019 * 00020 * Version 2.0.0 2011/12/15 00021 * Final release. 00022 * 00023 * -------------------------------------------------------------------- */ 00024 #include "arm_math.h" 00025 00060 void arm_lms_norm_q31( 00061 arm_lms_norm_instance_q31 * S, 00062 q31_t * pSrc, 00063 q31_t * pRef, 00064 q31_t * pOut, 00065 q31_t * pErr, 00066 uint32_t blockSize) 00067 { 00068 q31_t *pState = S->pState; /* State pointer */ 00069 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00070 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00071 q31_t *px, *pb; /* Temporary pointers for state and coefficient buffers */ 00072 q31_t mu = S->mu; /* Adaptive factor */ 00073 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */ 00074 uint32_t tapCnt, blkCnt; /* Loop counters */ 00075 q63_t energy; /* Energy of the input */ 00076 q63_t acc; /* Accumulator */ 00077 q31_t e = 0, d = 0; /* error, reference data sample */ 00078 q31_t w = 0, in; /* weight factor and state */ 00079 q31_t x0; /* temporary variable to hold input sample */ 00080 // uint32_t shift = 32u - ((uint32_t) S->postShift + 1u); /* Shift to be applied to the output */ 00081 q31_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */ 00082 q31_t postShift; /* Post shift to be applied to weight after reciprocal calculation */ 00083 q31_t coef; /* Temporary variable for coef */ 00084 q31_t x1, x2; 00085 q31_t c0, c1; 00086 q31_t coef0, coef1; 00087 q31_t acc_l, acc_h; /* temporary input */ 00088 uint32_t uShift = ((uint32_t) S->postShift + 1u); 00089 uint32_t lShift = 32u - uShift; /* Shift to be applied to the output */ 00090 00091 00092 energy = S->energy; 00093 x0 = S->x0; 00094 00095 /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */ 00096 /* pStateCurnt points to the location where the new input data should be written */ 00097 pStateCurnt = &(S->pState[(numTaps - 1u)]); 00098 00099 /* Initializing blkCnt with blockSize */ 00100 blkCnt = blockSize; 00101 00102 while(blkCnt > 0u) 00103 { 00104 00105 /* Copy the new input sample into the state buffer */ 00106 *pStateCurnt++ = *pSrc; 00107 00108 /* Initialize pState pointer */ 00109 px = pState; 00110 00111 /* Initialize coeff pointer */ 00112 pb = (pCoeffs); 00113 00114 /* Read the sample from input buffer */ 00115 in = *pSrc++; 00116 00117 /* Update the energy calculation */ 00118 energy = (q31_t) ((((q63_t) energy << 32) - 00119 (((q63_t) x0 * x0) << 1)) >> 32); 00120 energy = (q31_t) (((((q63_t) in * in) << 1) + (energy << 32)) >> 32); 00121 00122 /* Set the accumulator to zero */ 00123 acc = 0; 00124 00125 /* Loop unrolling. Process 4 taps at a time. */ 00126 tapCnt = numTaps >> 2; 00127 00128 while(tapCnt > 0u) 00129 { 00130 00131 /* Read two values from state and coefficient buffers */ 00132 x1 = *px; 00133 c0 = *pb++; 00134 x2 = *(px + 1u); 00135 c1 = *pb++; 00136 00137 /* Perform the multiply-accumulate */ 00138 /* acc += b[N] * x[n-N] */ 00139 acc += (q63_t)x1 * c0; 00140 /* acc += b[N-1] * x[n-N-1] */ 00141 acc += (q63_t)x2 * c1; 00142 00143 /* Read next two values from state and coefficient buffers */ 00144 x1 = *(px + 2u); 00145 c0 = *pb++; 00146 x2 = *(px + 3u); 00147 c1 = *pb++; 00148 00149 /* update state pointer */ 00150 px += 4u; 00151 00152 /* Perform the multiply-accumulate */ 00153 /* acc += b[N-2] * x[n-N-2] */ 00154 acc += (q63_t)x1 * c0; 00155 /* acc += b[N-3] * x[n-N-3] */ 00156 acc += (q63_t)x2 * c1; 00157 00158 /* Decrement the loop counter */ 00159 tapCnt--; 00160 } 00161 00162 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00163 tapCnt = numTaps % 0x4u; 00164 00165 while(tapCnt > 0u) 00166 { 00167 /* Perform the multiply-accumulate */ 00168 acc += ((q63_t) (*px++)) * (*pb++); 00169 00170 /* Decrement the loop counter */ 00171 tapCnt--; 00172 } 00173 00174 /* Converting the result to 1.31 format */ 00175 /* Calc lower part of acc */ 00176 acc_l = acc & 0xffffffff; 00177 00178 /* Calc upper part of acc */ 00179 acc_h = (acc >> 32) & 0xffffffff; 00180 00181 acc = (uint32_t)acc_l >> lShift | acc_h << uShift; 00182 00183 /* Store the result from accumulator into the destination buffer. */ 00184 *pOut++ = (q31_t) acc; 00185 00186 /* Compute and store error */ 00187 d = *pRef++; 00188 e = d - (q31_t) acc; 00189 *pErr++ = e; 00190 00191 /* Calculates the reciprocal of energy */ 00192 postShift = arm_recip_q31(energy + DELTA_Q31, 00193 &oneByEnergy, &S->recipTable[0]); 00194 00195 /* Calculation of product of (e * mu) */ 00196 errorXmu = (q31_t) (((q63_t) e * mu) >> 31); 00197 00198 /* Weighting factor for the normalized version */ 00199 w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift)); 00200 00201 /* Initialize pState pointer */ 00202 px = pState; 00203 00204 /* Initialize coeff pointer */ 00205 pb = (pCoeffs); 00206 00207 /* Loop unrolling. Process 4 taps at a time. */ 00208 tapCnt = numTaps >> 2; 00209 00210 /* Update filter coefficients */ 00211 while(tapCnt > 0u) 00212 { 00213 /* Read two values from state and coefficient buffers */ 00214 x1 = *px; 00215 x2 = *(px + 1u); 00216 c0 = *pb; 00217 c1 = *(pb + 1u); 00218 00219 /* update coefficient value and store in coeff buffer */ 00220 coef0 = (q31_t) (((q63_t) w * x1) >> (32)); 00221 coef1 = (q31_t) (((q63_t) w * x2) >> (32)); 00222 c0 = clip_q63_to_q31((q63_t)c0 + (coef0 << 1u)); 00223 c1 = clip_q63_to_q31((q63_t)c1 + (coef1 << 1u)); 00224 00225 /* update coefficient value and store in coeff buffer */ 00226 *pb = c0; 00227 *(pb + 1u) = c1; 00228 00229 /* Read two values from state and coefficient buffers */ 00230 x1 = *(px + 2u); 00231 x2 = *(px + 3u); 00232 c0 = *(pb + 2u); 00233 c1 = *(pb + 3u); 00234 00235 /* update coefficient value and store in coeff buffer */ 00236 /* coef0 is in 2.30 format */ 00237 coef0 = (q31_t) (((q63_t) w * x1) >> (32)); 00238 coef1 = (q31_t) (((q63_t) w * x2) >> (32)); 00239 /* get coef in 1.31 format by left shifting */ 00240 c0 = clip_q63_to_q31((q63_t)c0 + (coef0 << 1u)); 00241 c1 = clip_q63_to_q31((q63_t)c1 + (coef1 << 1u)); 00242 00243 /* update coefficient value and store in coeff buffer */ 00244 *(pb + 2u) = c0; 00245 *(pb + 3u) = c1; 00246 00247 /* update pointers */ 00248 px += 4u; 00249 pb += 4u; 00250 00251 /* Decrement the loop counter */ 00252 tapCnt--; 00253 } 00254 00255 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00256 tapCnt = numTaps % 0x4u; 00257 00258 while(tapCnt > 0u) 00259 { 00260 /* Perform the multiply-accumulate */ 00261 coef = (q31_t) (((q63_t) w * (*px++)) >> (32)); 00262 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u)); 00263 pb++; 00264 00265 /* Decrement the loop counter */ 00266 tapCnt--; 00267 } 00268 00269 /* Read the sample from state buffer */ 00270 x0 = *pState; 00271 00272 /* Advance state pointer by 1 for the next sample */ 00273 pState = pState + 1; 00274 00275 /* Decrement the loop counter */ 00276 blkCnt--; 00277 } 00278 00279 /* Save energy and x0 values for the next frame */ 00280 S->energy = (q31_t) energy; 00281 S->x0 = x0; 00282 00283 /* Processing is complete. Now copy the last numTaps - 1 samples to the 00284 satrt of the state buffer. This prepares the state buffer for the 00285 next function call. */ 00286 00287 /* Points to the start of the pState buffer */ 00288 pStateCurnt = S->pState; 00289 00290 /* Loop unrolling for (numTaps - 1u) samples copy */ 00291 tapCnt = (numTaps - 1u) >> 2u; 00292 00293 /* copy data */ 00294 while(tapCnt > 0u) 00295 { 00296 *pStateCurnt++ = *pState++; 00297 *pStateCurnt++ = *pState++; 00298 *pStateCurnt++ = *pState++; 00299 *pStateCurnt++ = *pState++; 00300 00301 /* Decrement the loop counter */ 00302 tapCnt--; 00303 } 00304 00305 /* Calculate remaining number of copies */ 00306 tapCnt = (numTaps - 1u) % 0x4u; 00307 00308 /* Copy the remaining q31_t data */ 00309 while(tapCnt > 0u) 00310 { 00311 *pStateCurnt++ = *pState++; 00312 00313 /* Decrement the loop counter */ 00314 tapCnt--; 00315 } 00316 00317 } 00318
