CMSIS DSP Software Library: arm_fir_lattice_q31.c Source File
Main Page
Modules
Data Structures
Files
Examples
File List
Globals
arm_fir_lattice_q31.c
Go to the documentation of this file.00001 /* ----------------------------------------------------------------------
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
00003 *
00004 * $Date: 29. November 2010
00005 * $Revision: V1.0.3
00006 *
00007 * Project: CMSIS DSP Library
00008 * Title: arm_fir_lattice_q31.c
00009 *
00010 * Description: Q31 FIR lattice filter processing function.
00011 *
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *
00014 * Version 1.0.3 2010/11/29
00015 * Re-organized the CMSIS folders and updated documentation.
00016 *
00017 * Version 1.0.2 2010/11/11
00018 * Documentation updated.
00019 *
00020 * Version 1.0.1 2010/10/05
00021 * Production release and review comments incorporated.
00022 *
00023 * Version 1.0.0 2010/09/20
00024 * Production release and review comments incorporated
00025 *
00026 * Version 0.0.7 2010/06/10
00027 * Misra-C changes done
00028 * -------------------------------------------------------------------- */
00029
00030 #include "arm_math.h"
00031
00055 void arm_fir_lattice_q31(
00056 const arm_fir_lattice_instance_q31 * S,
00057 q31_t * pSrc,
00058 q31_t * pDst,
00059 uint32_t blockSize)
00060 {
00061 q31_t *pState; /* State pointer */
00062 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
00063 q31_t *px; /* temporary state pointer */
00064 q31_t *pk; /* temporary coefficient pointer */
00065 q31_t fcurr1, fnext1, gcurr1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */
00066 q63_t fcurr2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */
00067 q63_t fcurr3, fnext3, gnext3; /* temporary variables for third sample in loop unrolling */
00068 q63_t fcurr4, fnext4, gnext4; /* temporary variables for fourth sample in loop unrolling */
00069 uint32_t numStages = S->numStages; /* Length of the filter */
00070 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
00071
00072 pState = &S->pState[0];
00073
00074 blkCnt = blockSize >> 2u;
00075
00076 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
00077 ** a second loop below computes the remaining 1 to 3 samples. */
00078 while(blkCnt > 0u)
00079 {
00080
00081 /* Read two samples from input buffer */
00082 /* f0(n) = x(n) */
00083 fcurr1 = *pSrc++;
00084 /* f0(n) = x(n) */
00085 fcurr2 = *pSrc++;
00086
00087 /* Initialize coeff pointer */
00088 pk = (pCoeffs);
00089
00090 /* Initialize state pointer */
00091 px = pState;
00092
00093 /* Read g0(n-1) from state */
00094 gcurr1 = *px;
00095
00096 /* Process first sample for first tap */
00097 /* f1(n) = f0(n) + K1 * g0(n-1) */
00098 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00099 /* g1(n) = f0(n) * K1 + g0(n-1) */
00100 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + gcurr1;
00101
00102 /* Process second sample for first tap */
00103 /* for sample 2 processing */
00104 fnext2 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + fcurr2;
00105 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr1;
00106
00107
00108 /* Read next two samples from input buffer */
00109 /* f0(n+2) = x(n+2) */
00110 fcurr3 = *pSrc++;
00111 fcurr4 = *pSrc++;
00112
00113 /* Copy only last input samples into the state buffer
00114 which will be used for next four samples processing */
00115 *px++ = (q31_t) fcurr4;
00116
00117 /* Process third sample for first tap */
00118 fnext3 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr3;
00119 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr2;
00120
00121 /* Process fourth sample for first tap */
00122 fnext4 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr4;
00123 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk++)) >> 31) + fcurr3;
00124
00125 /* save g1(n) in state buffer for next sample processing */
00126 /* *px++ = gnext4; */
00127
00128 /* Update of f values for next coefficient set processing */
00129 fcurr1 = fnext1;
00130 fcurr2 = fnext2;
00131 fcurr3 = fnext3;
00132 fcurr4 = fnext4;
00133
00134
00135 /* Loop unrolling. Process 4 taps at a time . */
00136 stageCnt = (numStages - 1u) >> 2u;
00137
00138
00139 /* Loop over the number of taps. Unroll by a factor of 4.
00140 ** Repeat until we've computed numStages-3 coefficients. */
00141
00142 /* Process 2nd, 3rd, 4th and 5th taps ... here */
00143 while(stageCnt > 0u)
00144 {
00145 /* Read g1(n-1), g3(n-1) .... from state */
00146 gcurr1 = *px;
00147
00148 /* save g1(n) in state buffer */
00149 *px++ = (q31_t) gnext4;
00150
00151 /* Process first sample for 2nd, 6th .. tap */
00152 /* Sample processing for K2, K6.... */
00153 /* f2(n) = f1(n) + K2 * g1(n-1) */
00154 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00155 /* Process second sample for 2nd, 6th .. tap */
00156 /* for sample 2 processing */
00157 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00158 /* Process third sample for 2nd, 6th .. tap */
00159 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00160 /* Process fourth sample for 2nd, 6th .. tap */
00161 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00162
00163 /* g2(n) = f1(n) * K2 + g1(n-1) */
00164 /* Calculation of state values for next stage */
00165 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00166 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00167 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00168 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00169
00170
00171 /* Read g2(n-1), g4(n-1) .... from state */
00172 gcurr1 = *px;
00173
00174 /* save g2(n) in state buffer */
00175 *px++ = (q31_t) gnext4;
00176
00177 /* Sample processing for K3, K7.... */
00178 /* Process first sample for 3rd, 7th .. tap */
00179 /* f3(n) = f2(n) + K3 * g2(n-1) */
00180 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00181 /* Process second sample for 3rd, 7th .. tap */
00182 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00183 /* Process third sample for 3rd, 7th .. tap */
00184 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00185 /* Process fourth sample for 3rd, 7th .. tap */
00186 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00187
00188 /* Calculation of state values for next stage */
00189 /* gnext4 = fnext4 * (*pk) + gnext3; */
00190 gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3;
00191 gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2;
00192 /* gnext2 = fnext2 * (*pk) + gnext1; */
00193 gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1;
00194
00195 /* g1(n) = f0(n) * K1 + g0(n-1) */
00196 /* gnext1 = fnext1 * (*pk++) + gcurr1; */
00197 gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1;
00198
00199 /* Read g1(n-1), g3(n-1) .... from state */
00200 gcurr1 = *px;
00201
00202 /* save g1(n) in state buffer */
00203 *px++ = (q31_t) gnext4;
00204
00205 /* Sample processing for K4, K8.... */
00206 /* Process first sample for 4th, 8th .. tap */
00207 /* f4(n) = f3(n) + K4 * g3(n-1) */
00208 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00209 /* Process second sample for 4th, 8th .. tap */
00210 /* for sample 2 processing */
00211 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00212 /* Process third sample for 4th, 8th .. tap */
00213 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00214 /* Process fourth sample for 4th, 8th .. tap */
00215 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00216
00217 /* g4(n) = f3(n) * K4 + g3(n-1) */
00218 /* Calculation of state values for next stage */
00219 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00220 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00221 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00222 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00223
00224 /* Read g2(n-1), g4(n-1) .... from state */
00225 gcurr1 = *px;
00226
00227 /* save g4(n) in state buffer */
00228 *px++ = (q31_t) gnext4;
00229
00230 /* Sample processing for K5, K9.... */
00231 /* Process first sample for 5th, 9th .. tap */
00232 /* f5(n) = f4(n) + K5 * g4(n-1) */
00233 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00234 /* Process second sample for 5th, 9th .. tap */
00235 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00236 /* Process third sample for 5th, 9th .. tap */
00237 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00238 /* Process fourth sample for 5th, 9th .. tap */
00239 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00240
00241 /* Calculation of state values for next stage */
00242 /* g5(n) = f4(n) * K5 + g4(n-1) */
00243 gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3;
00244 gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2;
00245 gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1;
00246 gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1;
00247
00248 stageCnt--;
00249 }
00250
00251 /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
00252 stageCnt = (numStages - 1u) % 0x4u;
00253
00254 while(stageCnt > 0u)
00255 {
00256 gcurr1 = *px;
00257
00258 /* save g value in state buffer */
00259 *px++ = (q31_t) gnext4;
00260
00261 /* Process four samples for last three taps here */
00262 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00263 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00264 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00265 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00266
00267 /* g1(n) = f0(n) * K1 + g0(n-1) */
00268 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00269 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00270 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00271 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00272
00273 /* Update of f values for next coefficient set processing */
00274 fcurr1 = fnext1;
00275 fcurr2 = fnext2;
00276 fcurr3 = fnext3;
00277 fcurr4 = fnext4;
00278
00279 stageCnt--;
00280
00281 }
00282
00283 /* The results in the 4 accumulators, store in the destination buffer. */
00284 /* y(n) = fN(n) */
00285 *pDst++ = fcurr1;
00286 *pDst++ = (q31_t) fcurr2;
00287 *pDst++ = (q31_t) fcurr3;
00288 *pDst++ = (q31_t) fcurr4;
00289
00290 blkCnt--;
00291 }
00292
00293 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
00294 ** No loop unrolling is used. */
00295 blkCnt = blockSize % 0x4u;
00296
00297 while(blkCnt > 0u)
00298 {
00299 /* f0(n) = x(n) */
00300 fcurr1 = *pSrc++;
00301
00302 /* Initialize coeff pointer */
00303 pk = (pCoeffs);
00304
00305 /* Initialize state pointer */
00306 px = pState;
00307
00308 /* read g2(n) from state buffer */
00309 gcurr1 = *px;
00310
00311 /* for sample 1 processing */
00312 /* f1(n) = f0(n) + K1 * g0(n-1) */
00313 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00314 /* g1(n) = f0(n) * K1 + g0(n-1) */
00315 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00316 /* save g1(n) in state buffer */
00317 *px++ = fcurr1;
00318
00319 /* f1(n) is saved in fcurr1
00320 for next stage processing */
00321 fcurr1 = fnext1;
00322
00323 stageCnt = (numStages - 1u);
00324
00325 /* stage loop */
00326 while(stageCnt > 0u)
00327 {
00328 /* read g2(n) from state buffer */
00329 gcurr1 = *px;
00330
00331 /* save g1(n) in state buffer */
00332 *px++ = gnext1;
00333
00334 /* Sample processing for K2, K3.... */
00335 /* f2(n) = f1(n) + K2 * g1(n-1) */
00336 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00337 /* g2(n) = f1(n) * K2 + g1(n-1) */
00338 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00339
00340 /* f1(n) is saved in fcurr1
00341 for next stage processing */
00342 fcurr1 = fnext1;
00343
00344 stageCnt--;
00345
00346 }
00347
00348 /* y(n) = fN(n) */
00349 *pDst++ = fcurr1;
00350
00351 blkCnt--;
00352
00353 }
00354 }
00355
All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines
Generated on Mon Nov 29 2010 17:19:56 for CMSIS DSP Software Library by
1.7.2
Wyszukiwarka
Podobne podstrony:
arm fir interpolate q31? sourcearm fir ?cimate q31? sourcearm fir lattice ?2? sourcearm fir lattice q31?arm fir lattice q15? sourcearm fir sparse q31? sourcearm iir lattice q31? sourcearm fir init q31? sourcearm fir ?st q31? sourcearm fir lattice init q31? sourcearm fir lattice init ?2? sourcearm fir lattice init q31?arm fir lattice init q15? sourcearm dot prod q31? sourcearm sin cos q31? sourcearm pid init q31? sourcearm conv partial q31? sourcearm mat ?d q31? sourcearm fir init q15? sourcewięcej podobnych podstron