arm_shift_q31.c
1 /* ---------------------------------------------------------------------- 2 * Project: CMSIS DSP Library 3 * Title: arm_shift_q31.c 4 * Description: Shifts the elements of a Q31 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 @defgroup BasicShift Vector Shift 36 37 Shifts the elements of a fixed-point vector by a specified number of bits. 38 There are separate functions for Q7, Q15, and Q31 data types. 39 The underlying algorithm used is: 40 41 <pre> 42 pDst[n] = pSrc[n] << shift, 0 <= n < blockSize. 43 </pre> 44 45 If <code>shift</code> is positive then the elements of the vector are shifted to the left. 46 If <code>shift</code> is negative then the elements of the vector are shifted to the right. 47 48 The functions support in-place computation allowing the source and destination 49 pointers to reference the same memory buffer. 50 */ 51 52 /** 53 @addtogroup BasicShift 54 @{ 55 */ 56 57 /** 58 @brief Shifts the elements of a Q31 vector a specified number of bits. 59 @param[in] pSrc points to the input vector 60 @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right. 61 @param[out] pDst points to the output vector 62 @param[in] blockSize number of samples in the vector 63 @return none 64 65 @par Scaling and Overflow Behavior 66 The function uses saturating arithmetic. 67 Results outside of the allowable Q31 range [0x80000000 0x7FFFFFFF] are saturated. 68 */ 69 70 #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE) 71 72 #include "arm_helium_utils.h" 73 74 void arm_shift_q31( 75 const q31_t * pSrc, 76 int8_t shiftBits, 77 q31_t * pDst, 78 uint32_t blockSize) 79 { 80 uint32_t blkCnt; /* loop counters */ 81 q31x4_t vecSrc; 82 q31x4_t vecDst; 83 84 /* Compute 4 outputs at a time */ 85 blkCnt = blockSize >> 2; 86 while (blkCnt > 0U) 87 { 88 /* 89 * C = A (>> or <<) shiftBits 90 * Shift the input and then store the result in the destination buffer. 91 */ 92 vecSrc = vld1q((q31_t const *) pSrc); 93 vecDst = vqshlq_r(vecSrc, shiftBits); 94 vst1q(pDst, vecDst); 95 /* 96 * Decrement the blockSize loop counter 97 */ 98 blkCnt--; 99 /* 100 * advance vector source and destination pointers 101 */ 102 pSrc += 4; 103 pDst += 4; 104 } 105 /* 106 * tail 107 */ 108 blkCnt = blockSize & 3; 109 if (blkCnt > 0U) 110 { 111 mve_pred16_t p0 = vctp32q(blkCnt); 112 vecSrc = vld1q((q31_t const *) pSrc); 113 vecDst = vqshlq_r(vecSrc, shiftBits); 114 vstrwq_p(pDst, vecDst, p0); 115 } 116 } 117 118 119 #else 120 void arm_shift_q31( 121 const q31_t * pSrc, 122 int8_t shiftBits, 123 q31_t * pDst, 124 uint32_t blockSize) 125 { 126 uint32_t blkCnt; /* Loop counter */ 127 uint8_t sign = (shiftBits & 0x80); /* Sign of shiftBits */ 128 129 #if defined (ARM_MATH_LOOPUNROLL) 130 131 q31_t in, out; /* Temporary variables */ 132 133 /* Loop unrolling: Compute 4 outputs at a time */ 134 blkCnt = blockSize >> 2U; 135 136 /* If the shift value is positive then do right shift else left shift */ 137 if (sign == 0U) 138 { 139 while (blkCnt > 0U) 140 { 141 /* C = A << shiftBits */ 142 143 /* Shift input and store result in destination buffer. */ 144 in = *pSrc++; 145 out = in << shiftBits; 146 if (in != (out >> shiftBits)) 147 out = 0x7FFFFFFF ^ (in >> 31); 148 *pDst++ = out; 149 150 in = *pSrc++; 151 out = in << shiftBits; 152 if (in != (out >> shiftBits)) 153 out = 0x7FFFFFFF ^ (in >> 31); 154 *pDst++ = out; 155 156 in = *pSrc++; 157 out = in << shiftBits; 158 if (in != (out >> shiftBits)) 159 out = 0x7FFFFFFF ^ (in >> 31); 160 *pDst++ = out; 161 162 in = *pSrc++; 163 out = in << shiftBits; 164 if (in != (out >> shiftBits)) 165 out = 0x7FFFFFFF ^ (in >> 31); 166 *pDst++ = out; 167 168 /* Decrement loop counter */ 169 blkCnt--; 170 } 171 } 172 else 173 { 174 while (blkCnt > 0U) 175 { 176 /* C = A >> shiftBits */ 177 178 /* Shift input and store results in destination buffer. */ 179 *pDst++ = (*pSrc++ >> -shiftBits); 180 *pDst++ = (*pSrc++ >> -shiftBits); 181 *pDst++ = (*pSrc++ >> -shiftBits); 182 *pDst++ = (*pSrc++ >> -shiftBits); 183 184 /* Decrement loop counter */ 185 blkCnt--; 186 } 187 } 188 189 /* Loop unrolling: Compute remaining outputs */ 190 blkCnt = blockSize % 0x4U; 191 192 #else 193 194 /* Initialize blkCnt with number of samples */ 195 blkCnt = blockSize; 196 197 #endif /* #if defined (ARM_MATH_LOOPUNROLL) */ 198 199 /* If the shift value is positive then do right shift else left shift */ 200 if (sign == 0U) 201 { 202 while (blkCnt > 0U) 203 { 204 /* C = A << shiftBits */ 205 206 /* Shift input and store result in destination buffer. */ 207 *pDst++ = clip_q63_to_q31((q63_t) *pSrc++ << shiftBits); 208 209 /* Decrement loop counter */ 210 blkCnt--; 211 } 212 } 213 else 214 { 215 while (blkCnt > 0U) 216 { 217 /* C = A >> shiftBits */ 218 219 /* Shift input and store result in destination buffer. */ 220 *pDst++ = (*pSrc++ >> -shiftBits); 221 222 /* Decrement loop counter */ 223 blkCnt--; 224 } 225 } 226 227 } 228 #endif /* defined(ARM_MATH_MVEI) */ 229 230 /** 231 @} end of BasicShift group 232 */