text: /* ----------------------------------------------------------------------    
  * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    
  *    
  * $Date:        19. March 2015
  * $Revision: 	V.1.4.5
  *    
  * Project: 	    CMSIS DSP Library    
  * Title:	    arm_fir_decimate_fast_q15.c    
  *    
  * Description:	Fast Q15 FIR Decimator.    
  *    
  * Target Processor: Cortex-M4/Cortex-M3
  *  
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted provided that the following conditions
  * are met:
  *   - Redistributions of source code must retain the above copyright
  *     notice, this list of conditions and the following disclaimer.
  *   - Redistributions in binary form must reproduce the above copyright
  *     notice, this list of conditions and the following disclaimer in
  *     the documentation and/or other materials provided with the 
  *     distribution.
  *   - Neither the name of ARM LIMITED nor the names of its contributors
  *     may be used to endorse or promote products derived from this
  *     software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.    
  
   Modifications 2017  Mostafa Saleh       (Ported to RISC-V PULPino)
  * -------------------------------------------------------------------- */
  
  #include <riscv_dsp/riscv_math.h>
  
  /**    
   * @ingroup groupFilters    
   */
  
  /**    
   * @addtogroup FIR_decimate    
   * @{    
   */
  
  /**    
   * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.    
   * @param[in] *S points to an instance of the Q15 FIR decimator structure.    
   * @param[in] *pSrc points to the block of input data.    
   * @param[out] *pDst points to the block of output data    
   * @param[in] blockSize number of input samples to process per call.    
   * @return none    
   *    
   * \par Restrictions   
   *  If the silicon does not support unaligned memory access enable the macro UNALIGNED_SUPPORT_DISABLE   
   *	In this case input, output, state buffers should be aligned by 32-bit   
   *    
   * <b>Scaling and Overflow Behavior:</b>    
   * \par    
   * This fast version uses a 32-bit accumulator with 2.30 format.    
   * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.    
   * Thus, if the accumulator result overflows it wraps around and distorts the result.    
   * In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (log2 is read as log to the base 2).    
   * The 2.30 accumulator is then truncated to 2.15 format and saturated to yield the 1.15 result.    
   *    
   * \par    
   * Refer to the function <code>riscv_fir_decimate_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.    
   * Both the slow and the fast versions use the same instance structure.    
   * Use the function <code>riscv_fir_decimate_init_q15()</code> to initialize the filter structure.    
   */
  
  #if defined (USE_DSP_RISCV)
  
  void riscv_fir_decimate_fast_q15(
    const riscv_fir_decimate_instance_q15 * S,
    q15_t * pSrc,
    q15_t * pDst,
    uint32_t blockSize)
  {
    q15_t *pState = S->pState;                     /* State pointer */
    q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
    q15_t *pStateCurnt;                            /* Points to the current sample of the state */
    q15_t *px;                                     /* Temporary pointer for state buffer */
    q15_t *pb;                                     /* Temporary pointer coefficient buffer */
    q31_t x0, x1, c0, c1;                          /* Temporary variables to hold state and coefficient values */
    q31_t sum0;                                    /* Accumulators */
    q31_t acc0, acc1;
    q15_t *px0, *px1;
    uint32_t blkCntN3;
    uint32_t numTaps = S->numTaps;                 /* Number of taps */
    uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M;  /* Loop counters */
    shortV *VectInA;
    shortV *VectInB;
    shortV *VectInC;
    shortV *VectInC1;
    /* S->pState buffer contains previous frame (numTaps - 1) samples */
    /* pStateCurnt points to the location where the new input data should be written */
    pStateCurnt = S->pState + (numTaps - 1u);
  
  
    /* Total number of output samples to be computed */
    blkCnt = outBlockSize / 2;
    blkCntN3 = outBlockSize - (2 * blkCnt);
  
  
    while(blkCnt > 0u)
    {
      /* Copy decimation factor number of new input samples into the state buffer */
      i = 2 * S->M;
  
      do
      {
        *pStateCurnt++ = *pSrc++;
  
      } while(--i);
  
      /* Set accumulator to zero */
      acc0 = 0;
      acc1 = 0;
  
      /* Initialize state pointer */
      px0 = pState;
  
      px1 = pState + S->M;
  
  
      /* Initialize coeff pointer */
      pb = pCoeffs;
  
      /* Loop unrolling.  Process 4 taps at a time. */
      tapCnt = numTaps >> 2;
  
      /* Loop over the number of taps.  Unroll by a factor of 4.       
       ** Repeat until we've computed numTaps-4 coefficients. */
      while(tapCnt > 0u)
      {
        /* Read the Read b[numTaps-1] and b[numTaps-2]  coefficients */
        VectInC = (shortV*)pb;
        pb+=2;
        /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
        VectInA = (shortV*)px0;
        px0+=2;
        VectInB = (shortV*)px1;
        px1+=2;
        /* Perform the multiply-accumulate */
        acc0 = sumdotpv2(*VectInA, *VectInC, acc0);
        acc1 = sumdotpv2(*VectInB, *VectInC, acc1);
        /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
        VectInC = (shortV*)pb;
        pb+=2;
        /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
        VectInA = (shortV*)px0;
        px0+=2;
        VectInB = (shortV*)px1;
        px1+=2;
        /* Perform the multiply-accumulate */
        acc0 = sumdotpv2(*VectInA, *VectInC, acc0);
        acc1 = sumdotpv2(*VectInB, *VectInC, acc1);
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* If the filter length is not a multiple of 4, compute the remaining filter taps */
      tapCnt = numTaps % 0x4u;
  
      while(tapCnt > 0u)
      {
        /* Read coefficients */
        c0 = *pb++;
  
        /* Fetch 1 state variable */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 = mac(x0, c0, acc0);
        acc1 = mac(x1, c0, acc1);
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* Advance the state pointer by the decimation factor       
       * to process the next group of decimation factor number samples */
      pState = pState + S->M * 2;
  
      /* Store filter output, smlad returns the values in 2.14 format */
      /* so downsacle by 15 to get output in 1.15 */
      *pDst++ = (q15_t) (clip((acc0 >> 15), -32768,32767));
      *pDst++ = (q15_t) (clip((acc1 >> 15), -32768,32767));
  
      /* Decrement the loop counter */
      blkCnt--;
    }
  
  
  
    while(blkCntN3 > 0u)
    {
      /* Copy decimation factor number of new input samples into the state buffer */
      i = S->M;
  
      do
      {
        *pStateCurnt++ = *pSrc++;
  
      } while(--i);
  
      /*Set sum to zero */
      sum0 = 0;
  
      /* Initialize state pointer */
      px = pState;
  
      /* Initialize coeff pointer */
      pb = pCoeffs;
  
      /* Loop unrolling.  Process 4 taps at a time. */
      tapCnt = numTaps >> 2;
  
      /* Loop over the number of taps.  Unroll by a factor of 4.       
       ** Repeat until we've computed numTaps-4 coefficients. */
      while(tapCnt > 0u)
      {
        /* Read the Read b[numTaps-1] and b[numTaps-2]  coefficients */
        VectInC = (shortV*)pb;
        pb+=2;
        /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
        VectInA = (shortV*)px;
        px+=2;
        /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
        VectInC1 = (shortV*)pb;
        pb+=2;
        /* Perform the multiply-accumulate */
        sum0 = sumdotpv2(*VectInA, *VectInC, sum0);
        /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
        VectInA = (shortV*)px;
        px+=2;
        /* Perform the multiply-accumulate */
        sum0 = sumdotpv2(*VectInA, *VectInC1, sum0);
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* If the filter length is not a multiple of 4, compute the remaining filter taps */
      tapCnt = numTaps % 0x4u;
  
      while(tapCnt > 0u)
      {
        /* Read coefficients */
        c0 = *pb++;
  
        /* Fetch 1 state variable */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 = mac(x0, c0, sum0);
  
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* Advance the state pointer by the decimation factor       
       * to process the next group of decimation factor number samples */
      pState = pState + S->M;
  
      /* Store filter output, smlad returns the values in 2.14 format */
      /* so downsacle by 15 to get output in 1.15 */
      *pDst++ = (q15_t) (clip((sum0 >> 15), -32768,32767));
      /* Decrement the loop counter */
      blkCntN3--;
    }
  
    /* Processing is complete.       
     ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.       
     ** This prepares the state buffer for the next function call. */
  
    /* Points to the start of the state buffer */
    pStateCurnt = S->pState;
  
    i = (numTaps - 1u) >> 1u;
  
    /* copy data */
    while(i > 0u)
    {
      *(shortV*)pStateCurnt =*(shortV*)pState;
      pStateCurnt+=2;
      pState+=2;
      *(shortV*)pStateCurnt =*(shortV*)pState;
      pStateCurnt+=2;
      pState+=2;
      /* Decrement the loop counter */
      i--;
    }
  
    i = (numTaps - 1u) % 0x02u;
  
    /* copy data */
    while(i > 0u)
    {
      *pStateCurnt++ = *pState++;
  
      /* Decrement the loop counter */
      i--;
    }
  }
  
  #else
  
  
  void riscv_fir_decimate_fast_q15(
    const riscv_fir_decimate_instance_q15 * S,
    q15_t * pSrc,
    q15_t * pDst,
    uint32_t blockSize)
  {
    q15_t *pState = S->pState;                     /* State pointer */
    q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
    q15_t *pStateCurnt;                            /* Points to the current sample of the state */
    q15_t *px;                                     /* Temporary pointer for state buffer */
    q15_t *pb;                                     /* Temporary pointer coefficient buffer */
    q15_t x0, x1, c0;                              /* Temporary variables to hold state and coefficient values */
    q31_t sum0;                                    /* Accumulators */
    q31_t acc0, acc1;
    q15_t *px0, *px1;
    uint32_t blkCntN3;
    uint32_t numTaps = S->numTaps;                 /* Number of taps */
    uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M;  /* Loop counters */
  
  
    /* S->pState buffer contains previous frame (numTaps - 1) samples */
    /* pStateCurnt points to the location where the new input data should be written */
    pStateCurnt = S->pState + (numTaps - 1u);
  
  
    /* Total number of output samples to be computed */
    blkCnt = outBlockSize / 2;
    blkCntN3 = outBlockSize - (2 * blkCnt);
  
    while(blkCnt > 0u)
    {
      /* Copy decimation factor number of new input samples into the state buffer */
      i = 2 * S->M;
  
      do
      {
        *pStateCurnt++ = *pSrc++;
  
      } while(--i);
  
      /* Set accumulator to zero */
      acc0 = 0;
      acc1 = 0;
  
      /* Initialize state pointer */
      px0 = pState;
  
      px1 = pState + S->M;
  
  
      /* Initialize coeff pointer */
      pb = pCoeffs;
  
      /* Loop unrolling.  Process 4 taps at a time. */
      tapCnt = numTaps >> 2;
  
      /* Loop over the number of taps.  Unroll by a factor of 4.       
       ** Repeat until we've computed numTaps-4 coefficients. */
      while(tapCnt > 0u)
      {
        /* Read the Read b[numTaps-1] coefficients */
        c0 = *pb++;
  
        /* Read x[n-numTaps-1] for sample 0 and for sample 1 */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 += x0 * c0;
        acc1 += x1 * c0;
  
        /* Read the b[numTaps-2] coefficient */
        c0 = *pb++;
  
        /* Read x[n-numTaps-2] for sample 0 and sample 1 */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 += x0 * c0;
        acc1 += x1 * c0;
  
        /* Read the b[numTaps-3]  coefficients */
        c0 = *pb++;
  
        /* Read x[n-numTaps-3] for sample 0 and sample 1 */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 += x0 * c0;
        acc1 += x1 * c0;
  
        /* Read the b[numTaps-4] coefficient */
        c0 = *pb++;
  
        /* Read x[n-numTaps-4] for sample 0 and sample 1 */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 += x0 * c0;
        acc1 += x1 * c0;
  
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* If the filter length is not a multiple of 4, compute the remaining filter taps */
      tapCnt = numTaps % 0x4u;
  
      while(tapCnt > 0u)
      {
        /* Read coefficients */
        c0 = *pb++;
  
        /* Fetch 1 state variable */
        x0 = *px0++;
        x1 = *px1++;
  
        /* Perform the multiply-accumulate */
        acc0 += x0 * c0;
        acc1 += x1 * c0;
  
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* Advance the state pointer by the decimation factor       
       * to process the next group of decimation factor number samples */
      pState = pState + S->M * 2;
  
      /* Store filter output, smlad returns the values in 2.14 format */
      /* so downsacle by 15 to get output in 1.15 */
  
      *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
      *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
  
  
      /* Decrement the loop counter */
      blkCnt--;
    }
  
    while(blkCntN3 > 0u)
    {
      /* Copy decimation factor number of new input samples into the state buffer */
      i = S->M;
  
      do
      {
        *pStateCurnt++ = *pSrc++;
  
      } while(--i);
  
      /*Set sum to zero */
      sum0 = 0;
  
      /* Initialize state pointer */
      px = pState;
  
      /* Initialize coeff pointer */
      pb = pCoeffs;
  
      /* Loop unrolling.  Process 4 taps at a time. */
      tapCnt = numTaps >> 2;
  
      /* Loop over the number of taps.  Unroll by a factor of 4.       
       ** Repeat until we've computed numTaps-4 coefficients. */
      while(tapCnt > 0u)
      {
        /* Read the Read b[numTaps-1] coefficients */
        c0 = *pb++;
  
        /* Read x[n-numTaps-1] and sample */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 += x0 * c0;
  
        /* Read the b[numTaps-2] coefficient */
        c0 = *pb++;
  
        /* Read x[n-numTaps-2] and  sample */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 += x0 * c0;
  
        /* Read the b[numTaps-3]  coefficients */
        c0 = *pb++;
  
        /* Read x[n-numTaps-3] sample */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 += x0 * c0;
  
        /* Read the b[numTaps-4] coefficient */
        c0 = *pb++;
  
        /* Read x[n-numTaps-4] sample */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 += x0 * c0;
  
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* If the filter length is not a multiple of 4, compute the remaining filter taps */
      tapCnt = numTaps % 0x4u;
  
      while(tapCnt > 0u)
      {
        /* Read coefficients */
        c0 = *pb++;
  
        /* Fetch 1 state variable */
        x0 = *px++;
  
        /* Perform the multiply-accumulate */
        sum0 += x0 * c0;
  
        /* Decrement the loop counter */
        tapCnt--;
      }
  
      /* Advance the state pointer by the decimation factor       
       * to process the next group of decimation factor number samples */
      pState = pState + S->M;
  
      /* Store filter output, smlad returns the values in 2.14 format */
      /* so downsacle by 15 to get output in 1.15 */
      *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
  
      /* Decrement the loop counter */
      blkCntN3--;
    }
  
    /* Processing is complete.       
     ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.       
     ** This prepares the state buffer for the next function call. */
  
    /* Points to the start of the state buffer */
    pStateCurnt = S->pState;
  
    i = (numTaps - 1u) >> 2u;
  
    /* copy data */
    while(i > 0u)
    {
      *pStateCurnt++ = *pState++;
      *pStateCurnt++ = *pState++;
      *pStateCurnt++ = *pState++;
      *pStateCurnt++ = *pState++;
  
      /* Decrement the loop counter */
      i--;
    }
  
    i = (numTaps - 1u) % 0x04u;
  
    /* copy data */
    while(i > 0u)
    {
      *pStateCurnt++ = *pState++;
  
      /* Decrement the loop counter */
      i--;
    }
  }
  
  
  #endif	
  
  /**    
   * @} end of FIR_decimate group    
   */
  