arm_lms_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_q31.c 00009 * 00010 * Description: Processing function for the Q31 LMS 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" 00057 void arm_lms_q31( 00058 const arm_lms_instance_q31 * S, 00059 q31_t * pSrc, 00060 q31_t * pRef, 00061 q31_t * pOut, 00062 q31_t * pErr, 00063 uint32_t blockSize) 00064 { 00065 q31_t *pState = S->pState; /* State pointer */ 00066 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */ 00067 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00068 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00069 q31_t mu = S->mu; /* Adaptive factor */ 00070 q31_t *px; /* Temporary pointer for state */ 00071 q31_t *pb; /* Temporary pointer for coefficient buffer */ 00072 uint32_t tapCnt, blkCnt; /* Loop counters */ 00073 q63_t acc; /* Accumulator */ 00074 q31_t e = 0; /* error of data sample */ 00075 q31_t alpha; /* Intermediate constant for taps update */ 00076 q31_t coef; /* Temporary variable for coef */ 00077 q31_t x0, x1; 00078 q31_t c0, c1; 00079 q31_t coef0, coef1; 00080 q31_t acc_l, acc_h; /* temporary input */ 00081 uint32_t uShift = ((uint32_t) S->postShift + 1u); 00082 uint32_t lShift = 32u - uShift; /* Shift to be applied to the output */ 00083 00084 /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */ 00085 /* pStateCurnt points to the location where the new input data should be written */ 00086 pStateCurnt = &(S->pState[(numTaps - 1u)]); 00087 00088 /* Initializing blkCnt with blockSize */ 00089 blkCnt = blockSize; 00090 00091 while(blkCnt > 0u) 00092 { 00093 00094 /* Copy the new input sample into the state buffer */ 00095 *pStateCurnt++ = *pSrc++; 00096 00097 /* Initialize state pointer */ 00098 px = pState; 00099 00100 /* Initialize coefficient pointer */ 00101 pb = pCoeffs; 00102 00103 /* Set the accumulator to zero */ 00104 acc = 0; 00105 00106 /* Loop unrolling. Process 4 taps at a time. */ 00107 tapCnt = numTaps >> 2; 00108 00109 while(tapCnt > 0u) 00110 { 00111 00112 /* Read two values from state and coefficient buffers */ 00113 x0 = *px; 00114 c0 = *pb++; 00115 x1 = *(px + 1u); 00116 c1 = *pb++; 00117 00118 /* Perform the multiply-accumulate */ 00119 /* acc += b[N] * x[n-N] */ 00120 acc += (q63_t)x0 * c0; 00121 /* acc += b[N-1] * x[n-N-1] */ 00122 acc += (q63_t)x1 * c1; 00123 00124 /* Read next two values from state and coefficient buffers */ 00125 x0 = *(px + 2u); 00126 c0 = *pb++; 00127 x1 = *(px + 3u); 00128 c1 = *pb++; 00129 00130 /* update state pointer */ 00131 px += 4u; 00132 00133 /* Perform the multiply-accumulate */ 00134 /* acc += b[N-2] * x[n-N-2] */ 00135 acc += (q63_t)x0 * c0; 00136 /* acc += b[N-3] * x[n-N-3] */ 00137 acc += (q63_t)x1 * c1; 00138 00139 /* Decrement the loop counter */ 00140 tapCnt--; 00141 } 00142 00143 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00144 tapCnt = numTaps % 0x4u; 00145 00146 while(tapCnt > 0u) 00147 { 00148 /* Perform the multiply-accumulate */ 00149 acc += ((q63_t) (*px++)) * (*pb++); 00150 00151 /* Decrement the loop counter */ 00152 tapCnt--; 00153 } 00154 00155 /* Converting the result to 1.31 format */ 00156 /* Calc lower part of acc */ 00157 acc_l = acc & 0xffffffff; 00158 00159 /* Calc upper part of acc */ 00160 acc_h = (acc >> 32) & 0xffffffff; 00161 00162 acc = (uint32_t)acc_l >> lShift | acc_h << uShift; 00163 00164 /* Store the result from accumulator into the destination buffer. */ 00165 *pOut++ = (q31_t) acc; 00166 00167 /* Compute and store error */ 00168 e = *pRef++ - (q31_t) acc; 00169 00170 *pErr++ = (q31_t) e; 00171 00172 /* Compute alpha i.e. intermediate constant for taps update */ 00173 alpha = (q31_t) (((q63_t) e * mu) >> 31); 00174 00175 /* Initialize state pointer */ 00176 /* Advance state pointer by 1 for the next sample */ 00177 px = pState++; 00178 00179 /* Initialize coefficient pointer */ 00180 pb = pCoeffs; 00181 00182 /* Loop unrolling. Process 4 taps at a time. */ 00183 tapCnt = numTaps >> 2; 00184 00185 /* Update filter coefficients */ 00186 while(tapCnt > 0u) 00187 { 00188 /* Read two values from state and coefficient buffers */ 00189 x0 = *px; 00190 x1 = *(px + 1u); 00191 c0 = *pb; 00192 c1 = *(pb + 1u); 00193 00194 /* update coefficient value and store in coeff buffer */ 00195 coef0 = (q31_t) (((q63_t) alpha * x0) >> (32)); 00196 coef1 = (q31_t) (((q63_t) alpha * x1) >> (32)); 00197 00198 c0 = clip_q63_to_q31((q63_t)c0 + (coef0 << 1u)); 00199 c1 = clip_q63_to_q31((q63_t)c1 + (coef1 << 1u)); 00200 00201 /* update coefficient value and store in coeff buffer */ 00202 *pb = c0; 00203 *(pb + 1u) = c1; 00204 00205 /* Read two values from state and coefficient buffers */ 00206 x0 = *(px + 2u); 00207 x1 = *(px + 3u); 00208 c0 = *(pb + 2u); 00209 c1 = *(pb + 3u); 00210 00211 /* update coefficient value and store in coeff buffer */ 00212 /* coef0 is in 2.30 format */ 00213 coef0 = (q31_t) (((q63_t) alpha * x0) >> (32)); 00214 coef1 = (q31_t) (((q63_t) alpha * x1) >> (32)); 00215 00216 /* get coef in 1.31 format by left shifting */ 00217 c0 = clip_q63_to_q31((q63_t)c0 + (coef0 << 1u)); 00218 c1 = clip_q63_to_q31((q63_t)c1 + (coef1 << 1u)); 00219 00220 /* update coefficient value and store in coeff buffer */ 00221 *(pb + 2u) = c0; 00222 *(pb + 3u) = c1; 00223 00224 /* update pointers */ 00225 px += 4u; 00226 pb += 4u; 00227 00228 /* Decrement the loop counter */ 00229 tapCnt--; 00230 } 00231 00232 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00233 tapCnt = numTaps % 0x4u; 00234 00235 while(tapCnt > 0u) 00236 { 00237 /* Perform the multiply-accumulate */ 00238 coef = (q31_t) (((q63_t) alpha * (*px++)) >> (32)); 00239 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u)); 00240 pb++; 00241 00242 /* Decrement the loop counter */ 00243 tapCnt--; 00244 } 00245 00246 /* Decrement the loop counter */ 00247 blkCnt--; 00248 } 00249 00250 /* Processing is complete. Now copy the last numTaps - 1 samples to the 00251 satrt of the state buffer. This prepares the state buffer for the 00252 next function call. */ 00253 00254 /* Points to the start of the pState buffer */ 00255 pStateCurnt = S->pState; 00256 00257 /* Loop unrolling for (numTaps - 1u) samples copy */ 00258 tapCnt = (numTaps - 1u) >> 2u; 00259 00260 /* copy data */ 00261 while(tapCnt > 0u) 00262 { 00263 *pStateCurnt++ = *pState++; 00264 *pStateCurnt++ = *pState++; 00265 *pStateCurnt++ = *pState++; 00266 *pStateCurnt++ = *pState++; 00267 00268 /* Decrement the loop counter */ 00269 tapCnt--; 00270 } 00271 00272 /* Calculate remaining number of copies */ 00273 tapCnt = (numTaps - 1u) % 0x4u; 00274 00275 /* Copy the remaining q31_t data */ 00276 while(tapCnt > 0u) 00277 { 00278 *pStateCurnt++ = *pState++; 00279 00280 /* Decrement the loop counter */ 00281 tapCnt--; 00282 } 00283 00284 } 00285
