arm_shift_q15.c 7.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248
  1. /* ----------------------------------------------------------------------
  2. * Copyright (C) 2010-2014 ARM Limited. All rights reserved.
  3. *
  4. * $Date: 12. March 2014
  5. * $Revision: V1.4.4
  6. *
  7. * Project: CMSIS DSP Library
  8. * Title: arm_shift_q15.c
  9. *
  10. * Description: Shifts the elements of a Q15 vector by a specified number of bits.
  11. *
  12. * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13. *
  14. * Redistribution and use in source and binary forms, with or without
  15. * modification, are permitted provided that the following conditions
  16. * are met:
  17. * - Redistributions of source code must retain the above copyright
  18. * notice, this list of conditions and the following disclaimer.
  19. * - Redistributions in binary form must reproduce the above copyright
  20. * notice, this list of conditions and the following disclaimer in
  21. * the documentation and/or other materials provided with the
  22. * distribution.
  23. * - Neither the name of ARM LIMITED nor the names of its contributors
  24. * may be used to endorse or promote products derived from this
  25. * software without specific prior written permission.
  26. *
  27. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38. * POSSIBILITY OF SUCH DAMAGE.
  39. * -------------------------------------------------------------------- */
  40. #include "arm_math.h"
  41. /**
  42. * @ingroup groupMath
  43. */
  44. /**
  45. * @addtogroup shift
  46. * @{
  47. */
  48. /**
  49. * @brief Shifts the elements of a Q15 vector a specified number of bits.
  50. * @param[in] *pSrc points to the input vector
  51. * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
  52. * @param[out] *pDst points to the output vector
  53. * @param[in] blockSize number of samples in the vector
  54. * @return none.
  55. *
  56. * <b>Scaling and Overflow Behavior:</b>
  57. * \par
  58. * The function uses saturating arithmetic.
  59. * Results outside of the allowable Q15 range [0x8000 0x7FFF] will be saturated.
  60. */
  61. void arm_shift_q15(
  62. q15_t * pSrc,
  63. int8_t shiftBits,
  64. q15_t * pDst,
  65. uint32_t blockSize)
  66. {
  67. uint32_t blkCnt; /* loop counter */
  68. uint8_t sign; /* Sign of shiftBits */
  69. #ifndef ARM_MATH_CM0_FAMILY
  70. /* Run the below code for Cortex-M4 and Cortex-M3 */
  71. q15_t in1, in2; /* Temporary variables */
  72. /*loop Unrolling */
  73. blkCnt = blockSize >> 2u;
  74. /* Getting the sign of shiftBits */
  75. sign = (shiftBits & 0x80);
  76. /* If the shift value is positive then do right shift else left shift */
  77. if(sign == 0u)
  78. {
  79. /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
  80. ** a second loop below computes the remaining 1 to 3 samples. */
  81. while(blkCnt > 0u)
  82. {
  83. /* Read 2 inputs */
  84. in1 = *pSrc++;
  85. in2 = *pSrc++;
  86. /* C = A << shiftBits */
  87. /* Shift the inputs and then store the results in the destination buffer. */
  88. #ifndef ARM_MATH_BIG_ENDIAN
  89. *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
  90. __SSAT((in2 << shiftBits), 16), 16);
  91. #else
  92. *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
  93. __SSAT((in1 << shiftBits), 16), 16);
  94. #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
  95. in1 = *pSrc++;
  96. in2 = *pSrc++;
  97. #ifndef ARM_MATH_BIG_ENDIAN
  98. *__SIMD32(pDst)++ = __PKHBT(__SSAT((in1 << shiftBits), 16),
  99. __SSAT((in2 << shiftBits), 16), 16);
  100. #else
  101. *__SIMD32(pDst)++ = __PKHBT(__SSAT((in2 << shiftBits), 16),
  102. __SSAT((in1 << shiftBits), 16), 16);
  103. #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
  104. /* Decrement the loop counter */
  105. blkCnt--;
  106. }
  107. /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
  108. ** No loop unrolling is used. */
  109. blkCnt = blockSize % 0x4u;
  110. while(blkCnt > 0u)
  111. {
  112. /* C = A << shiftBits */
  113. /* Shift and then store the results in the destination buffer. */
  114. *pDst++ = __SSAT((*pSrc++ << shiftBits), 16);
  115. /* Decrement the loop counter */
  116. blkCnt--;
  117. }
  118. }
  119. else
  120. {
  121. /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
  122. ** a second loop below computes the remaining 1 to 3 samples. */
  123. while(blkCnt > 0u)
  124. {
  125. /* Read 2 inputs */
  126. in1 = *pSrc++;
  127. in2 = *pSrc++;
  128. /* C = A >> shiftBits */
  129. /* Shift the inputs and then store the results in the destination buffer. */
  130. #ifndef ARM_MATH_BIG_ENDIAN
  131. *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
  132. (in2 >> -shiftBits), 16);
  133. #else
  134. *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
  135. (in1 >> -shiftBits), 16);
  136. #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
  137. in1 = *pSrc++;
  138. in2 = *pSrc++;
  139. #ifndef ARM_MATH_BIG_ENDIAN
  140. *__SIMD32(pDst)++ = __PKHBT((in1 >> -shiftBits),
  141. (in2 >> -shiftBits), 16);
  142. #else
  143. *__SIMD32(pDst)++ = __PKHBT((in2 >> -shiftBits),
  144. (in1 >> -shiftBits), 16);
  145. #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
  146. /* Decrement the loop counter */
  147. blkCnt--;
  148. }
  149. /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
  150. ** No loop unrolling is used. */
  151. blkCnt = blockSize % 0x4u;
  152. while(blkCnt > 0u)
  153. {
  154. /* C = A >> shiftBits */
  155. /* Shift the inputs and then store the results in the destination buffer. */
  156. *pDst++ = (*pSrc++ >> -shiftBits);
  157. /* Decrement the loop counter */
  158. blkCnt--;
  159. }
  160. }
  161. #else
  162. /* Run the below code for Cortex-M0 */
  163. /* Getting the sign of shiftBits */
  164. sign = (shiftBits & 0x80);
  165. /* If the shift value is positive then do right shift else left shift */
  166. if(sign == 0u)
  167. {
  168. /* Initialize blkCnt with number of samples */
  169. blkCnt = blockSize;
  170. while(blkCnt > 0u)
  171. {
  172. /* C = A << shiftBits */
  173. /* Shift and then store the results in the destination buffer. */
  174. *pDst++ = __SSAT(((q31_t) * pSrc++ << shiftBits), 16);
  175. /* Decrement the loop counter */
  176. blkCnt--;
  177. }
  178. }
  179. else
  180. {
  181. /* Initialize blkCnt with number of samples */
  182. blkCnt = blockSize;
  183. while(blkCnt > 0u)
  184. {
  185. /* C = A >> shiftBits */
  186. /* Shift the inputs and then store the results in the destination buffer. */
  187. *pDst++ = (*pSrc++ >> -shiftBits);
  188. /* Decrement the loop counter */
  189. blkCnt--;
  190. }
  191. }
  192. #endif /* #ifndef ARM_MATH_CM0_FAMILY */
  193. }
  194. /**
  195. * @} end of shift group
  196. */