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