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
00031
00032
00033 #include "arm_math.h"
00034
00058 void arm_fir_lattice_q31(
00059 const arm_fir_lattice_instance_q31 * S,
00060 q31_t * pSrc,
00061 q31_t * pDst,
00062 uint32_t blockSize)
00063 {
00064 q31_t *pState;
00065 q31_t *pCoeffs = S->pCoeffs;
00066 q31_t *px;
00067 q31_t *pk;
00068
00069
00070 #ifndef ARM_MATH_CM0
00071
00072
00073
00074 q31_t fcurr1, fnext1, gcurr1 = 0, gnext1;
00075 q63_t fcurr2, fnext2, gnext2;
00076 q63_t fcurr3, fnext3, gnext3;
00077 q63_t fcurr4, fnext4, gnext4;
00078 uint32_t numStages = S->numStages;
00079 uint32_t blkCnt, stageCnt;
00080
00081 pState = &S->pState[0];
00082
00083 blkCnt = blockSize >> 2u;
00084
00085
00086
00087 while(blkCnt > 0u)
00088 {
00089
00090
00091
00092 fcurr1 = *pSrc++;
00093
00094 fcurr2 = *pSrc++;
00095
00096
00097 pk = (pCoeffs);
00098
00099
00100 px = pState;
00101
00102
00103 gcurr1 = *px;
00104
00105
00106
00107 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00108
00109 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + gcurr1;
00110
00111
00112
00113 fnext2 = (q31_t) (((q63_t) fcurr1 * (*pk)) >> 31) + fcurr2;
00114 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr1;
00115
00116
00117
00118
00119 fcurr3 = *pSrc++;
00120 fcurr4 = *pSrc++;
00121
00122
00123
00124 *px++ = (q31_t) fcurr4;
00125
00126
00127 fnext3 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + fcurr3;
00128 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr2;
00129
00130
00131 fnext4 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + fcurr4;
00132 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk++)) >> 31) + fcurr3;
00133
00134
00135
00136
00137
00138 fcurr1 = fnext1;
00139 fcurr2 = fnext2;
00140 fcurr3 = fnext3;
00141 fcurr4 = fnext4;
00142
00143
00144
00145 stageCnt = (numStages - 1u) >> 2u;
00146
00147
00148
00149
00150
00151
00152 while(stageCnt > 0u)
00153 {
00154
00155 gcurr1 = *px;
00156
00157
00158 *px++ = (q31_t) gnext4;
00159
00160
00161
00162
00163 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00164
00165
00166 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00167
00168 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00169
00170 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00171
00172
00173
00174 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00175 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00176 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00177 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00178
00179
00180
00181 gcurr1 = *px;
00182
00183
00184 *px++ = (q31_t) gnext4;
00185
00186
00187
00188
00189 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00190
00191 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00192
00193 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00194
00195 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00196
00197
00198
00199 gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3;
00200 gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2;
00201
00202 gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1;
00203
00204
00205
00206 gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1;
00207
00208
00209 gcurr1 = *px;
00210
00211
00212 *px++ = (q31_t) gnext4;
00213
00214
00215
00216
00217 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00218
00219
00220 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00221
00222 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00223
00224 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00225
00226
00227
00228 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00229 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00230 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00231 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00232
00233
00234 gcurr1 = *px;
00235
00236
00237 *px++ = (q31_t) gnext4;
00238
00239
00240
00241
00242 fcurr1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fnext1;
00243
00244 fcurr2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fnext2;
00245
00246 fcurr3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fnext3;
00247
00248 fcurr4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fnext4;
00249
00250
00251
00252 gnext4 = (q31_t) (((q63_t) fnext4 * (*pk)) >> 31) + gnext3;
00253 gnext3 = (q31_t) (((q63_t) fnext3 * (*pk)) >> 31) + gnext2;
00254 gnext2 = (q31_t) (((q63_t) fnext2 * (*pk)) >> 31) + gnext1;
00255 gnext1 = (q31_t) (((q63_t) fnext1 * (*pk++)) >> 31) + gcurr1;
00256
00257 stageCnt--;
00258 }
00259
00260
00261 stageCnt = (numStages - 1u) % 0x4u;
00262
00263 while(stageCnt > 0u)
00264 {
00265 gcurr1 = *px;
00266
00267
00268 *px++ = (q31_t) gnext4;
00269
00270
00271 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00272 fnext2 = (q31_t) (((q63_t) gnext1 * (*pk)) >> 31) + fcurr2;
00273 fnext3 = (q31_t) (((q63_t) gnext2 * (*pk)) >> 31) + fcurr3;
00274 fnext4 = (q31_t) (((q63_t) gnext3 * (*pk)) >> 31) + fcurr4;
00275
00276
00277 gnext4 = (q31_t) (((q63_t) fcurr4 * (*pk)) >> 31) + gnext3;
00278 gnext3 = (q31_t) (((q63_t) fcurr3 * (*pk)) >> 31) + gnext2;
00279 gnext2 = (q31_t) (((q63_t) fcurr2 * (*pk)) >> 31) + gnext1;
00280 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00281
00282
00283 fcurr1 = fnext1;
00284 fcurr2 = fnext2;
00285 fcurr3 = fnext3;
00286 fcurr4 = fnext4;
00287
00288 stageCnt--;
00289
00290 }
00291
00292
00293
00294 *pDst++ = fcurr1;
00295 *pDst++ = (q31_t) fcurr2;
00296 *pDst++ = (q31_t) fcurr3;
00297 *pDst++ = (q31_t) fcurr4;
00298
00299 blkCnt--;
00300 }
00301
00302
00303
00304 blkCnt = blockSize % 0x4u;
00305
00306 while(blkCnt > 0u)
00307 {
00308
00309 fcurr1 = *pSrc++;
00310
00311
00312 pk = (pCoeffs);
00313
00314
00315 px = pState;
00316
00317
00318 gcurr1 = *px;
00319
00320
00321
00322 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00323
00324 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00325
00326 *px++ = fcurr1;
00327
00328
00329
00330 fcurr1 = fnext1;
00331
00332 stageCnt = (numStages - 1u);
00333
00334
00335 while(stageCnt > 0u)
00336 {
00337
00338 gcurr1 = *px;
00339
00340
00341 *px++ = gnext1;
00342
00343
00344
00345 fnext1 = (q31_t) (((q63_t) gcurr1 * (*pk)) >> 31) + fcurr1;
00346
00347 gnext1 = (q31_t) (((q63_t) fcurr1 * (*pk++)) >> 31) + gcurr1;
00348
00349
00350
00351 fcurr1 = fnext1;
00352
00353 stageCnt--;
00354
00355 }
00356
00357
00358 *pDst++ = fcurr1;
00359
00360 blkCnt--;
00361
00362 }
00363
00364 #else
00365
00366
00367
00368 q31_t fcurr, fnext, gcurr, gnext;
00369 uint32_t numStages = S->numStages;
00370 uint32_t blkCnt, stageCnt;
00371
00372 pState = &S->pState[0];
00373
00374 blkCnt = blockSize;
00375
00376 while(blkCnt > 0u)
00377 {
00378
00379 fcurr = *pSrc++;
00380
00381
00382 pk = (pCoeffs);
00383
00384
00385 px = pState;
00386
00387
00388 gcurr = *px;
00389
00390
00391
00392 fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
00393
00394 gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
00395
00396 *px++ = fcurr;
00397
00398
00399
00400 fcurr = fnext;
00401
00402 stageCnt = (numStages - 1u);
00403
00404
00405 while(stageCnt > 0u)
00406 {
00407
00408 gcurr = *px;
00409
00410
00411 *px++ = gnext;
00412
00413
00414
00415 fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
00416
00417 gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
00418
00419
00420
00421 fcurr = fnext;
00422
00423 stageCnt--;
00424
00425 }
00426
00427
00428 *pDst++ = fcurr;
00429
00430 blkCnt--;
00431
00432 }
00433
00434 #endif
00435
00436 }
00437