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_q15.c 00009 * 00010 * Description: Q15 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 00062 void arm_fir_interpolate_q15( 00063 const arm_fir_interpolate_instance_q15 * S, 00064 q15_t * pSrc, 00065 q15_t * pDst, 00066 uint32_t blockSize) 00067 { 00068 q15_t *pState = S->pState; /* State pointer */ 00069 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ 00070 q15_t *pStateCurnt; /* Points to the current sample of the state */ 00071 q15_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */ 00072 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 q15_t x0, c0, c1; /* Temporary variables to hold state and coefficient values */ 00080 q31_t c, x; 00081 uint32_t i, blkCnt, j, tapCnt; /* Loop counters */ 00082 uint16_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */ 00083 00084 00085 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00086 /* pStateCurnt points to the location where the new input data should be written */ 00087 pStateCurnt = S->pState + (phaseLen - 1u); 00088 00089 /* Total number of intput samples */ 00090 blkCnt = blockSize; 00091 00092 /* Loop over the blockSize. */ 00093 while(blkCnt > 0u) 00094 { 00095 /* Copy new input sample into the state buffer */ 00096 *pStateCurnt++ = *pSrc++; 00097 00098 /* Address modifier index of coefficient buffer */ 00099 j = 1u; 00100 00101 /* Loop over the Interpolation factor. */ 00102 i = S->L; 00103 while(i > 0u) 00104 { 00105 /* Set accumulator to zero */ 00106 sum0 = 0; 00107 00108 /* Initialize state pointer */ 00109 ptr1 = pState; 00110 00111 /* Initialize coefficient pointer */ 00112 ptr2 = pCoeffs + (S->L - j); 00113 00114 /* Loop over the polyPhase length. Unroll by a factor of 4. 00115 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 00116 tapCnt = (uint32_t) phaseLen >> 2u; 00117 while(tapCnt > 0u) 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 coefficient */ 00128 c1 = *(ptr2); 00129 00130 /* Increment the coefficient pointer by interpolation factor times. */ 00131 ptr2 += S->L; 00132 00133 /* Pack the coefficients */ 00134 #ifndef ARM_MATH_BIG_ENDIAN 00135 00136 c = __PKHBT(c0, c1, 16); 00137 00138 #else 00139 00140 c = __PKHBT(c1, c0, 16); 00141 00142 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00143 00144 /* Read twp consecutive input samples */ 00145 x = *__SIMD32(ptr1)++; 00146 00147 /* Perform the multiply-accumulate */ 00148 sum0 = __SMLALD(x, c, sum0); 00149 00150 /* Read the coefficient */ 00151 c0 = *(ptr2); 00152 00153 /* Upsampling is done by stuffing L-1 zeros between each sample. 00154 * So insted of multiplying zeros with coefficients, 00155 * Increment the coefficient pointer by interpolation factor times. */ 00156 ptr2 += S->L; 00157 00158 /* Read the coefficient */ 00159 c1 = *(ptr2); 00160 00161 /* Increment the coefficient pointer by interpolation factor times. */ 00162 ptr2 += S->L; 00163 00164 /* Pack the coefficients */ 00165 #ifndef ARM_MATH_BIG_ENDIAN 00166 00167 c = __PKHBT(c0, c1, 16); 00168 00169 #else 00170 00171 c = __PKHBT(c1, c0, 16); 00172 00173 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */ 00174 00175 /* Read twp consecutive input samples */ 00176 x = *__SIMD32(ptr1)++; 00177 00178 /* Perform the multiply-accumulate */ 00179 sum0 = __SMLALD(x, c, sum0); 00180 00181 /* Decrement the loop counter */ 00182 tapCnt--; 00183 } 00184 00185 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 00186 tapCnt = (uint32_t) phaseLen & 0x3u; 00187 00188 while(tapCnt > 0u) 00189 { 00190 /* Read the coefficient */ 00191 c0 = *(ptr2); 00192 00193 /* Increment the coefficient pointer by interpolation factor times. */ 00194 ptr2 += S->L; 00195 00196 /* Read the input sample */ 00197 x0 = *(ptr1++); 00198 00199 /* Perform the multiply-accumulate */ 00200 sum0 = __SMLALD(x0, c0, sum0); 00201 00202 /* Decrement the loop counter */ 00203 tapCnt--; 00204 } 00205 00206 /* The result is in the accumulator, store in the destination buffer. */ 00207 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16)); 00208 00209 /* Increment the address modifier index of coefficient buffer */ 00210 j++; 00211 00212 /* Decrement the loop counter */ 00213 i--; 00214 } 00215 00216 /* Advance the state pointer by 1 00217 * to process the next group of interpolation factor number samples */ 00218 pState = pState + 1; 00219 00220 /* Decrement the loop counter */ 00221 blkCnt--; 00222 } 00223 00224 /* Processing is complete. 00225 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer. 00226 ** This prepares the state buffer for the next function call. */ 00227 00228 /* Points to the start of the state buffer */ 00229 pStateCurnt = S->pState; 00230 00231 i = ((uint32_t) phaseLen - 1u) >> 2u; 00232 00233 /* copy data */ 00234 while(i > 0u) 00235 { 00236 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 00237 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++; 00238 00239 /* Decrement the loop counter */ 00240 i--; 00241 } 00242 00243 i = ((uint32_t) phaseLen - 1u) % 0x04u; 00244 00245 while(i > 0u) 00246 { 00247 *pStateCurnt++ = *pState++; 00248 00249 /* Decrement the loop counter */ 00250 i--; 00251 } 00252 00253 #else 00254 00255 /* Run the below code for Cortex-M0 */ 00256 00257 q63_t sum; /* Accumulator */ 00258 q15_t x0, c0; /* Temporary variables to hold state and coefficient values */ 00259 uint32_t i, blkCnt, tapCnt; /* Loop counters */ 00260 uint16_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */ 00261 00262 00263 /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 00264 /* pStateCurnt points to the location where the new input data should be written */ 00265 pStateCurnt = S->pState + (phaseLen - 1u); 00266 00267 /* Total number of intput samples */ 00268 blkCnt = blockSize; 00269 00270 /* Loop over the blockSize. */ 00271 while(blkCnt > 0u) 00272 { 00273 /* Copy new input sample into the state buffer */ 00274 *pStateCurnt++ = *pSrc++; 00275 00276 /* Loop over the Interpolation factor. */ 00277 i = S->L; 00278 00279 while(i > 0u) 00280 { 00281 /* Set accumulator to zero */ 00282 sum = 0; 00283 00284 /* Initialize state pointer */ 00285 ptr1 = pState; 00286 00287 /* Initialize coefficient pointer */ 00288 ptr2 = pCoeffs + (i - 1u); 00289 00290 /* Loop over the polyPhase length */ 00291 tapCnt = (uint32_t) phaseLen; 00292 00293 while(tapCnt > 0u) 00294 { 00295 /* Read the coefficient */ 00296 c0 = *ptr2; 00297 00298 /* Increment the coefficient pointer by interpolation factor times. */ 00299 ptr2 += S->L; 00300 00301 /* Read the input sample */ 00302 x0 = *ptr1++; 00303 00304 /* Perform the multiply-accumulate */ 00305 sum += ((q31_t) x0 * c0); 00306 00307 /* Decrement the loop counter */ 00308 tapCnt--; 00309 } 00310 00311 /* Store the result after converting to 1.15 format in the destination buffer */ 00312 *pDst++ = (q15_t) (__SSAT((sum >> 15), 16)); 00313 00314 /* Decrement the loop counter */ 00315 i--; 00316 } 00317 00318 /* Advance the state pointer by 1 00319 * to process the next group of interpolation factor number samples */ 00320 pState = pState + 1; 00321 00322 /* Decrement the loop counter */ 00323 blkCnt--; 00324 } 00325 00326 /* Processing is complete. 00327 ** Now copy the last phaseLen - 1 samples to the start of the state buffer. 00328 ** This prepares the state buffer for the next function call. */ 00329 00330 /* Points to the start of the state buffer */ 00331 pStateCurnt = S->pState; 00332 00333 i = (uint32_t) phaseLen - 1u; 00334 00335 while(i > 0u) 00336 { 00337 *pStateCurnt++ = *pState++; 00338 00339 /* Decrement the loop counter */ 00340 i--; 00341 } 00342 00343 #endif /* #ifndef ARM_MATH_CM0 */ 00344 00345 } 00346