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
arm_shift_q15(const q15_t * pSrc,int8_t shiftBits,q15_t * pDst,uint32_t blockSize)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
arm_shift_q15(const q15_t * pSrc,int8_t shiftBits,q15_t * pDst,uint32_t blockSize)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 */
252