arm_fir_interpolate_q31.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_q31.c  
00009 *  
00010 * Description:  Q31 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 Q31 FIR interpolator.  
00043  * @param[in] *S        points to an instance of the Q31 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 an internal 64-bit accumulator.  
00052  * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.  
00053  * Thus, if the accumulator result overflows it wraps around rather than clip.  
00054  * In order to avoid overflows completely the input signal must be scaled down by <code>1/(numTaps/L)</code>.  
00055  * since <code>numTaps/L</code> additions occur per output sample.  
00056  * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.  
00057  */ 
00058  
00059  
00060 void arm_fir_interpolate_q31( 
00061   const arm_fir_interpolate_instance_q31 * S, 
00062   q31_t * pSrc, 
00063   q31_t * pDst, 
00064   uint32_t blockSize) 
00065 { 
00066   q31_t *pState = S->pState;                     /* State pointer */ 
00067   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */ 
00068   q31_t *pStateCurnt;                            /* Points to the current sample of the state */ 
00069   q31_t *ptr1, *ptr2;                            /* Temporary pointers for state and coefficient buffers */ 
00070   q63_t sum0;                                    /* Accumulators */ 
00071   q31_t x0, c0;                                  /* Temporary variables to hold state and coefficient values */ 
00072   uint32_t i, blkCnt, j;                         /* Loop counters */ 
00073   uint16_t phaseLen = S->phaseLength, tapCnt;    /* 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 + ((q31_t) phaseLen - 1); 
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 = phaseLen >> 2; 
00108       while(tapCnt > 0u) 
00109       { 
00110  
00111         /* Read the coefficient */ 
00112         c0 = *(ptr2); 
00113  
00114         /* Upsampling is done by stuffing L-1 zeros between each sample.  
00115          * So instead of multiplying zeros with coefficients,  
00116          * Increment the coefficient pointer by interpolation factor times. */ 
00117         ptr2 += S->L; 
00118  
00119         /* Read the input sample */ 
00120         x0 = *(ptr1++); 
00121  
00122         /* Perform the multiply-accumulate */ 
00123         sum0 += (q63_t) x0 *c0; 
00124  
00125         /* Read the coefficient */ 
00126         c0 = *(ptr2); 
00127  
00128         /* Increment the coefficient pointer by interpolation factor times. */ 
00129         ptr2 += S->L; 
00130  
00131         /* Read the input sample */ 
00132         x0 = *(ptr1++); 
00133  
00134         /* Perform the multiply-accumulate */ 
00135         sum0 += (q63_t) x0 *c0; 
00136  
00137         /* Read the coefficient */ 
00138         c0 = *(ptr2); 
00139  
00140         /* Increment the coefficient pointer by interpolation factor times. */ 
00141         ptr2 += S->L; 
00142  
00143         /* Read the input sample */ 
00144         x0 = *(ptr1++); 
00145  
00146         /* Perform the multiply-accumulate */ 
00147         sum0 += (q63_t) x0 *c0; 
00148  
00149         /* Read the coefficient */ 
00150         c0 = *(ptr2); 
00151  
00152         /* Increment the coefficient pointer by interpolation factor times. */ 
00153         ptr2 += S->L; 
00154  
00155         /* Read the input sample */ 
00156         x0 = *(ptr1++); 
00157  
00158         /* Perform the multiply-accumulate */ 
00159         sum0 += (q63_t) x0 *c0; 
00160  
00161         /* Decrement the loop counter */ 
00162         tapCnt--; 
00163       } 
00164  
00165       /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 
00166       tapCnt = phaseLen & 0x3u; 
00167  
00168       while(tapCnt > 0u) 
00169       { 
00170         /* Read the coefficient */ 
00171         c0 = *(ptr2); 
00172  
00173         /* Increment the coefficient pointer by interpolation factor times. */ 
00174         ptr2 += S->L; 
00175  
00176         /* Read the input sample */ 
00177         x0 = *(ptr1++); 
00178  
00179         /* Perform the multiply-accumulate */ 
00180         sum0 += (q63_t) x0 *c0; 
00181  
00182         /* Decrement the loop counter */ 
00183         tapCnt--; 
00184       } 
00185  
00186       /* The result is in the accumulator, store in the destination buffer. */ 
00187       *pDst++ = (q31_t) (sum0 >> 31); 
00188  
00189       /* Increment the address modifier index of coefficient buffer */ 
00190       j++; 
00191  
00192       /* Decrement the loop counter */ 
00193       i--; 
00194     } 
00195  
00196     /* Advance the state pointer by 1  
00197      * to process the next group of interpolation factor number samples */ 
00198     pState = pState + 1; 
00199  
00200     /* Decrement the loop counter */ 
00201     blkCnt--; 
00202   } 
00203  
00204   /* Processing is complete.  
00205    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.  
00206    ** This prepares the state buffer for the next function call. */ 
00207  
00208   /* Points to the start of the state buffer */ 
00209   pStateCurnt = S->pState; 
00210  
00211   tapCnt = (phaseLen - 1u) >> 2u; 
00212  
00213   /* copy data */ 
00214   while(tapCnt > 0u) 
00215   { 
00216     *pStateCurnt++ = *pState++; 
00217     *pStateCurnt++ = *pState++; 
00218     *pStateCurnt++ = *pState++; 
00219     *pStateCurnt++ = *pState++; 
00220  
00221     /* Decrement the loop counter */ 
00222     tapCnt--; 
00223   } 
00224  
00225   tapCnt = (phaseLen - 1u) % 0x04u; 
00226  
00227   /* copy data */ 
00228   while(tapCnt > 0u) 
00229   { 
00230     *pStateCurnt++ = *pState++; 
00231  
00232     /* Decrement the loop counter */ 
00233     tapCnt--; 
00234   } 
00235 } 
00236  
00237  /**  
00238   * @} end of FIR_Interpolate group  
00239   */