]> git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_lattice_q15.c
Merge commit '1fe4406f374291ab2e86e95a97341fd9c475fcb8'
[tmk_keyboard.git] / tmk_core / tool / mbed / mbed-sdk / libraries / dsp / cmsis_dsp / FilteringFunctions / arm_fir_lattice_q15.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
3 *
4 * $Date: 17. January 2013
5 * $Revision: V1.4.1
6 *
7 * Project: CMSIS DSP Library
8 * Title: arm_fir_lattice_q15.c
9 *
10 * Description: Q15 FIR lattice filter processing function.
11 *
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
13 *
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
16 * are met:
17 * - Redistributions of source code must retain the above copyright
18 * notice, this list of conditions and the following disclaimer.
19 * - Redistributions in binary form must reproduce the above copyright
20 * notice, this list of conditions and the following disclaimer in
21 * the documentation and/or other materials provided with the
22 * distribution.
23 * - Neither the name of ARM LIMITED nor the names of its contributors
24 * may be used to endorse or promote products derived from this
25 * software without specific prior written permission.
26 *
27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
38 * POSSIBILITY OF SUCH DAMAGE.
39 * -------------------------------------------------------------------- */
40
41 #include "arm_math.h"
42
43 /**
44 * @ingroup groupFilters
45 */
46
47 /**
48 * @addtogroup FIR_Lattice
49 * @{
50 */
51
52
53 /**
54 * @brief Processing function for the Q15 FIR lattice filter.
55 * @param[in] *S points to an instance of the Q15 FIR lattice structure.
56 * @param[in] *pSrc points to the block of input data.
57 * @param[out] *pDst points to the block of output data
58 * @param[in] blockSize number of samples to process.
59 * @return none.
60 */
61
62 void arm_fir_lattice_q15(
63 const arm_fir_lattice_instance_q15 * S,
64 q15_t * pSrc,
65 q15_t * pDst,
66 uint32_t blockSize)
67 {
68 q15_t *pState; /* State pointer */
69 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
70 q15_t *px; /* temporary state pointer */
71 q15_t *pk; /* temporary coefficient pointer */
72
73
74 #ifndef ARM_MATH_CM0_FAMILY
75
76 /* Run the below code for Cortex-M4 and Cortex-M3 */
77
78 q31_t fcurnt1, fnext1, gcurnt1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */
79 q31_t fcurnt2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */
80 q31_t fcurnt3, fnext3, gnext3; /* temporary variables for third sample in loop unrolling */
81 q31_t fcurnt4, fnext4, gnext4; /* temporary variables for fourth sample in loop unrolling */
82 uint32_t numStages = S->numStages; /* Number of stages in the filter */
83 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
84
85 pState = &S->pState[0];
86
87 blkCnt = blockSize >> 2u;
88
89 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
90 ** a second loop below computes the remaining 1 to 3 samples. */
91 while(blkCnt > 0u)
92 {
93
94 /* Read two samples from input buffer */
95 /* f0(n) = x(n) */
96 fcurnt1 = *pSrc++;
97 fcurnt2 = *pSrc++;
98
99 /* Initialize coeff pointer */
100 pk = (pCoeffs);
101
102 /* Initialize state pointer */
103 px = pState;
104
105 /* Read g0(n-1) from state */
106 gcurnt1 = *px;
107
108 /* Process first sample for first tap */
109 /* f1(n) = f0(n) + K1 * g0(n-1) */
110 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
111 fnext1 = __SSAT(fnext1, 16);
112
113 /* g1(n) = f0(n) * K1 + g0(n-1) */
114 gnext1 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + gcurnt1;
115 gnext1 = __SSAT(gnext1, 16);
116
117 /* Process second sample for first tap */
118 /* for sample 2 processing */
119 fnext2 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + fcurnt2;
120 fnext2 = __SSAT(fnext2, 16);
121
122 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt1;
123 gnext2 = __SSAT(gnext2, 16);
124
125
126 /* Read next two samples from input buffer */
127 /* f0(n+2) = x(n+2) */
128 fcurnt3 = *pSrc++;
129 fcurnt4 = *pSrc++;
130
131 /* Copy only last input samples into the state buffer
132 which is used for next four samples processing */
133 *px++ = (q15_t) fcurnt4;
134
135 /* Process third sample for first tap */
136 fnext3 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt3;
137 fnext3 = __SSAT(fnext3, 16);
138 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt2;
139 gnext3 = __SSAT(gnext3, 16);
140
141 /* Process fourth sample for first tap */
142 fnext4 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt4;
143 fnext4 = __SSAT(fnext4, 16);
144 gnext4 = (q31_t) ((fcurnt4 * (*pk++)) >> 15u) + fcurnt3;
145 gnext4 = __SSAT(gnext4, 16);
146
147 /* Update of f values for next coefficient set processing */
148 fcurnt1 = fnext1;
149 fcurnt2 = fnext2;
150 fcurnt3 = fnext3;
151 fcurnt4 = fnext4;
152
153
154 /* Loop unrolling. Process 4 taps at a time . */
155 stageCnt = (numStages - 1u) >> 2;
156
157
158 /* Loop over the number of taps. Unroll by a factor of 4.
159 ** Repeat until we've computed numStages-3 coefficients. */
160
161 /* Process 2nd, 3rd, 4th and 5th taps ... here */
162 while(stageCnt > 0u)
163 {
164 /* Read g1(n-1), g3(n-1) .... from state */
165 gcurnt1 = *px;
166
167 /* save g1(n) in state buffer */
168 *px++ = (q15_t) gnext4;
169
170 /* Process first sample for 2nd, 6th .. tap */
171 /* Sample processing for K2, K6.... */
172 /* f1(n) = f0(n) + K1 * g0(n-1) */
173 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
174 fnext1 = __SSAT(fnext1, 16);
175
176
177 /* Process second sample for 2nd, 6th .. tap */
178 /* for sample 2 processing */
179 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
180 fnext2 = __SSAT(fnext2, 16);
181 /* Process third sample for 2nd, 6th .. tap */
182 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
183 fnext3 = __SSAT(fnext3, 16);
184 /* Process fourth sample for 2nd, 6th .. tap */
185 /* fnext4 = fcurnt4 + (*pk) * gnext3; */
186 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
187 fnext4 = __SSAT(fnext4, 16);
188
189 /* g1(n) = f0(n) * K1 + g0(n-1) */
190 /* Calculation of state values for next stage */
191 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
192 gnext4 = __SSAT(gnext4, 16);
193 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
194 gnext3 = __SSAT(gnext3, 16);
195
196 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
197 gnext2 = __SSAT(gnext2, 16);
198
199 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
200 gnext1 = __SSAT(gnext1, 16);
201
202
203 /* Read g2(n-1), g4(n-1) .... from state */
204 gcurnt1 = *px;
205
206 /* save g1(n) in state buffer */
207 *px++ = (q15_t) gnext4;
208
209 /* Sample processing for K3, K7.... */
210 /* Process first sample for 3rd, 7th .. tap */
211 /* f3(n) = f2(n) + K3 * g2(n-1) */
212 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
213 fcurnt1 = __SSAT(fcurnt1, 16);
214
215 /* Process second sample for 3rd, 7th .. tap */
216 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
217 fcurnt2 = __SSAT(fcurnt2, 16);
218
219 /* Process third sample for 3rd, 7th .. tap */
220 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
221 fcurnt3 = __SSAT(fcurnt3, 16);
222
223 /* Process fourth sample for 3rd, 7th .. tap */
224 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
225 fcurnt4 = __SSAT(fcurnt4, 16);
226
227 /* Calculation of state values for next stage */
228 /* g3(n) = f2(n) * K3 + g2(n-1) */
229 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
230 gnext4 = __SSAT(gnext4, 16);
231
232 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
233 gnext3 = __SSAT(gnext3, 16);
234
235 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
236 gnext2 = __SSAT(gnext2, 16);
237
238 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
239 gnext1 = __SSAT(gnext1, 16);
240
241 /* Read g1(n-1), g3(n-1) .... from state */
242 gcurnt1 = *px;
243
244 /* save g1(n) in state buffer */
245 *px++ = (q15_t) gnext4;
246
247 /* Sample processing for K4, K8.... */
248 /* Process first sample for 4th, 8th .. tap */
249 /* f4(n) = f3(n) + K4 * g3(n-1) */
250 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
251 fnext1 = __SSAT(fnext1, 16);
252
253 /* Process second sample for 4th, 8th .. tap */
254 /* for sample 2 processing */
255 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
256 fnext2 = __SSAT(fnext2, 16);
257
258 /* Process third sample for 4th, 8th .. tap */
259 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
260 fnext3 = __SSAT(fnext3, 16);
261
262 /* Process fourth sample for 4th, 8th .. tap */
263 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
264 fnext4 = __SSAT(fnext4, 16);
265
266 /* g4(n) = f3(n) * K4 + g3(n-1) */
267 /* Calculation of state values for next stage */
268 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
269 gnext4 = __SSAT(gnext4, 16);
270
271 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
272 gnext3 = __SSAT(gnext3, 16);
273
274 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
275 gnext2 = __SSAT(gnext2, 16);
276 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
277 gnext1 = __SSAT(gnext1, 16);
278
279
280 /* Read g2(n-1), g4(n-1) .... from state */
281 gcurnt1 = *px;
282
283 /* save g4(n) in state buffer */
284 *px++ = (q15_t) gnext4;
285
286 /* Sample processing for K5, K9.... */
287 /* Process first sample for 5th, 9th .. tap */
288 /* f5(n) = f4(n) + K5 * g4(n-1) */
289 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
290 fcurnt1 = __SSAT(fcurnt1, 16);
291
292 /* Process second sample for 5th, 9th .. tap */
293 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
294 fcurnt2 = __SSAT(fcurnt2, 16);
295
296 /* Process third sample for 5th, 9th .. tap */
297 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
298 fcurnt3 = __SSAT(fcurnt3, 16);
299
300 /* Process fourth sample for 5th, 9th .. tap */
301 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
302 fcurnt4 = __SSAT(fcurnt4, 16);
303
304 /* Calculation of state values for next stage */
305 /* g5(n) = f4(n) * K5 + g4(n-1) */
306 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
307 gnext4 = __SSAT(gnext4, 16);
308 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
309 gnext3 = __SSAT(gnext3, 16);
310 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
311 gnext2 = __SSAT(gnext2, 16);
312 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
313 gnext1 = __SSAT(gnext1, 16);
314
315 stageCnt--;
316 }
317
318 /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
319 stageCnt = (numStages - 1u) % 0x4u;
320
321 while(stageCnt > 0u)
322 {
323 gcurnt1 = *px;
324
325 /* save g value in state buffer */
326 *px++ = (q15_t) gnext4;
327
328 /* Process four samples for last three taps here */
329 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
330 fnext1 = __SSAT(fnext1, 16);
331 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
332 fnext2 = __SSAT(fnext2, 16);
333
334 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
335 fnext3 = __SSAT(fnext3, 16);
336
337 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
338 fnext4 = __SSAT(fnext4, 16);
339
340 /* g1(n) = f0(n) * K1 + g0(n-1) */
341 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
342 gnext4 = __SSAT(gnext4, 16);
343 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
344 gnext3 = __SSAT(gnext3, 16);
345 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
346 gnext2 = __SSAT(gnext2, 16);
347 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
348 gnext1 = __SSAT(gnext1, 16);
349
350 /* Update of f values for next coefficient set processing */
351 fcurnt1 = fnext1;
352 fcurnt2 = fnext2;
353 fcurnt3 = fnext3;
354 fcurnt4 = fnext4;
355
356 stageCnt--;
357
358 }
359
360 /* The results in the 4 accumulators, store in the destination buffer. */
361 /* y(n) = fN(n) */
362
363 #ifndef ARM_MATH_BIG_ENDIAN
364
365 *__SIMD32(pDst)++ = __PKHBT(fcurnt1, fcurnt2, 16);
366 *__SIMD32(pDst)++ = __PKHBT(fcurnt3, fcurnt4, 16);
367
368 #else
369
370 *__SIMD32(pDst)++ = __PKHBT(fcurnt2, fcurnt1, 16);
371 *__SIMD32(pDst)++ = __PKHBT(fcurnt4, fcurnt3, 16);
372
373 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
374
375 blkCnt--;
376 }
377
378 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
379 ** No loop unrolling is used. */
380 blkCnt = blockSize % 0x4u;
381
382 while(blkCnt > 0u)
383 {
384 /* f0(n) = x(n) */
385 fcurnt1 = *pSrc++;
386
387 /* Initialize coeff pointer */
388 pk = (pCoeffs);
389
390 /* Initialize state pointer */
391 px = pState;
392
393 /* read g2(n) from state buffer */
394 gcurnt1 = *px;
395
396 /* for sample 1 processing */
397 /* f1(n) = f0(n) + K1 * g0(n-1) */
398 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
399 fnext1 = __SSAT(fnext1, 16);
400
401
402 /* g1(n) = f0(n) * K1 + g0(n-1) */
403 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
404 gnext1 = __SSAT(gnext1, 16);
405
406 /* save g1(n) in state buffer */
407 *px++ = (q15_t) fcurnt1;
408
409 /* f1(n) is saved in fcurnt1
410 for next stage processing */
411 fcurnt1 = fnext1;
412
413 stageCnt = (numStages - 1u);
414
415 /* stage loop */
416 while(stageCnt > 0u)
417 {
418 /* read g2(n) from state buffer */
419 gcurnt1 = *px;
420
421 /* save g1(n) in state buffer */
422 *px++ = (q15_t) gnext1;
423
424 /* Sample processing for K2, K3.... */
425 /* f2(n) = f1(n) + K2 * g1(n-1) */
426 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
427 fnext1 = __SSAT(fnext1, 16);
428
429 /* g2(n) = f1(n) * K2 + g1(n-1) */
430 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
431 gnext1 = __SSAT(gnext1, 16);
432
433
434 /* f1(n) is saved in fcurnt1
435 for next stage processing */
436 fcurnt1 = fnext1;
437
438 stageCnt--;
439
440 }
441
442 /* y(n) = fN(n) */
443 *pDst++ = __SSAT(fcurnt1, 16);
444
445
446 blkCnt--;
447
448 }
449
450 #else
451
452 /* Run the below code for Cortex-M0 */
453
454 q31_t fcurnt, fnext, gcurnt, gnext; /* temporary variables */
455 uint32_t numStages = S->numStages; /* Length of the filter */
456 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
457
458 pState = &S->pState[0];
459
460 blkCnt = blockSize;
461
462 while(blkCnt > 0u)
463 {
464 /* f0(n) = x(n) */
465 fcurnt = *pSrc++;
466
467 /* Initialize coeff pointer */
468 pk = (pCoeffs);
469
470 /* Initialize state pointer */
471 px = pState;
472
473 /* read g0(n-1) from state buffer */
474 gcurnt = *px;
475
476 /* for sample 1 processing */
477 /* f1(n) = f0(n) + K1 * g0(n-1) */
478 fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
479 fnext = __SSAT(fnext, 16);
480
481
482 /* g1(n) = f0(n) * K1 + g0(n-1) */
483 gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
484 gnext = __SSAT(gnext, 16);
485
486 /* save f0(n) in state buffer */
487 *px++ = (q15_t) fcurnt;
488
489 /* f1(n) is saved in fcurnt
490 for next stage processing */
491 fcurnt = fnext;
492
493 stageCnt = (numStages - 1u);
494
495 /* stage loop */
496 while(stageCnt > 0u)
497 {
498 /* read g1(n-1) from state buffer */
499 gcurnt = *px;
500
501 /* save g0(n-1) in state buffer */
502 *px++ = (q15_t) gnext;
503
504 /* Sample processing for K2, K3.... */
505 /* f2(n) = f1(n) + K2 * g1(n-1) */
506 fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
507 fnext = __SSAT(fnext, 16);
508
509 /* g2(n) = f1(n) * K2 + g1(n-1) */
510 gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
511 gnext = __SSAT(gnext, 16);
512
513
514 /* f1(n) is saved in fcurnt
515 for next stage processing */
516 fcurnt = fnext;
517
518 stageCnt--;
519
520 }
521
522 /* y(n) = fN(n) */
523 *pDst++ = __SSAT(fcurnt, 16);
524
525
526 blkCnt--;
527
528 }
529
530 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
531
532 }
533
534 /**
535 * @} end of FIR_Lattice group
536 */
Imprint / Impressum