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
00121 void arm_fir_lattice_f32(
00122 const arm_fir_lattice_instance_f32 * S,
00123 float32_t * pSrc,
00124 float32_t * pDst,
00125 uint32_t blockSize)
00126 {
00127 float32_t *pState;
00128 float32_t *pCoeffs = S->pCoeffs;
00129 float32_t *px;
00130 float32_t *pk;
00131
00132
00133 #ifndef ARM_MATH_CM0
00134
00135
00136
00137 float32_t fcurr1, fnext1, gcurr1, gnext1;
00138 float32_t fcurr2, fnext2, gnext2;
00139 float32_t fcurr3, fnext3, gnext3;
00140 float32_t fcurr4, fnext4, gnext4;
00141 uint32_t numStages = S->numStages;
00142 uint32_t blkCnt, stageCnt;
00143
00144 gcurr1 = 0.0f;
00145 pState = &S->pState[0];
00146
00147 blkCnt = blockSize >> 2;
00148
00149
00150
00151 while(blkCnt > 0u)
00152 {
00153
00154
00155
00156 fcurr1 = *pSrc++;
00157 fcurr2 = *pSrc++;
00158
00159
00160 pk = (pCoeffs);
00161
00162
00163 px = pState;
00164
00165
00166 gcurr1 = *px;
00167
00168
00169
00170 fnext1 = fcurr1 + ((*pk) * gcurr1);
00171
00172 gnext1 = (fcurr1 * (*pk)) + gcurr1;
00173
00174
00175
00176 fnext2 = fcurr2 + ((*pk) * fcurr1);
00177 gnext2 = (fcurr2 * (*pk)) + fcurr1;
00178
00179
00180
00181 fcurr3 = *pSrc++;
00182 fcurr4 = *pSrc++;
00183
00184
00185
00186 *px++ = fcurr4;
00187
00188
00189 fnext3 = fcurr3 + ((*pk) * fcurr2);
00190 gnext3 = (fcurr3 * (*pk)) + fcurr2;
00191
00192
00193 fnext4 = fcurr4 + ((*pk) * fcurr3);
00194 gnext4 = (fcurr4 * (*pk++)) + fcurr3;
00195
00196
00197 fcurr1 = fnext1;
00198 fcurr2 = fnext2;
00199 fcurr3 = fnext3;
00200 fcurr4 = fnext4;
00201
00202
00203 stageCnt = (numStages - 1u) >> 2u;
00204
00205
00206
00207
00208
00209 while(stageCnt > 0u)
00210 {
00211
00212 gcurr1 = *px;
00213
00214
00215 *px++ = gnext4;
00216
00217
00218
00219
00220 fnext1 = fcurr1 + ((*pk) * gcurr1);
00221
00222
00223 fnext2 = fcurr2 + ((*pk) * gnext1);
00224
00225 fnext3 = fcurr3 + ((*pk) * gnext2);
00226
00227 fnext4 = fcurr4 + ((*pk) * gnext3);
00228
00229
00230
00231 gnext4 = (fcurr4 * (*pk)) + gnext3;
00232 gnext3 = (fcurr3 * (*pk)) + gnext2;
00233 gnext2 = (fcurr2 * (*pk)) + gnext1;
00234 gnext1 = (fcurr1 * (*pk++)) + gcurr1;
00235
00236
00237
00238 gcurr1 = *px;
00239
00240
00241 *px++ = gnext4;
00242
00243
00244
00245
00246 fcurr1 = fnext1 + ((*pk) * gcurr1);
00247
00248 fcurr2 = fnext2 + ((*pk) * gnext1);
00249
00250 fcurr3 = fnext3 + ((*pk) * gnext2);
00251
00252 fcurr4 = fnext4 + ((*pk) * gnext3);
00253
00254
00255
00256 gnext4 = (fnext4 * (*pk)) + gnext3;
00257 gnext3 = (fnext3 * (*pk)) + gnext2;
00258 gnext2 = (fnext2 * (*pk)) + gnext1;
00259 gnext1 = (fnext1 * (*pk++)) + gcurr1;
00260
00261
00262
00263 gcurr1 = *px;
00264
00265
00266 *px++ = gnext4;
00267
00268
00269
00270
00271 fnext1 = fcurr1 + ((*pk) * gcurr1);
00272
00273
00274 fnext2 = fcurr2 + ((*pk) * gnext1);
00275
00276 fnext3 = fcurr3 + ((*pk) * gnext2);
00277
00278 fnext4 = fcurr4 + ((*pk) * gnext3);
00279
00280
00281
00282 gnext4 = (fcurr4 * (*pk)) + gnext3;
00283 gnext3 = (fcurr3 * (*pk)) + gnext2;
00284 gnext2 = (fcurr2 * (*pk)) + gnext1;
00285 gnext1 = (fcurr1 * (*pk++)) + gcurr1;
00286
00287
00288 gcurr1 = *px;
00289
00290
00291 *px++ = gnext4;
00292
00293
00294
00295
00296 fcurr1 = fnext1 + ((*pk) * gcurr1);
00297
00298 fcurr2 = fnext2 + ((*pk) * gnext1);
00299
00300 fcurr3 = fnext3 + ((*pk) * gnext2);
00301
00302 fcurr4 = fnext4 + ((*pk) * gnext3);
00303
00304
00305
00306 gnext4 = (fnext4 * (*pk)) + gnext3;
00307 gnext3 = (fnext3 * (*pk)) + gnext2;
00308 gnext2 = (fnext2 * (*pk)) + gnext1;
00309 gnext1 = (fnext1 * (*pk++)) + gcurr1;
00310
00311 stageCnt--;
00312 }
00313
00314
00315 stageCnt = (numStages - 1u) % 0x4u;
00316
00317 while(stageCnt > 0u)
00318 {
00319 gcurr1 = *px;
00320
00321
00322 *px++ = gnext4;
00323
00324
00325 fnext1 = fcurr1 + ((*pk) * gcurr1);
00326 fnext2 = fcurr2 + ((*pk) * gnext1);
00327 fnext3 = fcurr3 + ((*pk) * gnext2);
00328 fnext4 = fcurr4 + ((*pk) * gnext3);
00329
00330
00331 gnext4 = (fcurr4 * (*pk)) + gnext3;
00332 gnext3 = (fcurr3 * (*pk)) + gnext2;
00333 gnext2 = (fcurr2 * (*pk)) + gnext1;
00334 gnext1 = (fcurr1 * (*pk++)) + gcurr1;
00335
00336
00337 fcurr1 = fnext1;
00338 fcurr2 = fnext2;
00339 fcurr3 = fnext3;
00340 fcurr4 = fnext4;
00341
00342 stageCnt--;
00343
00344 }
00345
00346
00347
00348 *pDst++ = fcurr1;
00349 *pDst++ = fcurr2;
00350 *pDst++ = fcurr3;
00351 *pDst++ = fcurr4;
00352
00353 blkCnt--;
00354 }
00355
00356
00357
00358 blkCnt = blockSize % 0x4u;
00359
00360 while(blkCnt > 0u)
00361 {
00362
00363 fcurr1 = *pSrc++;
00364
00365
00366 pk = (pCoeffs);
00367
00368
00369 px = pState;
00370
00371
00372 gcurr1 = *px;
00373
00374
00375
00376 fnext1 = fcurr1 + ((*pk) * gcurr1);
00377
00378 gnext1 = (fcurr1 * (*pk++)) + gcurr1;
00379
00380
00381 *px++ = fcurr1;
00382
00383
00384
00385 fcurr1 = fnext1;
00386
00387 stageCnt = (numStages - 1u);
00388
00389
00390 while(stageCnt > 0u)
00391 {
00392
00393 gcurr1 = *px;
00394
00395
00396 *px++ = gnext1;
00397
00398
00399
00400 fnext1 = fcurr1 + ((*pk) * gcurr1);
00401
00402 gnext1 = (fcurr1 * (*pk++)) + gcurr1;
00403
00404
00405
00406 fcurr1 = fnext1;
00407
00408 stageCnt--;
00409
00410 }
00411
00412
00413 *pDst++ = fcurr1;
00414
00415 blkCnt--;
00416
00417 }
00418
00419 #else
00420
00421
00422
00423 float32_t fcurr, fnext, gcurr, gnext;
00424 uint32_t numStages = S->numStages;
00425 uint32_t blkCnt, stageCnt;
00426
00427 pState = &S->pState[0];
00428
00429 blkCnt = blockSize;
00430
00431 while(blkCnt > 0u)
00432 {
00433
00434 fcurr = *pSrc++;
00435
00436
00437 pk = pCoeffs;
00438
00439
00440 px = pState;
00441
00442
00443 gcurr = *px;
00444
00445
00446
00447 fnext = fcurr + ((*pk) * gcurr);
00448
00449 gnext = (fcurr * (*pk++)) + gcurr;
00450
00451
00452 *px++ = fcurr;
00453
00454
00455
00456 fcurr = fnext;
00457
00458 stageCnt = (numStages - 1u);
00459
00460
00461 while(stageCnt > 0u)
00462 {
00463
00464 gcurr = *px;
00465
00466
00467 *px++ = gnext;
00468
00469
00470
00471 fnext = fcurr + ((*pk) * gcurr);
00472
00473 gnext = (fcurr * (*pk++)) + gcurr;
00474
00475
00476
00477 fcurr = fnext;
00478
00479 stageCnt--;
00480
00481 }
00482
00483
00484 *pDst++ = fcurr;
00485
00486 blkCnt--;
00487
00488 }
00489
00490 #endif
00491
00492 }
00493