arm_mult_q15.c
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 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 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 (&pSrcA); 127 /* read 2 samples at a time from sourceB */ 128 inB1 = read_q15x2_ia (&pSrcB); 129 /* read 2 samples at a time from sourceA */ 130 inA2 = read_q15x2_ia (&pSrcA); 131 /* read 2 samples at a time from sourceB */ 132 inB2 = read_q15x2_ia (&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 */