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