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 #include "arm_math.h"
00033
00034
00058 void arm_fir_sparse_q31(
00059 arm_fir_sparse_instance_q31 * S,
00060 q31_t * pSrc,
00061 q31_t * pDst,
00062 q31_t * pScratchIn,
00063 uint32_t blockSize)
00064 {
00065
00066 q31_t *pState = S->pState;
00067 q31_t *pCoeffs = S->pCoeffs;
00068 q31_t *px;
00069 q31_t *py = pState;
00070 q31_t *pb = pScratchIn;
00071 q31_t *pOut;
00072 q63_t out;
00073 int32_t *pTapDelay = S->pTapDelay;
00074 uint32_t delaySize = S->maxDelay + blockSize;
00075 uint16_t numTaps = S->numTaps;
00076 int32_t readIndex;
00077 uint32_t tapCnt, blkCnt;
00078 q31_t coeff = *pCoeffs++;
00079 q31_t in;
00080
00081
00082
00083
00084 arm_circularWrite_f32((int32_t *) py, delaySize, &S->stateIndex, 1,
00085 (int32_t *) pSrc, 1, blockSize);
00086
00087
00088 readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
00089
00090
00091 if(readIndex < 0)
00092 {
00093 readIndex += (int32_t) delaySize;
00094 }
00095
00096
00097 py = pState;
00098
00099
00100 arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
00101 (int32_t *) pb, (int32_t *) pb, blockSize, 1,
00102 blockSize);
00103
00104
00105 px = pb;
00106
00107
00108 pOut = pDst;
00109
00110
00111 #ifndef ARM_MATH_CM0
00112
00113
00114
00115
00116
00117 blkCnt = blockSize >> 2;
00118
00119 while(blkCnt > 0u)
00120 {
00121
00122 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00123 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00124 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00125 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00126
00127
00128 blkCnt--;
00129 }
00130
00131
00132
00133 blkCnt = blockSize % 0x4u;
00134
00135 while(blkCnt > 0u)
00136 {
00137
00138 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00139
00140
00141 blkCnt--;
00142 }
00143
00144
00145
00146 coeff = *pCoeffs++;
00147
00148
00149 readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
00150
00151
00152 if(readIndex < 0)
00153 {
00154 readIndex += (int32_t) delaySize;
00155 }
00156
00157
00158 tapCnt = (uint32_t) numTaps - 1u;
00159
00160 while(tapCnt > 0u)
00161 {
00162
00163 py = pState;
00164
00165
00166 arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
00167 (int32_t *) pb, (int32_t *) pb, blockSize, 1,
00168 blockSize);
00169
00170
00171 px = pb;
00172
00173
00174 pOut = pDst;
00175
00176
00177
00178 blkCnt = blockSize >> 2;
00179
00180 while(blkCnt > 0u)
00181 {
00182 out = *pOut;
00183 out += ((q63_t) * px++ * coeff) >> 32;
00184 *pOut++ = (q31_t) (out);
00185
00186 out = *pOut;
00187 out += ((q63_t) * px++ * coeff) >> 32;
00188 *pOut++ = (q31_t) (out);
00189
00190 out = *pOut;
00191 out += ((q63_t) * px++ * coeff) >> 32;
00192 *pOut++ = (q31_t) (out);
00193
00194 out = *pOut;
00195 out += ((q63_t) * px++ * coeff) >> 32;
00196 *pOut++ = (q31_t) (out);
00197
00198
00199 blkCnt--;
00200 }
00201
00202
00203
00204 blkCnt = blockSize % 0x4u;
00205
00206 while(blkCnt > 0u)
00207 {
00208
00209 out = *pOut;
00210 out += ((q63_t) * px++ * coeff) >> 32;
00211 *pOut++ = (q31_t) (out);
00212
00213
00214 blkCnt--;
00215 }
00216
00217
00218
00219 coeff = *pCoeffs++;
00220
00221
00222 readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
00223
00224
00225 if(readIndex < 0)
00226 {
00227 readIndex += (int32_t) delaySize;
00228 }
00229
00230
00231 tapCnt--;
00232 }
00233
00234
00235 pOut = pDst;
00236
00237
00238
00239
00240 blkCnt = blockSize >> 2;
00241
00242 while(blkCnt > 0u)
00243 {
00244 in = *pOut << 1;
00245 *pOut++ = in;
00246 in = *pOut << 1;
00247 *pOut++ = in;
00248 in = *pOut << 1;
00249 *pOut++ = in;
00250 in = *pOut << 1;
00251 *pOut++ = in;
00252
00253
00254 blkCnt--;
00255 }
00256
00257
00258
00259 blkCnt = blockSize % 0x4u;
00260
00261 while(blkCnt > 0u)
00262 {
00263 in = *pOut << 1;
00264 *pOut++ = in;
00265
00266
00267 blkCnt--;
00268 }
00269
00270 #else
00271
00272
00273 blkCnt = blockSize;
00274
00275 while(blkCnt > 0u)
00276 {
00277
00278 *pOut++ = (q31_t) (((q63_t) * px++ * coeff) >> 32);
00279
00280
00281 blkCnt--;
00282 }
00283
00284
00285
00286 coeff = *pCoeffs++;
00287
00288
00289 readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
00290
00291
00292 if(readIndex < 0)
00293 {
00294 readIndex += (int32_t) delaySize;
00295 }
00296
00297
00298 tapCnt = (uint32_t) numTaps - 1u;
00299
00300 while(tapCnt > 0u)
00301 {
00302
00303 py = pState;
00304
00305
00306 arm_circularRead_f32((int32_t *) py, delaySize, &readIndex, 1,
00307 (int32_t *) pb, (int32_t *) pb, blockSize, 1,
00308 blockSize);
00309
00310
00311 px = pb;
00312
00313
00314 pOut = pDst;
00315
00316 blkCnt = blockSize;
00317
00318 while(blkCnt > 0u)
00319 {
00320
00321 out = *pOut;
00322 out += ((q63_t) * px++ * coeff) >> 32;
00323 *pOut++ = (q31_t) (out);
00324
00325
00326 blkCnt--;
00327 }
00328
00329
00330
00331 coeff = *pCoeffs++;
00332
00333
00334 readIndex = (int32_t) (S->stateIndex - blockSize) - *pTapDelay++;
00335
00336
00337 if(readIndex < 0)
00338 {
00339 readIndex += (int32_t) delaySize;
00340 }
00341
00342
00343 tapCnt--;
00344 }
00345
00346
00347 pOut = pDst;
00348
00349
00350 blkCnt = blockSize;
00351
00352 while(blkCnt > 0u)
00353 {
00354 in = *pOut << 1;
00355 *pOut++ = in;
00356
00357
00358 blkCnt--;
00359 }
00360
00361 #endif
00362
00363 }
00364