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