Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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;
00062 q31_t *pCoeffs = S->pCoeffs;
00063 q31_t *px;
00064 q31_t *pk;
00065 q31_t fcurr1, fnext1, gcurr1 = 0, gnext1;
00066 q63_t fcurr2, fnext2, gnext2;
00067 q63_t fcurr3, fnext3, gnext3;
00068 q63_t fcurr4, fnext4, gnext4;
00069 uint32_t numStages = S->numStages;
00070 uint32_t blkCnt, stageCnt;
00071
00072 pState = &S->pState[0];
00073
00074 blkCnt = blockSize >> 2u;
00075
00076
00077
00078 while(blkCnt > 0u)
00079 {
00080
00081
00082
00083 fcurr1 = *pSrc++;
00084
00085 fcurr2 = *pSrc++;
00086
00087
00088 pk = (pCoeffs);
00089
00090
00091 px = pState;
00092
00093
00094 gcurr1 = *px;
00095
00096
00097
00098 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00099
00100 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + gcurr1;
00101
00102
00103
00104 fnext2 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + fcurr2;
00105 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr1;
00106
00107
00108
00109
00110 fcurr3 = *pSrc++;
00111 fcurr4 = *pSrc++;
00112
00113
00114
00115 *px++ = (q31_t) fcurr4;
00116
00117
00118 fnext3 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr3;
00119 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr2;
00120
00121
00122 fnext4 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr4;
00123 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk++)) >> 31) + fcurr3;
00124
00125
00126
00127
00128
00129 fcurr1 = fnext1;
00130 fcurr2 = fnext2;
00131 fcurr3 = fnext3;
00132 fcurr4 = fnext4;
00133
00134
00135
00136 stageCnt = (numStages - 1u) >> 2u;
00137
00138
00139
00140
00141
00142
00143 while(stageCnt > 0u)
00144 {
00145
00146 gcurr1 = *px;
00147
00148
00149 *px++ = (q31_t) gnext4;
00150
00151
00152
00153
00154 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00155
00156
00157 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00158
00159 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00160
00161 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00162
00163
00164
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
00172 gcurr1 = *px;
00173
00174
00175 *px++ = (q31_t) gnext4;
00176
00177
00178
00179
00180 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00181
00182 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00183
00184 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00185
00186 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00187
00188
00189
00190 gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3;
00191 gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2;
00192
00193 gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1;
00194
00195
00196
00197 gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1;
00198
00199
00200 gcurr1 = *px;
00201
00202
00203 *px++ = (q31_t) gnext4;
00204
00205
00206
00207
00208 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00209
00210
00211 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00212
00213 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00214
00215 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00216
00217
00218
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
00225 gcurr1 = *px;
00226
00227
00228 *px++ = (q31_t) gnext4;
00229
00230
00231
00232
00233 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00234
00235 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00236
00237 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00238
00239 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00240
00241
00242
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
00252 stageCnt = (numStages - 1u) % 0x4u;
00253
00254 while(stageCnt > 0u)
00255 {
00256 gcurr1 = *px;
00257
00258
00259 *px++ = (q31_t) gnext4;
00260
00261
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
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
00274 fcurr1 = fnext1;
00275 fcurr2 = fnext2;
00276 fcurr3 = fnext3;
00277 fcurr4 = fnext4;
00278
00279 stageCnt--;
00280
00281 }
00282
00283
00284
00285 *pDst++ = fcurr1;
00286 *pDst++ = (q31_t) fcurr2;
00287 *pDst++ = (q31_t) fcurr3;
00288 *pDst++ = (q31_t) fcurr4;
00289
00290 blkCnt--;
00291 }
00292
00293
00294
00295 blkCnt = blockSize % 0x4u;
00296
00297 while(blkCnt > 0u)
00298 {
00299
00300 fcurr1 = *pSrc++;
00301
00302
00303 pk = (pCoeffs);
00304
00305
00306 px = pState;
00307
00308
00309 gcurr1 = *px;
00310
00311
00312
00313 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00314
00315 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00316
00317 *px++ = fcurr1;
00318
00319
00320
00321 fcurr1 = fnext1;
00322
00323 stageCnt = (numStages - 1u);
00324
00325
00326 while(stageCnt > 0u)
00327 {
00328
00329 gcurr1 = *px;
00330
00331
00332 *px++ = gnext1;
00333
00334
00335
00336 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00337
00338 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00339
00340
00341
00342 fcurr1 = fnext1;
00343
00344 stageCnt--;
00345
00346 }
00347
00348
00349 *pDst++ = fcurr1;
00350
00351 blkCnt--;
00352
00353 }
00354 }
00355