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
00060 void arm_iir_lattice_q15(
00061 const arm_iir_lattice_instance_q15 * S,
00062 q15_t * pSrc,
00063 q15_t * pDst,
00064 uint32_t blockSize)
00065 {
00066 q31_t fcurr, fnext, gcurr = 0, gnext;
00067 q15_t gnext1, gnext2;
00068 uint32_t stgCnt;
00069 q63_t acc;
00070 uint32_t blkCnt, tapCnt;
00071 q15_t *px1, *px2, *pk, *pv;
00072 uint32_t numStages = S->numStages;
00073 q15_t *pState;
00074 q15_t *pStateCurnt;
00075 q15_t out;
00076 q31_t v;
00077
00078
00079 blkCnt = blockSize;
00080
00081 pState = &S->pState[0];
00082
00083
00084 while(blkCnt > 0u)
00085 {
00086
00087
00088 fcurr = *pSrc++;
00089
00090
00091 px1 = pState;
00092
00093 px2 = pState;
00094
00095 acc = 0;
00096
00097 pv = &S->pvCoeffs[0];
00098
00099 pk = &S->pkCoeffs[0];
00100
00101
00102
00103 gcurr = *px1++;
00104
00105 fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
00106 fnext = __SSAT(fnext, 16);
00107
00108 gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
00109 gnext = __SSAT(gnext, 16);
00110
00111 *px2++ = (q15_t) gnext;
00112
00113 acc += (q31_t) ((gnext * (*pv++)));
00114
00115
00116
00117 fcurr = fnext;
00118
00119
00120 tapCnt = (numStages - 1u) >> 2;
00121
00122 while(tapCnt > 0u)
00123 {
00124
00125
00126
00127 gcurr = *px1++;
00128
00129
00130 fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
00131 fnext = __SSAT(fnext, 16);
00132
00133 gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
00134 gnext1 = (q15_t) __SSAT(gnext, 16);
00135
00136 *px2++ = (q15_t) gnext1;
00137
00138
00139
00140
00141 gcurr = *px1++;
00142
00143
00144 fcurr = fnext - (((q31_t) gcurr * (*pk)) >> 15);
00145 fcurr = __SSAT(fcurr, 16);
00146
00147 gnext = (((q31_t) fcurr * (*pk++)) >> 15) + gcurr;
00148 gnext2 = (q15_t) __SSAT(gnext, 16);
00149
00150 *px2++ = (q15_t) gnext2;
00151
00152
00153 v = *__SIMD32(pv)++;
00154
00155
00156
00157 gnext = __PKHBT(gnext1, gnext2, 16);
00158
00159
00160
00161
00162 acc = __SMLALD(gnext, v, acc);
00163
00164
00165
00166
00167 gcurr = *px1++;
00168
00169
00170 fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
00171 fnext = __SSAT(fnext, 16);
00172
00173 gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
00174 gnext1 = (q15_t) __SSAT(gnext, 16);
00175
00176 *px2++ = (q15_t) gnext1;
00177
00178
00179
00180
00181 gcurr = *px1++;
00182
00183
00184 fcurr = fnext - (((q31_t) gcurr * (*pk)) >> 15);
00185 fcurr = __SSAT(fcurr, 16);
00186
00187 gnext = (((q31_t) fcurr * (*pk++)) >> 15) + gcurr;
00188 gnext2 = (q15_t) __SSAT(gnext, 16);
00189
00190 *px2++ = (q15_t) gnext2;
00191
00192
00193 v = *__SIMD32(pv)++;
00194
00195
00196 gnext = __PKHBT(gnext1, gnext2, 16);
00197
00198
00199
00200
00201 acc = __SMLALD(gnext, v, acc);
00202
00203 tapCnt--;
00204
00205 }
00206
00207 fnext = fcurr;
00208
00209
00210 tapCnt = (numStages - 1u) % 0x4u;
00211
00212 while(tapCnt > 0u)
00213 {
00214 gcurr = *px1++;
00215
00216 fnext = fcurr - (((q31_t) gcurr * (*pk)) >> 15);
00217 fnext = __SSAT(fnext, 16);
00218 gnext = (((q31_t) fnext * (*pk++)) >> 15) + gcurr;
00219 gnext = __SSAT(gnext, 16);
00220
00221 acc += (q31_t) (((q31_t) gnext * (*pv++)));
00222 *px2++ = (q15_t) gnext;
00223 fcurr = fnext;
00224
00225 tapCnt--;
00226 }
00227
00228
00229 acc += (q31_t) (((q31_t) fnext * (*pv++)));
00230
00231 out = (q15_t) __SSAT(acc >> 15, 16);
00232 *px2++ = (q15_t) fnext;
00233
00234
00235 *pDst++ = out;
00236
00237
00238 pState = pState + 1u;
00239 blkCnt--;
00240
00241 }
00242
00243
00244
00245
00246 pStateCurnt = &S->pState[0];
00247 pState = &S->pState[blockSize];
00248
00249 stgCnt = (numStages >> 2u);
00250
00251
00252 while(stgCnt > 0u)
00253 {
00254 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00255 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00256
00257
00258 stgCnt--;
00259
00260 }
00261
00262
00263 stgCnt = (numStages) % 0x4u;
00264
00265
00266 while(stgCnt > 0u)
00267 {
00268 *pStateCurnt++ = *pState++;
00269
00270
00271 stgCnt--;
00272 }
00273
00274 }
00275
00276
00277
00278