arm_shift_q15.c
1 /* ---------------------------------------------------------------------- 2 * Project: CMSIS DSP Library 3 * Title: arm_shift_q15.c 4 * Description: Shifts the elements of a Q15 vector by a specified number of bits 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 BasicShift 37 @{ 38 */ 39 40 /** 41 @brief Shifts the elements of a Q15 vector a specified number of bits 42 @param[in] pSrc points to the input vector 43 @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. 44 @param[out] pDst points to the 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 53 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) 54 55 #include "arm_helium_utils.h" 56 57 void arm_shift_q15( 58 const q15_t * pSrc, 59 int8_t shiftBits, 60 q15_t * pDst, 61 uint32_t blockSize) 62 { 63 uint32_t blkCnt; /* loop counters */ 64 q15x8_t vecSrc; 65 q15x8_t vecDst; 66 67 /* Compute 8 outputs at a time */ 68 blkCnt = blockSize >> 3; 69 while (blkCnt > 0U) 70 { 71 /* 72 * C = A (>> or <<) shiftBits 73 * Shift the input and then store the result in the destination buffer. 74 */ 75 vecSrc = vld1q(pSrc); 76 vecDst = vqshlq_r(vecSrc, shiftBits); 77 vst1q(pDst, vecDst); 78 /* 79 * Decrement the blockSize loop counter 80 */ 81 blkCnt--; 82 /* 83 * advance vector source and destination pointers 84 */ 85 pSrc += 8; 86 pDst += 8; 87 } 88 /* 89 * tail 90 */ 91 blkCnt = blockSize & 7; 92 if (blkCnt > 0U) 93 { 94 mve_pred16_t p0 = vctp16q(blkCnt); 95 vecSrc = vld1q(pSrc); 96 vecDst = vqshlq_r(vecSrc, shiftBits); 97 vstrhq_p(pDst, vecDst, p0); 98 } 99 } 100 101 #else 102 void arm_shift_q15( 103 const q15_t * pSrc, 104 int8_t shiftBits, 105 q15_t * pDst, 106 uint32_t blockSize) 107 { 108 uint32_t blkCnt; /* Loop counter */ 109 uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */ 110 111 #if defined (ARM_MATH_LOOPUNROLL) 112 113 #if defined (ARM_MATH_DSP) 114 q15_t in1, in2; /* Temporary input variables */ 115 #endif 116 117 /* Loop unrolling: Compute 4 outputs at a time */ 118 blkCnt = blockSize >> 2U; 119 120 /* If the shift value is positive then do right shift else left shift */ 121 if (sign == 0U) 122 { 123 while (blkCnt > 0U) 124 { 125 /* C = A << shiftBits */ 126 127 #if defined (ARM_MATH_DSP) 128 /* read 2 samples from source */ 129 in1 = *pSrc++; 130 in2 = *pSrc++; 131 132 /* Shift the inputs and then store the results in the destination buffer. */ 133 #ifndef ARM_MATH_BIG_ENDIAN 134 write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in1 << shiftBits), 16), 135 __SSAT(((q31_t) in2 << shiftBits), 16), 16)); 136 #else 137 write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in2 << shiftBits), 16), 138 __SSAT(((q31_t) in1 << shiftBits), 16), 16)); 139 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 140 141 /* read 2 samples from source */ 142 in1 = *pSrc++; 143 in2 = *pSrc++; 144 145 #ifndef ARM_MATH_BIG_ENDIAN 146 write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in1 << shiftBits), 16), 147 __SSAT(((q31_t) in2 << shiftBits), 16), 16)); 148 #else 149 write_q15x2_ia (&pDst, __PKHBT(__SSAT(((q31_t) in2 << shiftBits), 16), 150 __SSAT(((q31_t) in1 << shiftBits), 16), 16)); 151 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 152 153 #else 154 *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); 155 *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); 156 *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); 157 *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); 158 #endif 159 160 /* Decrement loop counter */ 161 blkCnt--; 162 } 163 } 164 else 165 { 166 while (blkCnt > 0U) 167 { 168 /* C = A >> shiftBits */ 169 170 #if defined (ARM_MATH_DSP) 171 /* read 2 samples from source */ 172 in1 = *pSrc++; 173 in2 = *pSrc++; 174 175 /* Shift the inputs and then store the results in the destination buffer. */ 176 #ifndef ARM_MATH_BIG_ENDIAN 177 write_q15x2_ia (&pDst, __PKHBT((in1 >> -shiftBits), 178 (in2 >> -shiftBits), 16)); 179 #else 180 write_q15x2_ia (&pDst, __PKHBT((in2 >> -shiftBits), 181 (in1 >> -shiftBits), 16)); 182 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 183 184 /* read 2 samples from source */ 185 in1 = *pSrc++; 186 in2 = *pSrc++; 187 188 #ifndef ARM_MATH_BIG_ENDIAN 189 write_q15x2_ia (&pDst, __PKHBT((in1 >> -shiftBits), 190 (in2 >> -shiftBits), 16)); 191 #else 192 write_q15x2_ia (&pDst, __PKHBT((in2 >> -shiftBits), 193 (in1 >> -shiftBits), 16)); 194 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 195 196 #else 197 *pDst++ = (*pSrc++ >> -shiftBits); 198 *pDst++ = (*pSrc++ >> -shiftBits); 199 *pDst++ = (*pSrc++ >> -shiftBits); 200 *pDst++ = (*pSrc++ >> -shiftBits); 201 #endif 202 203 /* Decrement loop counter */ 204 blkCnt--; 205 } 206 } 207 208 /* Loop unrolling: Compute remaining outputs */ 209 blkCnt = blockSize % 0x4U; 210 211 #else 212 213 /* Initialize blkCnt with number of samples */ 214 blkCnt = blockSize; 215 216 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 217 218 /* If the shift value is positive then do right shift else left shift */ 219 if (sign == 0U) 220 { 221 while (blkCnt > 0U) 222 { 223 /* C = A << shiftBits */ 224 225 /* Shift input and store result in destination buffer. */ 226 *pDst++ = __SSAT(((q31_t) *pSrc++ << shiftBits), 16); 227 228 /* Decrement loop counter */ 229 blkCnt--; 230 } 231 } 232 else 233 { 234 while (blkCnt > 0U) 235 { 236 /* C = A >> shiftBits */ 237 238 /* Shift input and store result in destination buffer. */ 239 *pDst++ = (*pSrc++ >> -shiftBits); 240 241 /* Decrement loop counter */ 242 blkCnt--; 243 } 244 } 245 246 } 247 #endif /* defined(ARM_MATH_MVEI) */ 248 249 /** 250 @} end of BasicShift group 251 */