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