arm_fir_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_fir_lattice_q15.c 00009 * 00010 * Description: Q15 FIR 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 00045 void arm_fir_lattice_q15( 00046 const arm_fir_lattice_instance_q15 * S, 00047 q15_t * pSrc, 00048 q15_t * pDst, 00049 uint32_t blockSize) 00050 { 00051 q15_t *pState; /* State pointer */ 00052 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00053 q15_t *px; /* temporary state pointer */ 00054 q15_t *pk; /* temporary coefficient pointer */ 00055 q31_t fcurr1, fnext1, gcurr1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */ 00056 q31_t fcurr2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */ 00057 uint32_t numStages = S->numStages; /* Number of stages in the filter */ 00058 uint32_t blkCnt, stageCnt; /* temporary variables for counts */ 00059 00060 q15_t k; 00061 00062 pState = &S->pState[0]; 00063 00064 /* First part of the processing with loop unrolling. Compute 2 outputs at a time. 00065 a second loop below computes the remaining 1 sample. */ 00066 00067 blkCnt = blockSize >> 1u; 00068 00069 while(blkCnt > 0u) 00070 { 00071 /* f0(n) = x(n) */ 00072 fcurr1 = *pSrc++; 00073 00074 /* f0(n) = x(n) */ 00075 fcurr2 = *pSrc++; 00076 00077 /* Initialize coeff pointer */ 00078 pk = (pCoeffs); 00079 00080 /* Initialize state pointer */ 00081 px = pState; 00082 00083 /* read g0(n - 1) from state buffer */ 00084 gcurr1 = *px; 00085 00086 k = *pk++; 00087 00088 /* for sample 1 processing */ 00089 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00090 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 16); 00091 fnext1 = fcurr1 + (fnext1 << 1u); 00092 00093 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00094 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 16); 00095 gnext1 = gcurr1 + (gnext1 << 1u); 00096 00097 #ifndef CCS 00098 fnext1 = __SSAT(fnext1, 16); 00099 #else 00100 fnext1 = __SSATA(fnext1, 0, 16); 00101 #endif 00102 00103 /* for sample 1 processing */ 00104 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00105 fnext2 = (q31_t)(((q63_t) fcurr1 * k) >> 16); 00106 fnext2 = fcurr2 + (fnext2 << 1u); 00107 00108 #ifndef CCS 00109 gnext1 = __SSAT(gnext1, 16); 00110 #else 00111 gnext1 = __SSATA(gnext1, 0, 16); 00112 #endif 00113 00114 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00115 gnext2 = (q31_t)(((q63_t) fcurr2 * (k)) >> 16); 00116 gnext2 = fcurr1 + (gnext2 << 1u); 00117 00118 #ifndef CCS 00119 fnext2 = __SSAT(fnext2, 16); 00120 gnext2 = __SSAT(gnext2, 16); 00121 #else 00122 fnext2 = __SSATA(fnext2, 0, 16); 00123 gnext2 = __SSATA(gnext2, 0, 16); 00124 #endif 00125 00126 00127 /* save g1(n) in state buffer */ 00128 *px++ = fcurr2; 00129 00130 /* f1(n) is saved in fcurr1 00131 for next stage processing */ 00132 fcurr1 = fnext1; 00133 fcurr2 = fnext2; 00134 00135 stageCnt = (numStages - 1u); 00136 00137 /* stage loop */ 00138 while(stageCnt > 0u) 00139 { 00140 00141 k = *pk++; 00142 00143 /* read g2(n) from state buffer */ 00144 gcurr1 = *px; 00145 00146 /* save g1(n) in state buffer */ 00147 *px++ = gnext2; 00148 00149 /* Sample processing for K2, K3.... */ 00150 /* f2(n) = f1(n) + K2 * g1(n-1) */ 00151 00152 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 16); 00153 fnext2 = (q31_t)(((q63_t) gnext1 * k) >> 16); 00154 gnext2 = (q31_t)(((q63_t) fcurr2 * (k)) >> 16); 00155 00156 fnext1 = fcurr1 + (fnext1 << 1u); 00157 fnext2 = fcurr2 + (fnext2 << 1u); 00158 00159 #ifndef CCS 00160 fnext1 = __SSAT(fnext1, 16); 00161 fnext2 = __SSAT(fnext2, 16); 00162 #else 00163 fnext1 = __SSATA(fnext1, 0, 16); 00164 fnext2 = __SSATA(fnext2, 0, 16); 00165 #endif 00166 00167 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00168 00169 gnext2 = gnext1 + (gnext2 << 1u); 00170 00171 00172 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00173 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 16); 00174 gnext1 = gcurr1 + (gnext1 << 1u); 00175 00176 #ifndef CCS 00177 gnext2 = __SSAT(gnext2, 16); 00178 gnext1 = __SSAT(gnext1, 16); 00179 #else 00180 gnext2 = __SSATA(gnext2, 0, 16); 00181 gnext1 = __SSATA(gnext1, 0, 16); 00182 #endif 00183 00184 /* f1(n) is saved in fcurr1 00185 for next stage processing */ 00186 fcurr1 = fnext1; 00187 fcurr2 = fnext2; 00188 00189 stageCnt--; 00190 00191 } 00192 00193 /* y(n) = fN(n) */ 00194 *pDst++ = fcurr1; 00195 *pDst++ = fcurr2; 00196 00197 blkCnt--; 00198 00199 } 00200 00201 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00202 ** No loop unrolling is used. */ 00203 blkCnt = blockSize % 0x2u; 00204 00205 while(blkCnt > 0u) 00206 { 00207 /* f0(n) = x(n) */ 00208 fcurr1 = *pSrc++; 00209 00210 /* Initialize coeff pointer */ 00211 pk = (pCoeffs); 00212 00213 /* Initialize state pointer */ 00214 px = pState; 00215 00216 /* read g0(n - 1) from state buffer */ 00217 gcurr1 = *px; 00218 00219 k = *pk++; 00220 00221 /* for sample 1 processing */ 00222 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00223 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 16); 00224 fnext1 = fcurr1 + (fnext1 << 1u); 00225 00226 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00227 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 16); 00228 gnext1 = gcurr1 + (gnext1 << 1u); 00229 00230 #ifndef CCS 00231 fnext1 = __SSAT(fnext1, 16); 00232 gnext1 = __SSAT(gnext1, 16); 00233 #else 00234 fnext1 = __SSATA(fnext1, 0, 16); 00235 gnext1 = __SSATA(gnext1, 0, 16); 00236 #endif 00237 00238 00239 /* save g1(n) in state buffer */ 00240 *px++ = fcurr1; 00241 00242 /* f1(n) is saved in fcurr1 00243 for next stage processing */ 00244 fcurr1 = fnext1; 00245 00246 stageCnt = (numStages - 1u); 00247 00248 /* stage loop */ 00249 while(stageCnt > 0u) 00250 { 00251 00252 k = *pk++; 00253 00254 /* read g2(n) from state buffer */ 00255 gcurr1 = *px; 00256 00257 /* save g1(n) in state buffer */ 00258 *px++ = gnext1; 00259 00260 /* Sample processing for K2, K3.... */ 00261 /* f2(n) = f1(n) + K2 * g1(n-1) */ 00262 00263 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 16); 00264 fnext1 = fcurr1 + (fnext1 << 1u); 00265 00266 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00267 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 16); 00268 gnext1 = gcurr1 + (gnext1 << 1u); 00269 00270 #ifndef CCS 00271 fnext1 = __SSAT(fnext1, 16); 00272 #else 00273 fnext1 = __SSATA(fnext1, 0, 16); 00274 #endif 00275 00276 00277 #ifndef CCS 00278 gnext1 = __SSAT(gnext1, 16); 00279 #else 00280 gnext1 = __SSATA(gnext1, 0, 16); 00281 #endif 00282 00283 /* f1(n) is saved in fcurr1 00284 for next stage processing */ 00285 fcurr1 = fnext1; 00286 00287 stageCnt--; 00288 00289 } 00290 00291 00292 /* y(n) = fN(n) */ 00293 *pDst++ = fcurr1; 00294 00295 blkCnt--; 00296 00297 } 00298 00299 00300 00301 } 00302
