arm_fir_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_fir_lattice_q31.c 00009 * 00010 * Description: Q31 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 00049 void arm_fir_lattice_q31( 00050 const arm_fir_lattice_instance_q31 * S, 00051 q31_t * pSrc, 00052 q31_t * pDst, 00053 uint32_t blockSize) 00054 { 00055 q31_t *pState; /* State pointer */ 00056 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00057 q31_t *px; /* temporary state pointer */ 00058 q31_t *pk; /* temporary coefficient pointer */ 00059 q31_t fcurr1, fnext1, gcurr1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */ 00060 q31_t fcurr2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */ 00061 uint32_t numStages = S->numStages; /* Length of the filter */ 00062 uint32_t blkCnt, stageCnt; /* temporary variables for counts */ 00063 q31_t k; 00064 00065 pState = &S->pState[0]; 00066 00067 blkCnt = blockSize >> 1u; 00068 00069 /* First part of the processing with loop unrolling. Compute 2 outputs at a time. 00070 a second loop below computes the remaining 1 sample. */ 00071 while(blkCnt > 0u) 00072 { 00073 /* f0(n) = x(n) */ 00074 fcurr1 = *pSrc++; 00075 00076 /* f0(n) = x(n) */ 00077 fcurr2 = *pSrc++; 00078 00079 /* Initialize coeff pointer */ 00080 pk = (pCoeffs); 00081 00082 /* Initialize state pointer */ 00083 px = pState; 00084 00085 /* read g0(n - 1) from state buffer */ 00086 gcurr1 = *px; 00087 00088 /* Read the reflection coefficient */ 00089 k = *pk++; 00090 00091 /* for sample 1 processing */ 00092 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00093 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 32); 00094 00095 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00096 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 32); 00097 fnext1 = fcurr1 + (fnext1 << 1u); 00098 gnext1 = gcurr1 + (gnext1 << 1u); 00099 00100 /* for sample 1 processing */ 00101 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00102 fnext2 = (q31_t)(((q63_t) fcurr1 * k) >> 32); 00103 00104 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00105 gnext2 = (q31_t)(((q63_t) fcurr2 * (k)) >> 32); 00106 fnext2 = fcurr2 + (fnext2 << 1u); 00107 gnext2 = fcurr1 + (gnext2 << 1u); 00108 00109 /* save g1(n) in state buffer */ 00110 *px++ = fcurr2; 00111 00112 /* f1(n) is saved in fcurr1 00113 for next stage processing */ 00114 fcurr1 = fnext1; 00115 fcurr2 = fnext2; 00116 00117 stageCnt = (numStages - 1u); 00118 00119 /* stage loop */ 00120 while(stageCnt > 0u) 00121 { 00122 00123 /* Read the reflection coefficient */ 00124 k = *pk++; 00125 00126 /* read g2(n) from state buffer */ 00127 gcurr1 = *px; 00128 00129 /* save g1(n) in state buffer */ 00130 *px++ = gnext2; 00131 00132 /* Sample processing for K2, K3.... */ 00133 /* f2(n) = f1(n) + K2 * g1(n-1) */ 00134 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 32); 00135 fnext2 = (q31_t)(((q63_t) gnext1 * k) >> 32); 00136 00137 fnext1 = fcurr1 + (fnext1 << 1u); 00138 fnext2 = fcurr2 + (fnext2 << 1u); 00139 00140 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00141 gnext2 = (q31_t)(((q63_t) fcurr2 * (k)) >> 32); 00142 gnext2 = gnext1 + (gnext2 << 1u); 00143 00144 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00145 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 32); 00146 gnext1 = gcurr1 + (gnext1 << 1u); 00147 00148 /* f1(n) is saved in fcurr1 00149 for next stage processing */ 00150 fcurr1 = fnext1; 00151 fcurr2 = fnext2; 00152 00153 stageCnt--; 00154 00155 } 00156 00157 /* y(n) = fN(n) */ 00158 *pDst++ = fcurr1; 00159 *pDst++ = fcurr2; 00160 00161 blkCnt--; 00162 00163 } 00164 00165 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00166 ** No loop unrolling is used. */ 00167 blkCnt = blockSize % 0x2u; 00168 00169 while(blkCnt > 0u) 00170 { 00171 /* f0(n) = x(n) */ 00172 fcurr1 = *pSrc++; 00173 00174 /* Initialize coeff pointer */ 00175 pk = (pCoeffs); 00176 00177 /* Initialize state pointer */ 00178 px = pState; 00179 00180 /* read g0(n - 1) from state buffer */ 00181 gcurr1 = *px; 00182 00183 /* Read the reflection coefficient */ 00184 k = *pk++; 00185 00186 /* for sample 1 processing */ 00187 /* f1(n) = f0(n) + K1 * g0(n-1) */ 00188 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 32); 00189 fnext1 = fcurr1 + (fnext1 << 1u); 00190 00191 /* g1(n) = f0(n) * K1 + g0(n-1) */ 00192 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 32); 00193 gnext1 = gcurr1 + (gnext1 << 1u); 00194 00195 /* save g1(n) in state buffer */ 00196 *px++ = fcurr1; 00197 00198 /* f1(n) is saved in fcurr1 00199 for next stage processing */ 00200 fcurr1 = fnext1; 00201 00202 stageCnt = (numStages - 1u); 00203 00204 /* stage loop */ 00205 while(stageCnt > 0u) 00206 { 00207 /* Read the reflection coefficient */ 00208 k = *pk++; 00209 00210 /* read g2(n) from state buffer */ 00211 gcurr1 = *px; 00212 00213 /* save g1(n) in state buffer */ 00214 *px++ = gnext1; 00215 00216 /* Sample processing for K2, K3.... */ 00217 /* f2(n) = f1(n) + K2 * g1(n-1) */ 00218 fnext1 = (q31_t)(((q63_t) gcurr1 * k) >> 32); 00219 fnext1 = fcurr1 + (fnext1 << 1u); 00220 00221 /* g2(n) = f1(n) * K2 + g1(n-1) */ 00222 gnext1 = (q31_t)(((q63_t) fcurr1 * (k)) >> 32); 00223 gnext1 = gcurr1 + (gnext1 << 1u); 00224 00225 /* f1(n) is saved in fcurr1 00226 for next stage processing */ 00227 fcurr1 = fnext1; 00228 00229 stageCnt--; 00230 00231 } 00232 00233 00234 /* y(n) = fN(n) */ 00235 *pDst++ = fcurr1; 00236 00237 blkCnt--; 00238 00239 } 00240 00241 00242 } 00243
