1 /* ----------------------------------------------------------------------
2 * Project: CMSIS DSP Library
3 * Title: arm_mult_q15.c
4 * Description: Q15 vector multiplication
5 *
6 * $Date: 23 April 2021
7 * $Revision: V1.9.0
8 *
9 * Target Processor: Cortex-M and Cortex-A cores
10 * -------------------------------------------------------------------- */
11 /*
12 * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
13 *
14 * SPDX-License-Identifier: Apache-2.0
15 *
16 * Licensed under the Apache License, Version 2.0 (the License); you may
17 * not use this file except in compliance with the License.
18 * You may obtain a copy of the License at
19 *
20 * www.apache.org/licenses/LICENSE-2.0
21 *
22 * Unless required by applicable law or agreed to in writing, software
23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
25 * See the License for the specific language governing permissions and
26 * limitations under the License.
27 */
28
29 #include "dsp/basic_math_functions.h"
30
31 /**
32 @ingroup groupMath
33 */
34
35 /**
36 @addtogroup BasicMult
37 @{
38 */
39
40 /**
41 @brief Q15 vector multiplication
42 @param[in] pSrcA points to first input vector
43 @param[in] pSrcB points to second input vector
44 @param[out] pDst points to output vector
45 @param[in] blockSize number of samples in each vector
46 @return none
47
48 @par Scaling and Overflow Behavior
49 The function uses saturating arithmetic.
50 Results outside of the allowable Q15 range [0x8000 0x7FFF] are saturated.
51 */
52 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
53
54 #include "arm_helium_utils.h"
55
arm_mult_q15(const q15_t * pSrcA,const q15_t * pSrcB,q15_t * pDst,uint32_t blockSize)56 void arm_mult_q15(
57 const q15_t * pSrcA,
58 const q15_t * pSrcB,
59 q15_t * pDst,
60 uint32_t blockSize)
61 {
62 uint32_t blkCnt; /* loop counters */
63 q15x8_t vecA, vecB;
64
65 /* Compute 8 outputs at a time */
66 blkCnt = blockSize >> 3;
67 while (blkCnt > 0U)
68 {
69 /*
70 * C = A * B
71 * Multiply the inputs and then store the results in the destination buffer.
72 */
73 vecA = vld1q(pSrcA);
74 vecB = vld1q(pSrcB);
75 vst1q(pDst, vqdmulhq(vecA, vecB));
76 /*
77 * Decrement the blockSize loop counter
78 */
79 blkCnt--;
80 /*
81 * advance vector source and destination pointers
82 */
83 pSrcA += 8;
84 pSrcB += 8;
85 pDst += 8;
86 }
87 /*
88 * tail
89 */
90 blkCnt = blockSize & 7;
91 if (blkCnt > 0U)
92 {
93 mve_pred16_t p0 = vctp16q(blkCnt);
94 vecA = vld1q(pSrcA);
95 vecB = vld1q(pSrcB);
96 vstrhq_p(pDst, vqdmulhq(vecA, vecB), p0);
97 }
98 }
99
100 #else
arm_mult_q15(const q15_t * pSrcA,const q15_t * pSrcB,q15_t * pDst,uint32_t blockSize)101 void arm_mult_q15(
102 const q15_t * pSrcA,
103 const q15_t * pSrcB,
104 q15_t * pDst,
105 uint32_t blockSize)
106 {
107 uint32_t blkCnt; /* Loop counter */
108
109 #if defined (ARM_MATH_LOOPUNROLL)
110
111 #if defined (ARM_MATH_DSP)
112 q31_t inA1, inA2, inB1, inB2; /* Temporary input variables */
113 q15_t out1, out2, out3, out4; /* Temporary output variables */
114 q31_t mul1, mul2, mul3, mul4; /* Temporary variables */
115 #endif
116
117 /* Loop unrolling: Compute 4 outputs at a time */
118 blkCnt = blockSize >> 2U;
119
120 while (blkCnt > 0U)
121 {
122 /* C = A * B */
123
124 #if defined (ARM_MATH_DSP)
125 /* read 2 samples at a time from sourceA */
126 inA1 = read_q15x2_ia ((q15_t **) &pSrcA);
127 /* read 2 samples at a time from sourceB */
128 inB1 = read_q15x2_ia ((q15_t **) &pSrcB);
129 /* read 2 samples at a time from sourceA */
130 inA2 = read_q15x2_ia ((q15_t **) &pSrcA);
131 /* read 2 samples at a time from sourceB */
132 inB2 = read_q15x2_ia ((q15_t **) &pSrcB);
133
134 /* multiply mul = sourceA * sourceB */
135 mul1 = (q31_t) ((q15_t) (inA1 >> 16) * (q15_t) (inB1 >> 16));
136 mul2 = (q31_t) ((q15_t) (inA1 ) * (q15_t) (inB1 ));
137 mul3 = (q31_t) ((q15_t) (inA2 >> 16) * (q15_t) (inB2 >> 16));
138 mul4 = (q31_t) ((q15_t) (inA2 ) * (q15_t) (inB2 ));
139
140 /* saturate result to 16 bit */
141 out1 = (q15_t) __SSAT(mul1 >> 15, 16);
142 out2 = (q15_t) __SSAT(mul2 >> 15, 16);
143 out3 = (q15_t) __SSAT(mul3 >> 15, 16);
144 out4 = (q15_t) __SSAT(mul4 >> 15, 16);
145
146 /* store result to destination */
147 #ifndef ARM_MATH_BIG_ENDIAN
148 write_q15x2_ia (&pDst, __PKHBT(out2, out1, 16));
149 write_q15x2_ia (&pDst, __PKHBT(out4, out3, 16));
150 #else
151 write_q15x2_ia (&pDst, __PKHBT(out1, out2, 16));
152 write_q15x2_ia (&pDst, __PKHBT(out3, out4, 16));
153 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
154
155 #else
156 *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
157 *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
158 *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
159 *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
160 #endif
161
162 /* Decrement loop counter */
163 blkCnt--;
164 }
165
166 /* Loop unrolling: Compute remaining outputs */
167 blkCnt = blockSize % 0x4U;
168
169 #else
170
171 /* Initialize blkCnt with number of samples */
172 blkCnt = blockSize;
173
174 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
175
176 while (blkCnt > 0U)
177 {
178 /* C = A * B */
179
180 /* Multiply inputs and store result in destination buffer. */
181 *pDst++ = (q15_t) __SSAT((((q31_t) (*pSrcA++) * (*pSrcB++)) >> 15), 16);
182
183 /* Decrement loop counter */
184 blkCnt--;
185 }
186
187 }
188 #endif /* defined(ARM_MATH_MVEI) */
189
190 /**
191 @} end of BasicMult group
192 */
193
