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