arm_fir_interpolate_q15.c

00001 /*-----------------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
00006 *  
00007 * Project:      CMSIS DSP Library  
00008 * Title:        arm_fir_interpolate_q15.c  
00009 *  
00010 * Description:  Q15 FIR interpolation.  
00011 *  
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated  
00025 *  
00026 * Version 0.0.7  2010/06/10   
00027 *    Misra-C changes done  
00028 * ---------------------------------------------------------------------------*/ 
00029  
00030 #include "arm_math.h" 
00031  
00032 /**  
00033  * @ingroup groupFilters  
00034  */ 
00035  
00036 /**  
00037  * @addtogroup FIR_Interpolate  
00038  * @{  
00039  */ 
00040  
00041 /**  
00042  * @brief Processing function for the Q15 FIR interpolator.  
00043  * @param[in] *S        points to an instance of the Q15 FIR interpolator structure.  
00044  * @param[in] *pSrc     points to the block of input data.  
00045  * @param[out] *pDst    points to the block of output data.  
00046  * @param[in] blockSize number of input samples to process per call.  
00047  * @return none.  
00048  *  
00049  * <b>Scaling and Overflow Behavior:</b>  
00050  * \par  
00051  * The function is implemented using a 64-bit internal accumulator.  
00052  * Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.  
00053  * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.  
00054  * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.  
00055  * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.  
00056  * Lastly, the accumulator is saturated to yield a result in 1.15 format.  
00057  */ 
00058  
00059 void arm_fir_interpolate_q15( 
00060   const arm_fir_interpolate_instance_q15 * S, 
00061   q15_t * pSrc, 
00062   q15_t * pDst, 
00063   uint32_t blockSize) 
00064 { 
00065   q15_t *pState = S->pState;                     /* State pointer                                            */ 
00066   q15_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer                                      */ 
00067   q15_t *pStateCurnt;                            /* Points to the current sample of the state                */ 
00068   q15_t *ptr1, *ptr2;                            /* Temporary pointers for state and coefficient buffers     */ 
00069   q63_t sum0;                                    /* Accumulators                                             */ 
00070   q15_t x0, c0, c1;                              /* Temporary variables to hold state and coefficient values */ 
00071   q31_t c, x; 
00072   uint32_t i, blkCnt, j, tapCnt;                 /* Loop counters                                            */ 
00073   uint16_t phaseLen = S->phaseLength;            /* Length of each polyphase filter component */ 
00074  
00075  
00076   /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 
00077   /* pStateCurnt points to the location where the new input data should be written */ 
00078   pStateCurnt = S->pState + (phaseLen - 1u); 
00079  
00080   /* Total number of intput samples */ 
00081   blkCnt = blockSize; 
00082  
00083   /* Loop over the blockSize. */ 
00084   while(blkCnt > 0u) 
00085   { 
00086     /* Copy new input sample into the state buffer */ 
00087     *pStateCurnt++ = *pSrc++; 
00088  
00089     /* Address modifier index of coefficient buffer */ 
00090     j = 1u; 
00091  
00092     /* Loop over the Interpolation factor. */ 
00093     i = S->L; 
00094     while(i > 0u) 
00095     { 
00096       /* Set accumulator to zero */ 
00097       sum0 = 0; 
00098  
00099       /* Initialize state pointer */ 
00100       ptr1 = pState; 
00101  
00102       /* Initialize coefficient pointer */ 
00103       ptr2 = pCoeffs + (S->L - j); 
00104  
00105       /* Loop over the polyPhase length. Unroll by a factor of 4.  
00106        ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 
00107       tapCnt = (uint32_t) phaseLen >> 2u; 
00108       while(tapCnt > 0u) 
00109       { 
00110         /* Read the coefficient */ 
00111         c0 = *(ptr2); 
00112  
00113         /* Upsampling is done by stuffing L-1 zeros between each sample.  
00114          * So instead of multiplying zeros with coefficients,  
00115          * Increment the coefficient pointer by interpolation factor times. */ 
00116         ptr2 += S->L; 
00117  
00118         /* Read the coefficient */ 
00119         c1 = *(ptr2); 
00120  
00121         /* Increment the coefficient pointer by interpolation factor times. */ 
00122         ptr2 += S->L; 
00123  
00124         /* Pack the coefficients */ 
00125         c = __PKHBT(c0, c1, 16); 
00126  
00127         /* Read twp consecutive input samples */ 
00128         x = *__SIMD32(ptr1)++; 
00129  
00130         /* Perform the multiply-accumulate */ 
00131         sum0 = __SMLALD(x, c, sum0); 
00132  
00133         /* Read the coefficient */ 
00134         c0 = *(ptr2); 
00135  
00136         /* Upsampling is done by stuffing L-1 zeros between each sample.  
00137          * So insted of multiplying zeros with coefficients,  
00138          * Increment the coefficient pointer by interpolation factor times. */ 
00139         ptr2 += S->L; 
00140  
00141         /* Read the coefficient */ 
00142         c1 = *(ptr2); 
00143  
00144         /* Increment the coefficient pointer by interpolation factor times. */ 
00145         ptr2 += S->L; 
00146  
00147         /* Pack the coefficients */ 
00148         c = __PKHBT(c0, c1, 16); 
00149  
00150         /* Read twp consecutive input samples */ 
00151         x = *__SIMD32(ptr1)++; 
00152  
00153         /* Perform the multiply-accumulate */ 
00154         sum0 = __SMLALD(x, c, sum0); 
00155  
00156         /* Decrement the loop counter */ 
00157         tapCnt--; 
00158       } 
00159  
00160       /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 
00161       tapCnt = (uint32_t) phaseLen & 0x3u; 
00162  
00163       while(tapCnt > 0u) 
00164       { 
00165         /* Read the coefficient */ 
00166         c0 = *(ptr2); 
00167  
00168         /* Increment the coefficient pointer by interpolation factor times. */ 
00169         ptr2 += S->L; 
00170  
00171         /* Read the input sample */ 
00172         x0 = *(ptr1++); 
00173  
00174         /* Perform the multiply-accumulate */ 
00175         sum0 = __SMLALD(x0, c0, sum0); 
00176  
00177         /* Decrement the loop counter */ 
00178         tapCnt--; 
00179       } 
00180  
00181       /* The result is in the accumulator, store in the destination buffer. */ 
00182       *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16)); 
00183  
00184       /* Increment the address modifier index of coefficient buffer */ 
00185       j++; 
00186  
00187       /* Decrement the loop counter */ 
00188       i--; 
00189     } 
00190  
00191     /* Advance the state pointer by 1  
00192      * to process the next group of interpolation factor number samples */ 
00193     pState = pState + 1; 
00194  
00195     /* Decrement the loop counter */ 
00196     blkCnt--; 
00197   } 
00198  
00199   /* Processing is complete.  
00200    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.  
00201    ** This prepares the state buffer for the next function call. */ 
00202  
00203   /* Points to the start of the state buffer */ 
00204   pStateCurnt = S->pState; 
00205  
00206   i = ((uint32_t) phaseLen - 1u) >> 2u; 
00207  
00208   /* copy data */ 
00209   while(i > 0u) 
00210   { 
00211     *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 
00212     *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 
00213  
00214     /* Decrement the loop counter */ 
00215     i--; 
00216   } 
00217  
00218   i = ((uint32_t) phaseLen - 1u) % 0x04u; 
00219  
00220   while(i > 0u) 
00221   { 
00222     *pStateCurnt++ = *pState++; 
00223  
00224     /* Decrement the loop counter */ 
00225     i--; 
00226   } 
00227  
00228 } 
00229  
00230  /**  
00231   * @} end of FIR_Interpolate group  
00232   */