]>
git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_conv_q31.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
8 * Title: arm_conv_q31.c
10 * Description: Convolution of Q31 sequences.
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
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
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.
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 * -------------------------------------------------------------------- */
44 * @ingroup groupFilters
53 * @brief Convolution of Q31 sequences.
54 * @param[in] *pSrcA points to the first input sequence.
55 * @param[in] srcALen length of the first input sequence.
56 * @param[in] *pSrcB points to the second input sequence.
57 * @param[in] srcBLen length of the second input sequence.
58 * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
62 * <b>Scaling and Overflow Behavior:</b>
65 * The function is implemented using an internal 64-bit accumulator.
66 * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
67 * There is no saturation on intermediate additions.
68 * Thus, if the accumulator overflows it wraps around and distorts the result.
69 * The input signals should be scaled down to avoid intermediate overflows.
70 * Scale down the inputs by log2(min(srcALen, srcBLen)) (log2 is read as log to the base 2) times to avoid overflows,
71 * as maximum of min(srcALen, srcBLen) number of additions are carried internally.
72 * The 2.62 accumulator is right shifted by 31 bits and saturated to 1.31 format to yield the final result.
75 * See <code>arm_conv_fast_q31()</code> for a faster but less precise implementation of this function for Cortex-M3 and Cortex-M4.
87 #ifndef ARM_MATH_CM0_FAMILY
89 /* Run the below code for Cortex-M4 and Cortex-M3 */
91 q31_t
*pIn1
; /* inputA pointer */
92 q31_t
*pIn2
; /* inputB pointer */
93 q31_t
*pOut
= pDst
; /* output pointer */
94 q31_t
*px
; /* Intermediate inputA pointer */
95 q31_t
*py
; /* Intermediate inputB pointer */
96 q31_t
*pSrc1
, *pSrc2
; /* Intermediate pointers */
97 q63_t sum
; /* Accumulator */
98 q63_t acc0
, acc1
, acc2
; /* Accumulator */
99 q31_t x0
, x1
, x2
, c0
; /* Temporary variables to hold state and coefficient values */
100 uint32_t j
, k
, count
, blkCnt
, blockSize1
, blockSize2
, blockSize3
; /* loop counter */
102 /* The algorithm implementation is based on the lengths of the inputs. */
103 /* srcB is always made to slide across srcA. */
104 /* So srcBLen is always considered as shorter or equal to srcALen */
105 if(srcALen
>= srcBLen
)
107 /* Initialization of inputA pointer */
110 /* Initialization of inputB pointer */
115 /* Initialization of inputA pointer */
116 pIn1
= (q31_t
*) pSrcB
;
118 /* Initialization of inputB pointer */
119 pIn2
= (q31_t
*) pSrcA
;
121 /* srcBLen is always considered as shorter or equal to srcALen */
127 /* conv(x,y) at n = x[n] * y[0] + x[n-1] * y[1] + x[n-2] * y[2] + ...+ x[n-N+1] * y[N -1] */
128 /* The function is internally
129 * divided into three stages according to the number of multiplications that has to be
130 * taken place between inputA samples and inputB samples. In the first stage of the
131 * algorithm, the multiplications increase by one for every iteration.
132 * In the second stage of the algorithm, srcBLen number of multiplications are done.
133 * In the third stage of the algorithm, the multiplications decrease by one
134 * for every iteration. */
136 /* The algorithm is implemented in three stages.
137 The loop counters of each stage is initiated here. */
138 blockSize1
= srcBLen
- 1u;
139 blockSize2
= srcALen
- (srcBLen
- 1u);
140 blockSize3
= blockSize1
;
142 /* --------------------------
143 * Initializations of stage1
144 * -------------------------*/
147 * sum = x[0] * y[1] + x[1] * y[0]
149 * sum = x[0] * y[srcBlen - 1] + x[1] * y[srcBlen - 2] +...+ x[srcBLen - 1] * y[0]
152 /* In this stage the MAC operations are increased by 1 for every iteration.
153 The count variable holds the number of MAC operations performed */
156 /* Working pointer of inputA */
159 /* Working pointer of inputB */
163 /* ------------------------
165 * ----------------------*/
167 /* The first stage starts here */
168 while(blockSize1
> 0u)
170 /* Accumulator is made zero for every iteration */
173 /* Apply loop unrolling and compute 4 MACs simultaneously. */
176 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
177 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
180 /* x[0] * y[srcBLen - 1] */
181 sum
+= (q63_t
) * px
++ * (*py
--);
182 /* x[1] * y[srcBLen - 2] */
183 sum
+= (q63_t
) * px
++ * (*py
--);
184 /* x[2] * y[srcBLen - 3] */
185 sum
+= (q63_t
) * px
++ * (*py
--);
186 /* x[3] * y[srcBLen - 4] */
187 sum
+= (q63_t
) * px
++ * (*py
--);
189 /* Decrement the loop counter */
193 /* If the count is not a multiple of 4, compute any remaining MACs here.
194 ** No loop unrolling is used. */
199 /* Perform the multiply-accumulate */
200 sum
+= (q63_t
) * px
++ * (*py
--);
202 /* Decrement the loop counter */
206 /* Store the result in the accumulator in the destination buffer. */
207 *pOut
++ = (q31_t
) (sum
>> 31);
209 /* Update the inputA and inputB pointers for next MAC calculation */
213 /* Increment the MAC count */
216 /* Decrement the loop counter */
220 /* --------------------------
221 * Initializations of stage2
222 * ------------------------*/
224 /* sum = x[0] * y[srcBLen-1] + x[1] * y[srcBLen-2] +...+ x[srcBLen-1] * y[0]
225 * sum = x[1] * y[srcBLen-1] + x[2] * y[srcBLen-2] +...+ x[srcBLen] * y[0]
227 * sum = x[srcALen-srcBLen-2] * y[srcBLen-1] + x[srcALen] * y[srcBLen-2] +...+ x[srcALen-1] * y[0]
230 /* Working pointer of inputA */
233 /* Working pointer of inputB */
234 pSrc2
= pIn2
+ (srcBLen
- 1u);
237 /* count is index by which the pointer pIn1 to be incremented */
240 /* -------------------
242 * ------------------*/
244 /* Stage2 depends on srcBLen as in this stage srcBLen number of MACS are performed.
245 * So, to loop unroll over blockSize2,
246 * srcBLen should be greater than or equal to 4 */
249 /* Loop unroll by 3 */
250 blkCnt
= blockSize2
/ 3;
254 /* Set all accumulators to zero */
259 /* read x[0], x[1], x[2] samples */
263 /* Apply loop unrolling and compute 3 MACs simultaneously. */
266 /* First part of the processing with loop unrolling. Compute 3 MACs at a time.
267 ** a second loop below computes MACs for the remaining 1 to 2 samples. */
270 /* Read y[srcBLen - 1] sample */
273 /* Read x[3] sample */
276 /* Perform the multiply-accumulates */
277 /* acc0 += x[0] * y[srcBLen - 1] */
278 acc0
+= ((q63_t
) x0
* c0
);
279 /* acc1 += x[1] * y[srcBLen - 1] */
280 acc1
+= ((q63_t
) x1
* c0
);
281 /* acc2 += x[2] * y[srcBLen - 1] */
282 acc2
+= ((q63_t
) x2
* c0
);
284 /* Read y[srcBLen - 2] sample */
287 /* Read x[4] sample */
290 /* Perform the multiply-accumulate */
291 /* acc0 += x[1] * y[srcBLen - 2] */
292 acc0
+= ((q63_t
) x1
* c0
);
293 /* acc1 += x[2] * y[srcBLen - 2] */
294 acc1
+= ((q63_t
) x2
* c0
);
295 /* acc2 += x[3] * y[srcBLen - 2] */
296 acc2
+= ((q63_t
) x0
* c0
);
298 /* Read y[srcBLen - 3] sample */
301 /* Read x[5] sample */
304 /* Perform the multiply-accumulates */
305 /* acc0 += x[2] * y[srcBLen - 3] */
306 acc0
+= ((q63_t
) x2
* c0
);
307 /* acc1 += x[3] * y[srcBLen - 2] */
308 acc1
+= ((q63_t
) x0
* c0
);
309 /* acc2 += x[4] * y[srcBLen - 2] */
310 acc2
+= ((q63_t
) x1
* c0
);
312 /* update scratch pointers */
318 /* If the srcBLen is not a multiple of 3, compute any remaining MACs here.
319 ** No loop unrolling is used. */
320 k
= srcBLen
- (3 * (srcBLen
/ 3));
324 /* Read y[srcBLen - 5] sample */
327 /* Read x[7] sample */
330 /* Perform the multiply-accumulates */
331 /* acc0 += x[4] * y[srcBLen - 5] */
332 acc0
+= ((q63_t
) x0
* c0
);
333 /* acc1 += x[5] * y[srcBLen - 5] */
334 acc1
+= ((q63_t
) x1
* c0
);
335 /* acc2 += x[6] * y[srcBLen - 5] */
336 acc2
+= ((q63_t
) x2
* c0
);
338 /* Reuse the present samples for the next MAC */
342 /* Decrement the loop counter */
346 /* Store the results in the accumulators in the destination buffer. */
347 *pOut
++ = (q31_t
) (acc0
>> 31);
348 *pOut
++ = (q31_t
) (acc1
>> 31);
349 *pOut
++ = (q31_t
) (acc2
>> 31);
351 /* Increment the pointer pIn1 index, count by 3 */
354 /* Update the inputA and inputB pointers for next MAC calculation */
358 /* Decrement the loop counter */
362 /* If the blockSize2 is not a multiple of 3, compute any remaining output samples here.
363 ** No loop unrolling is used. */
364 blkCnt
= blockSize2
- 3 * (blockSize2
/ 3);
368 /* Accumulator is made zero for every iteration */
371 /* Apply loop unrolling and compute 4 MACs simultaneously. */
374 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
375 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
378 /* Perform the multiply-accumulates */
379 sum
+= (q63_t
) * px
++ * (*py
--);
380 sum
+= (q63_t
) * px
++ * (*py
--);
381 sum
+= (q63_t
) * px
++ * (*py
--);
382 sum
+= (q63_t
) * px
++ * (*py
--);
384 /* Decrement the loop counter */
388 /* If the srcBLen is not a multiple of 4, compute any remaining MACs here.
389 ** No loop unrolling is used. */
394 /* Perform the multiply-accumulate */
395 sum
+= (q63_t
) * px
++ * (*py
--);
397 /* Decrement the loop counter */
401 /* Store the result in the accumulator in the destination buffer. */
402 *pOut
++ = (q31_t
) (sum
>> 31);
404 /* Increment the MAC count */
407 /* Update the inputA and inputB pointers for next MAC calculation */
411 /* Decrement the loop counter */
417 /* If the srcBLen is not a multiple of 4,
418 * the blockSize2 loop cannot be unrolled by 4 */
423 /* Accumulator is made zero for every iteration */
426 /* srcBLen number of MACS should be performed */
431 /* Perform the multiply-accumulate */
432 sum
+= (q63_t
) * px
++ * (*py
--);
434 /* Decrement the loop counter */
438 /* Store the result in the accumulator in the destination buffer. */
439 *pOut
++ = (q31_t
) (sum
>> 31);
441 /* Increment the MAC count */
444 /* Update the inputA and inputB pointers for next MAC calculation */
448 /* Decrement the loop counter */
454 /* --------------------------
455 * Initializations of stage3
456 * -------------------------*/
458 /* sum += x[srcALen-srcBLen+1] * y[srcBLen-1] + x[srcALen-srcBLen+2] * y[srcBLen-2] +...+ x[srcALen-1] * y[1]
459 * sum += x[srcALen-srcBLen+2] * y[srcBLen-1] + x[srcALen-srcBLen+3] * y[srcBLen-2] +...+ x[srcALen-1] * y[2]
461 * sum += x[srcALen-2] * y[srcBLen-1] + x[srcALen-1] * y[srcBLen-2]
462 * sum += x[srcALen-1] * y[srcBLen-1]
465 /* In this stage the MAC operations are decreased by 1 for every iteration.
466 The blockSize3 variable holds the number of MAC operations performed */
468 /* Working pointer of inputA */
469 pSrc1
= (pIn1
+ srcALen
) - (srcBLen
- 1u);
472 /* Working pointer of inputB */
473 pSrc2
= pIn2
+ (srcBLen
- 1u);
476 /* -------------------
478 * ------------------*/
480 while(blockSize3
> 0u)
482 /* Accumulator is made zero for every iteration */
485 /* Apply loop unrolling and compute 4 MACs simultaneously. */
486 k
= blockSize3
>> 2u;
488 /* First part of the processing with loop unrolling. Compute 4 MACs at a time.
489 ** a second loop below computes MACs for the remaining 1 to 3 samples. */
492 /* sum += x[srcALen - srcBLen + 1] * y[srcBLen - 1] */
493 sum
+= (q63_t
) * px
++ * (*py
--);
494 /* sum += x[srcALen - srcBLen + 2] * y[srcBLen - 2] */
495 sum
+= (q63_t
) * px
++ * (*py
--);
496 /* sum += x[srcALen - srcBLen + 3] * y[srcBLen - 3] */
497 sum
+= (q63_t
) * px
++ * (*py
--);
498 /* sum += x[srcALen - srcBLen + 4] * y[srcBLen - 4] */
499 sum
+= (q63_t
) * px
++ * (*py
--);
501 /* Decrement the loop counter */
505 /* If the blockSize3 is not a multiple of 4, compute any remaining MACs here.
506 ** No loop unrolling is used. */
507 k
= blockSize3
% 0x4u
;
511 /* Perform the multiply-accumulate */
512 sum
+= (q63_t
) * px
++ * (*py
--);
514 /* Decrement the loop counter */
518 /* Store the result in the accumulator in the destination buffer. */
519 *pOut
++ = (q31_t
) (sum
>> 31);
521 /* Update the inputA and inputB pointers for next MAC calculation */
525 /* Decrement the loop counter */
531 /* Run the below code for Cortex-M0 */
533 q31_t
*pIn1
= pSrcA
; /* input pointer */
534 q31_t
*pIn2
= pSrcB
; /* coefficient pointer */
535 q63_t sum
; /* Accumulator */
536 uint32_t i
, j
; /* loop counter */
538 /* Loop to calculate output of convolution for output length number of times */
539 for (i
= 0; i
< (srcALen
+ srcBLen
- 1); i
++)
541 /* Initialize sum with zero to carry on MAC operations */
544 /* Loop to perform MAC operations according to convolution equation */
545 for (j
= 0; j
<= i
; j
++)
547 /* Check the array limitations */
548 if(((i
- j
) < srcBLen
) && (j
< srcALen
))
550 /* z[i] += x[i-j] * y[j] */
551 sum
+= ((q63_t
) pIn1
[j
] * (pIn2
[i
- j
]));
555 /* Store the output in the destination buffer */
556 pDst
[i
] = (q31_t
) (sum
>> 31u);
559 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
564 * @} end of Conv group