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