1  /* ----------------------------------------------------------------------
  2   * Project:      CMSIS DSP Library
  3   * Title:        arm_offset_q31.c
  4   * Description:  Q31 vector offset
  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 BasicOffset
 37    @{
 38   */
 39  
 40  /**
 41    @brief         Adds a constant offset to a Q31 vector.
 42    @param[in]     pSrc       points to the input vector
 43    @param[in]     offset     is the offset to be added
 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 Q31 range [0x80000000 0x7FFFFFFF] are saturated.
 51   */
 52  
 53  #if defined(ARM_MATH_MVEI) && !defined(ARM_MATH_AUTOVECTORIZE)
 54  
 55  #include "arm_helium_utils.h"
 56  
 57  void arm_offset_q31(
 58      const q31_t * pSrc,
 59      q31_t   offset,
 60      q31_t * pDst,
 61      uint32_t blockSize)
 62  {
 63      uint32_t  blkCnt;           /* loop counters */
 64      q31x4_t vecSrc;
 65  
 66      /* Compute 4 outputs at a time */
 67      blkCnt = blockSize >> 2;
 68      while (blkCnt > 0U)
 69      {
 70          /*
 71           * C = A + offset
 72           * Add offset and then store the result in the destination buffer.
 73           */
 74          vecSrc = vld1q(pSrc);
 75          vst1q(pDst, vqaddq(vecSrc, offset));
 76          /*
 77           * Decrement the blockSize loop counter
 78           */
 79          blkCnt--;
 80          /*
 81           * advance vector source and destination pointers
 82           */
 83          pSrc += 4;
 84          pDst += 4;
 85      }
 86      /*
 87       * tail
 88       */
 89      blkCnt = blockSize & 3;
 90      if (blkCnt > 0U)
 91      {
 92          mve_pred16_t p0 = vctp32q(blkCnt);
 93          vecSrc = vld1q(pSrc);
 94          vstrwq_p(pDst, vqaddq(vecSrc, offset), p0);
 95      }
 96  }
 97  
 98  #else
 99  void arm_offset_q31(
100    const q31_t * pSrc,
101          q31_t offset,
102          q31_t * pDst,
103          uint32_t blockSize)
104  {
105          uint32_t blkCnt;                               /* Loop counter */
106  
107  #if defined (ARM_MATH_LOOPUNROLL)
108  
109    /* Loop unrolling: Compute 4 outputs at a time */
110    blkCnt = blockSize >> 2U;
111  
112    while (blkCnt > 0U)
113    {
114      /* C = A + offset */
115  
116      /* Add offset and store result in destination buffer. */
117      *pDst++ = __QADD(*pSrc++, offset);
118      
119      *pDst++ = __QADD(*pSrc++, offset);
120      
121      *pDst++ = __QADD(*pSrc++, offset);
122      
123      *pDst++ = __QADD(*pSrc++, offset);
124  
125      /* Decrement loop counter */
126      blkCnt--;
127    }
128  
129    /* Loop unrolling: Compute remaining outputs */
130    blkCnt = blockSize % 0x4U;
131  
132  #else
133  
134    /* Initialize blkCnt with number of samples */
135    blkCnt = blockSize;
136  
137  #endif /* #if defined (ARM_MATH_LOOPUNROLL) */
138  
139    while (blkCnt > 0U)
140    {
141      /* C = A + offset */
142  
143      /* Add offset and store result in destination buffer. */
144  #if defined (ARM_MATH_DSP)
145      *pDst++ = __QADD(*pSrc++, offset);
146  #else
147      *pDst++ = (q31_t) clip_q63_to_q31((q63_t) * pSrc++ + offset);
148  #endif
149  
150      /* Decrement loop counter */
151      blkCnt--;
152    }
153  
154  }
155  #endif /* defined(ARM_MATH_MVEI) */
156  
157  /**
158    @} end of BasicOffset group
159   */