arm_iir_lattice_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_iir_lattice_q31.c 00009 * 00010 * Description: Q31 IIR lattice filter processing function. 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 00053 void arm_iir_lattice_q31( 00054 const arm_iir_lattice_instance_q31 * S, 00055 q31_t * pSrc, 00056 q31_t * pDst, 00057 uint32_t blockSize) 00058 { 00059 00060 q63_t acc; /* Accumlator */ 00061 q31_t k1, k2, v; 00062 q31_t fnext1, gcurr1 = 0, gnext; /* Temporary variables for lattice stages */ 00063 uint32_t blkCnt, tapCnt; /* Temporary variables for counts */ 00064 q31_t *px1, *px2, *pk, *pv; /* Temporary pointers for state and coef */ 00065 uint32_t numStages = S->numStages; /* number of stages */ 00066 q31_t *pState; /* State pointer */ 00067 q31_t *pStateCurnt; /* State current pointer */ 00068 q31_t gcurr2, fnext2; 00069 00070 00071 00072 /* initialise loop count */ 00073 blkCnt = blockSize; 00074 00075 /* initialise state pointer */ 00076 pState = &S->pState[0]; 00077 00078 /* Sample processing */ 00079 while(blkCnt > 0u) 00080 { 00081 /* Read Sample from input buffer */ 00082 /* fN(n) = x(n) */ 00083 fnext2 = *pSrc++; 00084 00085 /* Initialize state read pointer */ 00086 px1 = pState; 00087 /* Initialize state write pointer */ 00088 px2 = pState; 00089 /* Set accumulator to zero */ 00090 acc = 0; 00091 00092 /* Initialize Ladder coeff pointer */ 00093 pv = &S->pvCoeffs[0]; 00094 /* Initialize Reflection coeff pointer */ 00095 pk = &S->pkCoeffs[0]; 00096 00097 00098 /* Loop unrolling. Process 4 taps at a time. */ 00099 tapCnt = (numStages) >> 2; 00100 00101 while(tapCnt > 0u) 00102 { 00103 /* Read gN-1(n-1) from state buffer */ 00104 gcurr1 = *px1; 00105 00106 /* read reflection coefficient kN */ 00107 k1 = *pk; 00108 00109 /* fN-1(n) = fN(n) - kN * gN-1(n-1) */ 00110 fnext1 = (q31_t)(((q63_t) gcurr1 * (k1)) >> 32); 00111 00112 #ifdef CCS 00113 00114 fnext1 = __SDSUB(fnext2, fnext1); 00115 00116 #elif __GNUC__ 00117 00118 fnext1 = __QDSUB(fnext2, fnext1); 00119 00120 #else 00121 00122 fnext1 = __QSUB(fnext2, __QDBL(fnext1)); 00123 00124 #endif 00125 /* read next reflection coefficient kN-1 */ 00126 k2 = *(pk + 1u); 00127 00128 /* Read gN-2(n-1) from state buffer */ 00129 gcurr2 = *(px1 + 1u); 00130 00131 /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */ 00132 fnext2 = (q31_t)(((q63_t) gcurr2 * (k2)) >> 32); 00133 00134 /* gN(n) = kN * fN-1(n) + gN-1(n-1) */ 00135 gnext = (q31_t)(((q63_t) fnext1 * (k1)) >> 32); 00136 00137 #ifdef CCS 00138 00139 fnext2 = __SDSUB(fnext1, fnext2); 00140 gnext = __SDADD(gcurr1, gnext); 00141 00142 #elif __GNUC__ 00143 00144 fnext2 = __QDSUB(fnext1, fnext2); 00145 gnext = __QDADD(gcurr1, gnext); 00146 00147 #else 00148 00149 fnext2 = __QSUB(fnext1, __QDBL(fnext2)); 00150 gnext = __QADD(gcurr1, __QDBL(gnext)); 00151 00152 #endif 00153 00154 /* read reflection coefficient kN-2 */ 00155 k1 = *(pk + 2u); 00156 00157 /* write gN-1(n) into state for next sample processing */ 00158 *px2++ = gnext; 00159 00160 /* read ladder coefficient vN */ 00161 v = *pv++; 00162 00163 /* Read gN-3(n-1) from state buffer */ 00164 gcurr1 = *(px1 + 2u); 00165 00166 /* y(n) += gN(n) * vN */ 00167 acc += ((q63_t) gnext * v); 00168 00169 /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */ 00170 fnext1 = (q31_t)(((q63_t) gcurr1 * (k1)) >> 32); 00171 00172 /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */ 00173 00174 gnext = (q31_t)(((q63_t) fnext2 * (k2)) >> 32); 00175 00176 #ifdef CCS 00177 00178 fnext1 = __SDSUB(fnext2, fnext1); 00179 gnext = __SDADD(gcurr2, gnext); 00180 00181 #elif __GNUC__ 00182 00183 fnext1 = __QDSUB(fnext2, fnext1); 00184 gnext = __QDADD(gcurr2, gnext); 00185 00186 #else 00187 00188 fnext1 = __QSUB(fnext2, __QDBL(fnext1)); 00189 gnext = __QADD(gcurr2, __QDBL(gnext)); 00190 00191 #endif 00192 00193 /* read next ladder coefficient vN-1*/ 00194 v = *pv++; 00195 00196 /* Read gN-4(n-1) from state buffer */ 00197 gcurr2 = *(px1 + 3u); 00198 00199 /* y(n) += gN-1(n) * vN-1 */ 00200 acc += ((q63_t) gnext * v); 00201 00202 /* read reflection coefficient kN-3 */ 00203 k2 = *(pk + 3u); 00204 00205 /* write gN-1(n) into state for next sample processing */ 00206 *px2++ = gnext; 00207 00208 00209 /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */ 00210 fnext2 = (q31_t)(((q63_t) gcurr2 * (k2)) >> 32); 00211 00212 /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */ 00213 gnext = (q31_t)(((q63_t) fnext1 * (k1)) >> 32); 00214 00215 #ifdef CCS 00216 00217 fnext2 = __SDSUB(fnext1, fnext2); 00218 gnext = __SDADD(gcurr1, gnext); 00219 00220 #elif __GNUC__ 00221 00222 fnext2 = __QDSUB(fnext1, fnext2); 00223 gnext = __QDADD(gcurr1, gnext); 00224 00225 #else 00226 00227 fnext2 = __QSUB(fnext1, __QDBL(fnext2)); 00228 gnext = __QADD(gcurr1, __QDBL(gnext)); 00229 00230 #endif 00231 00232 /* read ladder coefficient vN-2*/ 00233 v = *pv++; 00234 00235 /* y(n) += gN-2(n) * vN-2 */ 00236 acc += ((q63_t) gnext * v); 00237 00238 /* write gN-2(n) into state for next sample processing */ 00239 *px2++ = gnext; 00240 00241 /* update pointer */ 00242 pk += 4u; 00243 00244 /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */ 00245 gnext = (q31_t)(((q63_t) fnext2 * (k2)) >> 32); 00246 00247 #ifdef CCS 00248 00249 gnext = __SDADD(gcurr2, gnext); 00250 00251 #elif __GNUC__ 00252 00253 gnext = __QDADD(gcurr2, gnext); 00254 00255 #else 00256 00257 gnext = __QADD(gcurr2, __QDBL(gnext)); 00258 00259 #endif 00260 00261 /* read next ladder coefficient vN-3*/ 00262 v = *pv++; 00263 00264 /* y(n) += gN-4(n) * vN-4 */ 00265 acc += ((q63_t) gnext * v); 00266 00267 /* write gN-3(n) into state for next sample processing */ 00268 *px2++ = gnext; 00269 00270 /* update pointers */ 00271 px1 += 4u; 00272 00273 00274 tapCnt--; 00275 00276 } 00277 00278 00279 00280 /* If the filter length is not a multiple of 4, compute the remaining filter taps */ 00281 tapCnt = (numStages) % 0x4u; 00282 00283 00284 00285 while(tapCnt > 0u) 00286 { 00287 gcurr1 = *px1++; 00288 /* Process sample for last taps */ 00289 fnext1 = __QSUB(fnext2, (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31)); 00290 gnext = __QADD(gcurr1, (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31)); 00291 /* Output samples for last taps */ 00292 acc += ((q63_t) gnext * *pv++); 00293 *px2++ = gnext; 00294 00295 fnext2 = fnext1; 00296 00297 tapCnt--; 00298 00299 } 00300 00301 /* y(n) += g0(n) * v0 */ 00302 acc += (q63_t) fnext2 * (*pv); 00303 00304 *px2++ = fnext2; 00305 00306 /* write out into pDst */ 00307 *pDst++ = (q31_t) (acc >> 31u); 00308 00309 /* Advance the state pointer by 4 to process the next group of 4 samples */ 00310 pState = pState + 1u; 00311 blkCnt--; 00312 00313 } 00314 00315 /* Processing is complete. Now copy last S->numStages samples to start of the buffer 00316 for the preperation of next frame process */ 00317 00318 /* Points to the start of the state buffer */ 00319 pStateCurnt = &S->pState[0]; 00320 pState = &S->pState[blockSize]; 00321 00322 tapCnt = numStages >> 2u; 00323 00324 /* copy data */ 00325 while(tapCnt > 0u) 00326 { 00327 *pStateCurnt++ = *pState++; 00328 *pStateCurnt++ = *pState++; 00329 *pStateCurnt++ = *pState++; 00330 *pStateCurnt++ = *pState++; 00331 00332 /* Decrement the loop counter */ 00333 tapCnt--; 00334 00335 } 00336 00337 /* Calculate remaining number of copies */ 00338 tapCnt = (numStages) % 0x4u; 00339 00340 /* Copy the remaining q31_t data */ 00341 while(tapCnt > 0u) 00342 { 00343 *pStateCurnt++ = *pState++; 00344 00345 /* Decrement the loop counter */ 00346 tapCnt--; 00347 } 00348 00349 } 00350 00351 00352 00353
