00001 /*----------------------------------------------------------------------------- 00002 * Copyright (C) 2010 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 15. July 2011 00005 * $Revision: V1.0.10 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/Cortex-M0 00013 * 00014 * Version 1.0.10 2011/7/15 00015 * Big Endian support added and Merged M0 and M3/M4 Source code. 00016 * 00017 * Version 1.0.3 2010/11/29 00018 * Re-organized the CMSIS folders and updated documentation. 00019 * 00020 * Version 1.0.2 2010/11/11 00021 * Documentation updated. 00022 * 00023 * Version 1.0.1 2010/10/05 00024 * Production release and review comments incorporated. 00025 * 00026 * Version 1.0.0 2010/09/20 00027 * Production release and review comments incorporated 00028 * 00029 * Version 0.0.7 2010/06/10 00030 * Misra-C changes done 00031 * ---------------------------------------------------------------------------*/ 00032 00033 #include "arm_math.h" 00034 00063 void arm_fir_interpolate_q31( 00064 const arm_fir_interpolate_instance_q31 * S, 00065 q31_t * pSrc, 00066 q31_t * pDst, 00067 uint32_t blockSize) 00068 { 00069 q31_t *pState = S->pState; /* State pointer */ 00070 q31_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00071 q31_t *pStateCurnt; /* Points to the current sample of the state */ 00072 q31_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */ 00073 00074 #ifndef ARM_MATH_CM0 00075 00076 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00077 00078 q63_t sum0; /* Accumulators */ 00079 q31_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00080 uint32_t i, blkCnt, j; /* Loop counters */ 00081 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */ 00082 00083 00084 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00085 /* pStateCurnt points to the location where the new input data should be written */ 00086 pStateCurnt = S->pState + ((q31_t) phaseLen - 1); 00087 00088 /* Total number of intput samples */ 00089 blkCnt = blockSize; 00090 00091 /* Loop over the blockSize. */ 00092 while(blkCnt > 0u) 00093 { 00094 /* Copy new input sample into the state buffer */ 00095 *pStateCurnt++ = *pSrc++; 00096 00097 /* Address modifier index of coefficient buffer */ 00098 j = 1u; 00099 00100 /* Loop over the Interpolation factor. */ 00101 i = S->L; 00102 while(i > 0u) 00103 { 00104 /* Set accumulator to zero */ 00105 sum0 = 0; 00106 00107 /* Initialize state pointer */ 00108 ptr1 = pState; 00109 00110 /* Initialize coefficient pointer */ 00111 ptr2 = pCoeffs + (S->L - j); 00112 00113 /* Loop over the polyPhase length. Unroll by a factor of 4. 00114 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00115 tapCnt = phaseLen >> 2; 00116 while(tapCnt > 0u) 00117 { 00118 00119 /* Read the coefficient */ 00120 c0 = *(ptr2); 00121 00122 /* Upsampling is done by stuffing L-1 zeros between each sample. 00123 * So instead of multiplying zeros with coefficients, 00124 * Increment the coefficient pointer by interpolation factor times. */ 00125 ptr2 += S->L; 00126 00127 /* Read the input sample */ 00128 x0 = *(ptr1++); 00129 00130 /* Perform the multiply-accumulate */ 00131 sum0 += (q63_t) x0 *c0; 00132 00133 /* Read the coefficient */ 00134 c0 = *(ptr2); 00135 00136 /* Increment the coefficient pointer by interpolation factor times. */ 00137 ptr2 += S->L; 00138 00139 /* Read the input sample */ 00140 x0 = *(ptr1++); 00141 00142 /* Perform the multiply-accumulate */ 00143 sum0 += (q63_t) x0 *c0; 00144 00145 /* Read the coefficient */ 00146 c0 = *(ptr2); 00147 00148 /* Increment the coefficient pointer by interpolation factor times. */ 00149 ptr2 += S->L; 00150 00151 /* Read the input sample */ 00152 x0 = *(ptr1++); 00153 00154 /* Perform the multiply-accumulate */ 00155 sum0 += (q63_t) x0 *c0; 00156 00157 /* Read the coefficient */ 00158 c0 = *(ptr2); 00159 00160 /* Increment the coefficient pointer by interpolation factor times. */ 00161 ptr2 += S->L; 00162 00163 /* Read the input sample */ 00164 x0 = *(ptr1++); 00165 00166 /* Perform the multiply-accumulate */ 00167 sum0 += (q63_t) x0 *c0; 00168 00169 /* Decrement the loop counter */ 00170 tapCnt--; 00171 } 00172 00173 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00174 tapCnt = phaseLen & 0x3u; 00175 00176 while(tapCnt > 0u) 00177 { 00178 /* Read the coefficient */ 00179 c0 = *(ptr2); 00180 00181 /* Increment the coefficient pointer by interpolation factor times. */ 00182 ptr2 += S->L; 00183 00184 /* Read the input sample */ 00185 x0 = *(ptr1++); 00186 00187 /* Perform the multiply-accumulate */ 00188 sum0 += (q63_t) x0 *c0; 00189 00190 /* Decrement the loop counter */ 00191 tapCnt--; 00192 } 00193 00194 /* The result is in the accumulator, store in the destination buffer. */ 00195 *pDst++ = (q31_t) (sum0 >> 31); 00196 00197 /* Increment the address modifier index of coefficient buffer */ 00198 j++; 00199 00200 /* Decrement the loop counter */ 00201 i--; 00202 } 00203 00204 /* Advance the state pointer by 1 00205 * to process the next group of interpolation factor number samples */ 00206 pState = pState + 1; 00207 00208 /* Decrement the loop counter */ 00209 blkCnt--; 00210 } 00211 00212 /* Processing is complete. 00213 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer. 00214 ** This prepares the state buffer for the next function call. */ 00215 00216 /* Points to the start of the state buffer */ 00217 pStateCurnt = S->pState; 00218 00219 tapCnt = (phaseLen - 1u) >> 2u; 00220 00221 /* copy data */ 00222 while(tapCnt > 0u) 00223 { 00224 *pStateCurnt++ = *pState++; 00225 *pStateCurnt++ = *pState++; 00226 *pStateCurnt++ = *pState++; 00227 *pStateCurnt++ = *pState++; 00228 00229 /* Decrement the loop counter */ 00230 tapCnt--; 00231 } 00232 00233 tapCnt = (phaseLen - 1u) % 0x04u; 00234 00235 /* copy data */ 00236 while(tapCnt > 0u) 00237 { 00238 *pStateCurnt++ = *pState++; 00239 00240 /* Decrement the loop counter */ 00241 tapCnt--; 00242 } 00243 00244 #else 00245 00246 /* Run the below code for Cortex-M0 */ 00247 00248 q63_t sum; /* Accumulator */ 00249 q31_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00250 uint32_t i, blkCnt; /* Loop counters */ 00251 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */ 00252 00253 00254 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00255 /* pStateCurnt points to the location where the new input data should be written */ 00256 pStateCurnt = S->pState + ((q31_t) phaseLen - 1); 00257 00258 /* Total number of intput samples */ 00259 blkCnt = blockSize; 00260 00261 /* Loop over the blockSize. */ 00262 while(blkCnt > 0u) 00263 { 00264 /* Copy new input sample into the state buffer */ 00265 *pStateCurnt++ = *pSrc++; 00266 00267 /* Loop over the Interpolation factor. */ 00268 i = S->L; 00269 00270 while(i > 0u) 00271 { 00272 /* Set accumulator to zero */ 00273 sum = 0; 00274 00275 /* Initialize state pointer */ 00276 ptr1 = pState; 00277 00278 /* Initialize coefficient pointer */ 00279 ptr2 = pCoeffs + (i - 1u); 00280 00281 tapCnt = phaseLen; 00282 00283 while(tapCnt > 0u) 00284 { 00285 /* Read the coefficient */ 00286 c0 = *(ptr2); 00287 00288 /* Increment the coefficient pointer by interpolation factor times. */ 00289 ptr2 += S->L; 00290 00291 /* Read the input sample */ 00292 x0 = *ptr1++; 00293 00294 /* Perform the multiply-accumulate */ 00295 sum += (q63_t) x0 *c0; 00296 00297 /* Decrement the loop counter */ 00298 tapCnt--; 00299 } 00300 00301 /* The result is in the accumulator, store in the destination buffer. */ 00302 *pDst++ = (q31_t) (sum >> 31); 00303 00304 /* Decrement the loop counter */ 00305 i--; 00306 } 00307 00308 /* Advance the state pointer by 1 00309 * to process the next group of interpolation factor number samples */ 00310 pState = pState + 1; 00311 00312 /* Decrement the loop counter */ 00313 blkCnt--; 00314 } 00315 00316 /* Processing is complete. 00317 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer. 00318 ** This prepares the state buffer for the next function call. */ 00319 00320 /* Points to the start of the state buffer */ 00321 pStateCurnt = S->pState; 00322 00323 tapCnt = phaseLen - 1u; 00324 00325 /* copy data */ 00326 while(tapCnt > 0u) 00327 { 00328 *pStateCurnt++ = *pState++; 00329 00330 /* Decrement the loop counter */ 00331 tapCnt--; 00332 } 00333 00334 #endif /* #ifndef ARM_MATH_CM0 */ 00335 00336 } 00337