Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

Dependents:   Nucleo-Heart-Rate ejercicioVrms2 PROYECTOFINAL ejercicioVrms ... more

functions/FilteringFunctions/arm_fir_interpolate_q15.c@3:4098b9d3d571, 2018-06-21 (annotated)

Committer:
xorjoep
Date:
Thu Jun 21 11:56:27 2018 +0000
Revision:
3:4098b9d3d571
Parent:
1:24714b45cd1b 
headers is a folder not a library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xorjoep 1:24714b45cd1b 1 /* ----------------------------------------------------------------------
xorjoep 1:24714b45cd1b 2 * Project: CMSIS DSP Library
xorjoep 1:24714b45cd1b 3 * Title: arm_fir_interpolate_q15.c
xorjoep 1:24714b45cd1b 4 * Description: Q15 FIR interpolation
xorjoep 1:24714b45cd1b 5 *
xorjoep 1:24714b45cd1b 6 * $Date: 27. January 2017
xorjoep 1:24714b45cd1b 7 * $Revision: V.1.5.1
xorjoep 1:24714b45cd1b 8 *
xorjoep 1:24714b45cd1b 9 * Target Processor: Cortex-M cores
xorjoep 1:24714b45cd1b 10 * -------------------------------------------------------------------- */
xorjoep 1:24714b45cd1b 11 /*
xorjoep 1:24714b45cd1b 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
xorjoep 1:24714b45cd1b 13 *
xorjoep 1:24714b45cd1b 14 * SPDX-License-Identifier: Apache-2.0
xorjoep 1:24714b45cd1b 15 *
xorjoep 1:24714b45cd1b 16 * Licensed under the Apache License, Version 2.0 (the License); you may
xorjoep 1:24714b45cd1b 17 * not use this file except in compliance with the License.
xorjoep 1:24714b45cd1b 18 * You may obtain a copy of the License at
xorjoep 1:24714b45cd1b 19 *
xorjoep 1:24714b45cd1b 20 * www.apache.org/licenses/LICENSE-2.0
xorjoep 1:24714b45cd1b 21 *
xorjoep 1:24714b45cd1b 22 * Unless required by applicable law or agreed to in writing, software
xorjoep 1:24714b45cd1b 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
xorjoep 1:24714b45cd1b 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
xorjoep 1:24714b45cd1b 25 * See the License for the specific language governing permissions and
xorjoep 1:24714b45cd1b 26 * limitations under the License.
xorjoep 1:24714b45cd1b 27 */
xorjoep 1:24714b45cd1b 28
xorjoep 1:24714b45cd1b 29 #include "arm_math.h"
xorjoep 1:24714b45cd1b 30
xorjoep 1:24714b45cd1b 31 /**
xorjoep 1:24714b45cd1b 32 * @ingroup groupFilters
xorjoep 1:24714b45cd1b 33 */
xorjoep 1:24714b45cd1b 34
xorjoep 1:24714b45cd1b 35 /**
xorjoep 1:24714b45cd1b 36 * @addtogroup FIR_Interpolate
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40 /**
xorjoep 1:24714b45cd1b 41 * @brief Processing function for the Q15 FIR interpolator.
xorjoep 1:24714b45cd1b 42 * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
xorjoep 1:24714b45cd1b 43 * @param[in] *pSrc points to the block of input data.
xorjoep 1:24714b45cd1b 44 * @param[out] *pDst points to the block of output data.
xorjoep 1:24714b45cd1b 45 * @param[in] blockSize number of input samples to process per call.
xorjoep 1:24714b45cd1b 46 * @return none.
xorjoep 1:24714b45cd1b 47 *
xorjoep 1:24714b45cd1b 48 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 49 * \par
xorjoep 1:24714b45cd1b 50 * The function is implemented using a 64-bit internal accumulator.
xorjoep 1:24714b45cd1b 51 * Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
xorjoep 1:24714b45cd1b 52 * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
xorjoep 1:24714b45cd1b 53 * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
xorjoep 1:24714b45cd1b 54 * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
xorjoep 1:24714b45cd1b 55 * Lastly, the accumulator is saturated to yield a result in 1.15 format.
xorjoep 1:24714b45cd1b 56 */
xorjoep 1:24714b45cd1b 57
xorjoep 1:24714b45cd1b 58 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 59
xorjoep 1:24714b45cd1b 60 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 61
xorjoep 1:24714b45cd1b 62 void arm_fir_interpolate_q15(
xorjoep 1:24714b45cd1b 63 const arm_fir_interpolate_instance_q15 * S,
xorjoep 1:24714b45cd1b 64 q15_t * pSrc,
xorjoep 1:24714b45cd1b 65 q15_t * pDst,
xorjoep 1:24714b45cd1b 66 uint32_t blockSize)
xorjoep 1:24714b45cd1b 67 {
xorjoep 1:24714b45cd1b 68 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 69 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 70 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 71 q15_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
xorjoep 1:24714b45cd1b 72 q63_t sum0; /* Accumulators */
xorjoep 1:24714b45cd1b 73 q15_t x0, c0; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 74 uint32_t i, blkCnt, j, tapCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 75 uint16_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
xorjoep 1:24714b45cd1b 76 uint32_t blkCntN2;
xorjoep 1:24714b45cd1b 77 q63_t acc0, acc1;
xorjoep 1:24714b45cd1b 78 q15_t x1;
xorjoep 1:24714b45cd1b 79
xorjoep 1:24714b45cd1b 80 /* S->pState buffer contains previous frame (phaseLen - 1) samples */
xorjoep 1:24714b45cd1b 81 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 82 pStateCurnt = S->pState + ((q31_t) phaseLen - 1);
xorjoep 1:24714b45cd1b 83
xorjoep 1:24714b45cd1b 84 /* Initialise blkCnt */
xorjoep 1:24714b45cd1b 85 blkCnt = blockSize / 2;
xorjoep 1:24714b45cd1b 86 blkCntN2 = blockSize - (2 * blkCnt);
xorjoep 1:24714b45cd1b 87
xorjoep 1:24714b45cd1b 88 /* Samples loop unrolled by 2 */
xorjoep 1:24714b45cd1b 89 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 90 {
xorjoep 1:24714b45cd1b 91 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 92 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 93 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 94
xorjoep 1:24714b45cd1b 95 /* Address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 96 j = 1U;
xorjoep 1:24714b45cd1b 97
xorjoep 1:24714b45cd1b 98 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 99 i = (S->L);
xorjoep 1:24714b45cd1b 100
xorjoep 1:24714b45cd1b 101 while (i > 0U)
xorjoep 1:24714b45cd1b 102 {
xorjoep 1:24714b45cd1b 103 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 104 acc0 = 0;
xorjoep 1:24714b45cd1b 105 acc1 = 0;
xorjoep 1:24714b45cd1b 106
xorjoep 1:24714b45cd1b 107 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 108 ptr1 = pState;
xorjoep 1:24714b45cd1b 109
xorjoep 1:24714b45cd1b 110 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 111 ptr2 = pCoeffs + (S->L - j);
xorjoep 1:24714b45cd1b 112
xorjoep 1:24714b45cd1b 113 /* Loop over the polyPhase length. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 114 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
xorjoep 1:24714b45cd1b 115 tapCnt = phaseLen >> 2U;
xorjoep 1:24714b45cd1b 116
xorjoep 1:24714b45cd1b 117 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 118
xorjoep 1:24714b45cd1b 119 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 120 {
xorjoep 1:24714b45cd1b 121
xorjoep 1:24714b45cd1b 122 /* Read the input sample */
xorjoep 1:24714b45cd1b 123 x1 = *(ptr1++);
xorjoep 1:24714b45cd1b 124
xorjoep 1:24714b45cd1b 125 /* Read the coefficient */
xorjoep 1:24714b45cd1b 126 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 127
xorjoep 1:24714b45cd1b 128 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 129 acc0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 130 acc1 += (q63_t) x1 *c0;
xorjoep 1:24714b45cd1b 131
xorjoep 1:24714b45cd1b 132
xorjoep 1:24714b45cd1b 133 /* Read the coefficient */
xorjoep 1:24714b45cd1b 134 c0 = *(ptr2 + S->L);
xorjoep 1:24714b45cd1b 135
xorjoep 1:24714b45cd1b 136 /* Read the input sample */
xorjoep 1:24714b45cd1b 137 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 138
xorjoep 1:24714b45cd1b 139 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 140 acc0 += (q63_t) x1 *c0;
xorjoep 1:24714b45cd1b 141 acc1 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 142
xorjoep 1:24714b45cd1b 143
xorjoep 1:24714b45cd1b 144 /* Read the coefficient */
xorjoep 1:24714b45cd1b 145 c0 = *(ptr2 + S->L * 2);
xorjoep 1:24714b45cd1b 146
xorjoep 1:24714b45cd1b 147 /* Read the input sample */
xorjoep 1:24714b45cd1b 148 x1 = *(ptr1++);
xorjoep 1:24714b45cd1b 149
xorjoep 1:24714b45cd1b 150 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 151 acc0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 152 acc1 += (q63_t) x1 *c0;
xorjoep 1:24714b45cd1b 153
xorjoep 1:24714b45cd1b 154 /* Read the coefficient */
xorjoep 1:24714b45cd1b 155 c0 = *(ptr2 + S->L * 3);
xorjoep 1:24714b45cd1b 156
xorjoep 1:24714b45cd1b 157 /* Read the input sample */
xorjoep 1:24714b45cd1b 158 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 159
xorjoep 1:24714b45cd1b 160 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 161 acc0 += (q63_t) x1 *c0;
xorjoep 1:24714b45cd1b 162 acc1 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 163
xorjoep 1:24714b45cd1b 164
xorjoep 1:24714b45cd1b 165 /* Upsampling is done by stuffing L-1 zeros between each sample.
xorjoep 1:24714b45cd1b 166 * So instead of multiplying zeros with coefficients,
xorjoep 1:24714b45cd1b 167 * Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 168 ptr2 += 4 * S->L;
xorjoep 1:24714b45cd1b 169
xorjoep 1:24714b45cd1b 170 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 171 tapCnt--;
xorjoep 1:24714b45cd1b 172 }
xorjoep 1:24714b45cd1b 173
xorjoep 1:24714b45cd1b 174 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 175 tapCnt = phaseLen % 0x4U;
xorjoep 1:24714b45cd1b 176
xorjoep 1:24714b45cd1b 177 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 178 {
xorjoep 1:24714b45cd1b 179
xorjoep 1:24714b45cd1b 180 /* Read the input sample */
xorjoep 1:24714b45cd1b 181 x1 = *(ptr1++);
xorjoep 1:24714b45cd1b 182
xorjoep 1:24714b45cd1b 183 /* Read the coefficient */
xorjoep 1:24714b45cd1b 184 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 185
xorjoep 1:24714b45cd1b 186 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 187 acc0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 188 acc1 += (q63_t) x1 *c0;
xorjoep 1:24714b45cd1b 189
xorjoep 1:24714b45cd1b 190 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 191 ptr2 += S->L;
xorjoep 1:24714b45cd1b 192
xorjoep 1:24714b45cd1b 193 /* update states for next sample processing */
xorjoep 1:24714b45cd1b 194 x0 = x1;
xorjoep 1:24714b45cd1b 195
xorjoep 1:24714b45cd1b 196 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 197 tapCnt--;
xorjoep 1:24714b45cd1b 198 }
xorjoep 1:24714b45cd1b 199
xorjoep 1:24714b45cd1b 200 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 201 *pDst = (q15_t) (__SSAT((acc0 >> 15), 16));
xorjoep 1:24714b45cd1b 202 *(pDst + S->L) = (q15_t) (__SSAT((acc1 >> 15), 16));
xorjoep 1:24714b45cd1b 203
xorjoep 1:24714b45cd1b 204 pDst++;
xorjoep 1:24714b45cd1b 205
xorjoep 1:24714b45cd1b 206 /* Increment the address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 207 j++;
xorjoep 1:24714b45cd1b 208
xorjoep 1:24714b45cd1b 209 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 210 i--;
xorjoep 1:24714b45cd1b 211 }
xorjoep 1:24714b45cd1b 212
xorjoep 1:24714b45cd1b 213 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 214 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 215 pState = pState + 2;
xorjoep 1:24714b45cd1b 216
xorjoep 1:24714b45cd1b 217 pDst += S->L;
xorjoep 1:24714b45cd1b 218
xorjoep 1:24714b45cd1b 219 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 220 blkCnt--;
xorjoep 1:24714b45cd1b 221 }
xorjoep 1:24714b45cd1b 222
xorjoep 1:24714b45cd1b 223 /* If the blockSize is not a multiple of 2, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 224 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 225 blkCnt = blkCntN2;
xorjoep 1:24714b45cd1b 226
xorjoep 1:24714b45cd1b 227 /* Loop over the blockSize. */
xorjoep 1:24714b45cd1b 228 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 229 {
xorjoep 1:24714b45cd1b 230 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 231 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 232
xorjoep 1:24714b45cd1b 233 /* Address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 234 j = 1U;
xorjoep 1:24714b45cd1b 235
xorjoep 1:24714b45cd1b 236 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 237 i = S->L;
xorjoep 1:24714b45cd1b 238 while (i > 0U)
xorjoep 1:24714b45cd1b 239 {
xorjoep 1:24714b45cd1b 240 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 241 sum0 = 0;
xorjoep 1:24714b45cd1b 242
xorjoep 1:24714b45cd1b 243 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 244 ptr1 = pState;
xorjoep 1:24714b45cd1b 245
xorjoep 1:24714b45cd1b 246 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 247 ptr2 = pCoeffs + (S->L - j);
xorjoep 1:24714b45cd1b 248
xorjoep 1:24714b45cd1b 249 /* Loop over the polyPhase length. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 250 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
xorjoep 1:24714b45cd1b 251 tapCnt = phaseLen >> 2;
xorjoep 1:24714b45cd1b 252 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 253 {
xorjoep 1:24714b45cd1b 254
xorjoep 1:24714b45cd1b 255 /* Read the coefficient */
xorjoep 1:24714b45cd1b 256 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 257
xorjoep 1:24714b45cd1b 258 /* Upsampling is done by stuffing L-1 zeros between each sample.
xorjoep 1:24714b45cd1b 259 * So instead of multiplying zeros with coefficients,
xorjoep 1:24714b45cd1b 260 * Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 261 ptr2 += S->L;
xorjoep 1:24714b45cd1b 262
xorjoep 1:24714b45cd1b 263 /* Read the input sample */
xorjoep 1:24714b45cd1b 264 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 265
xorjoep 1:24714b45cd1b 266 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 267 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 268
xorjoep 1:24714b45cd1b 269 /* Read the coefficient */
xorjoep 1:24714b45cd1b 270 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 271
xorjoep 1:24714b45cd1b 272 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 273 ptr2 += S->L;
xorjoep 1:24714b45cd1b 274
xorjoep 1:24714b45cd1b 275 /* Read the input sample */
xorjoep 1:24714b45cd1b 276 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 277
xorjoep 1:24714b45cd1b 278 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 279 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 280
xorjoep 1:24714b45cd1b 281 /* Read the coefficient */
xorjoep 1:24714b45cd1b 282 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 283
xorjoep 1:24714b45cd1b 284 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 285 ptr2 += S->L;
xorjoep 1:24714b45cd1b 286
xorjoep 1:24714b45cd1b 287 /* Read the input sample */
xorjoep 1:24714b45cd1b 288 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 289
xorjoep 1:24714b45cd1b 290 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 291 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 292
xorjoep 1:24714b45cd1b 293 /* Read the coefficient */
xorjoep 1:24714b45cd1b 294 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 295
xorjoep 1:24714b45cd1b 296 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 297 ptr2 += S->L;
xorjoep 1:24714b45cd1b 298
xorjoep 1:24714b45cd1b 299 /* Read the input sample */
xorjoep 1:24714b45cd1b 300 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 301
xorjoep 1:24714b45cd1b 302 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 303 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 304
xorjoep 1:24714b45cd1b 305 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 306 tapCnt--;
xorjoep 1:24714b45cd1b 307 }
xorjoep 1:24714b45cd1b 308
xorjoep 1:24714b45cd1b 309 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 310 tapCnt = phaseLen & 0x3U;
xorjoep 1:24714b45cd1b 311
xorjoep 1:24714b45cd1b 312 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 313 {
xorjoep 1:24714b45cd1b 314 /* Read the coefficient */
xorjoep 1:24714b45cd1b 315 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 316
xorjoep 1:24714b45cd1b 317 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 318 ptr2 += S->L;
xorjoep 1:24714b45cd1b 319
xorjoep 1:24714b45cd1b 320 /* Read the input sample */
xorjoep 1:24714b45cd1b 321 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 322
xorjoep 1:24714b45cd1b 323 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 324 sum0 += (q63_t) x0 *c0;
xorjoep 1:24714b45cd1b 325
xorjoep 1:24714b45cd1b 326 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 327 tapCnt--;
xorjoep 1:24714b45cd1b 328 }
xorjoep 1:24714b45cd1b 329
xorjoep 1:24714b45cd1b 330 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 331 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
xorjoep 1:24714b45cd1b 332
xorjoep 1:24714b45cd1b 333 j++;
xorjoep 1:24714b45cd1b 334
xorjoep 1:24714b45cd1b 335 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 336 i--;
xorjoep 1:24714b45cd1b 337 }
xorjoep 1:24714b45cd1b 338
xorjoep 1:24714b45cd1b 339 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 340 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 341 pState = pState + 1;
xorjoep 1:24714b45cd1b 342
xorjoep 1:24714b45cd1b 343 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 344 blkCnt--;
xorjoep 1:24714b45cd1b 345 }
xorjoep 1:24714b45cd1b 346
xorjoep 1:24714b45cd1b 347
xorjoep 1:24714b45cd1b 348 /* Processing is complete.
xorjoep 1:24714b45cd1b 349 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 350 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 351
xorjoep 1:24714b45cd1b 352 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 353 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 354
xorjoep 1:24714b45cd1b 355 i = ((uint32_t) phaseLen - 1U) >> 2U;
xorjoep 1:24714b45cd1b 356
xorjoep 1:24714b45cd1b 357 /* copy data */
xorjoep 1:24714b45cd1b 358 while (i > 0U)
xorjoep 1:24714b45cd1b 359 {
xorjoep 1:24714b45cd1b 360 #ifndef UNALIGNED_SUPPORT_DISABLE
xorjoep 1:24714b45cd1b 361
xorjoep 1:24714b45cd1b 362 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 363 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 364
xorjoep 1:24714b45cd1b 365 #else
xorjoep 1:24714b45cd1b 366
xorjoep 1:24714b45cd1b 367 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 368 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 369 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 370 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 371
xorjoep 1:24714b45cd1b 372 #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
xorjoep 1:24714b45cd1b 373
xorjoep 1:24714b45cd1b 374 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 375 i--;
xorjoep 1:24714b45cd1b 376 }
xorjoep 1:24714b45cd1b 377
xorjoep 1:24714b45cd1b 378 i = ((uint32_t) phaseLen - 1U) % 0x04U;
xorjoep 1:24714b45cd1b 379
xorjoep 1:24714b45cd1b 380 while (i > 0U)
xorjoep 1:24714b45cd1b 381 {
xorjoep 1:24714b45cd1b 382 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 383
xorjoep 1:24714b45cd1b 384 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 385 i--;
xorjoep 1:24714b45cd1b 386 }
xorjoep 1:24714b45cd1b 387 }
xorjoep 1:24714b45cd1b 388
xorjoep 1:24714b45cd1b 389 #else
xorjoep 1:24714b45cd1b 390
xorjoep 1:24714b45cd1b 391 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 392
xorjoep 1:24714b45cd1b 393 void arm_fir_interpolate_q15(
xorjoep 1:24714b45cd1b 394 const arm_fir_interpolate_instance_q15 * S,
xorjoep 1:24714b45cd1b 395 q15_t * pSrc,
xorjoep 1:24714b45cd1b 396 q15_t * pDst,
xorjoep 1:24714b45cd1b 397 uint32_t blockSize)
xorjoep 1:24714b45cd1b 398 {
xorjoep 1:24714b45cd1b 399 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 400 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 401 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 402 q15_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
xorjoep 1:24714b45cd1b 403 q63_t sum; /* Accumulator */
xorjoep 1:24714b45cd1b 404 q15_t x0, c0; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 405 uint32_t i, blkCnt, tapCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 406 uint16_t phaseLen = S->phaseLength; /* Length of each polyphase filter component */
xorjoep 1:24714b45cd1b 407
xorjoep 1:24714b45cd1b 408
xorjoep 1:24714b45cd1b 409 /* S->pState buffer contains previous frame (phaseLen - 1) samples */
xorjoep 1:24714b45cd1b 410 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 411 pStateCurnt = S->pState + (phaseLen - 1U);
xorjoep 1:24714b45cd1b 412
xorjoep 1:24714b45cd1b 413 /* Total number of intput samples */
xorjoep 1:24714b45cd1b 414 blkCnt = blockSize;
xorjoep 1:24714b45cd1b 415
xorjoep 1:24714b45cd1b 416 /* Loop over the blockSize. */
xorjoep 1:24714b45cd1b 417 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 418 {
xorjoep 1:24714b45cd1b 419 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 420 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 421
xorjoep 1:24714b45cd1b 422 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 423 i = S->L;
xorjoep 1:24714b45cd1b 424
xorjoep 1:24714b45cd1b 425 while (i > 0U)
xorjoep 1:24714b45cd1b 426 {
xorjoep 1:24714b45cd1b 427 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 428 sum = 0;
xorjoep 1:24714b45cd1b 429
xorjoep 1:24714b45cd1b 430 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 431 ptr1 = pState;
xorjoep 1:24714b45cd1b 432
xorjoep 1:24714b45cd1b 433 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 434 ptr2 = pCoeffs + (i - 1U);
xorjoep 1:24714b45cd1b 435
xorjoep 1:24714b45cd1b 436 /* Loop over the polyPhase length */
xorjoep 1:24714b45cd1b 437 tapCnt = (uint32_t) phaseLen;
xorjoep 1:24714b45cd1b 438
xorjoep 1:24714b45cd1b 439 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 440 {
xorjoep 1:24714b45cd1b 441 /* Read the coefficient */
xorjoep 1:24714b45cd1b 442 c0 = *ptr2;
xorjoep 1:24714b45cd1b 443
xorjoep 1:24714b45cd1b 444 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 445 ptr2 += S->L;
xorjoep 1:24714b45cd1b 446
xorjoep 1:24714b45cd1b 447 /* Read the input sample */
xorjoep 1:24714b45cd1b 448 x0 = *ptr1++;
xorjoep 1:24714b45cd1b 449
xorjoep 1:24714b45cd1b 450 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 451 sum += ((q31_t) x0 * c0);
xorjoep 1:24714b45cd1b 452
xorjoep 1:24714b45cd1b 453 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 454 tapCnt--;
xorjoep 1:24714b45cd1b 455 }
xorjoep 1:24714b45cd1b 456
xorjoep 1:24714b45cd1b 457 /* Store the result after converting to 1.15 format in the destination buffer */
xorjoep 1:24714b45cd1b 458 *pDst++ = (q15_t) (__SSAT((sum >> 15), 16));
xorjoep 1:24714b45cd1b 459
xorjoep 1:24714b45cd1b 460 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 461 i--;
xorjoep 1:24714b45cd1b 462 }
xorjoep 1:24714b45cd1b 463
xorjoep 1:24714b45cd1b 464 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 465 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 466 pState = pState + 1;
xorjoep 1:24714b45cd1b 467
xorjoep 1:24714b45cd1b 468 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 469 blkCnt--;
xorjoep 1:24714b45cd1b 470 }
xorjoep 1:24714b45cd1b 471
xorjoep 1:24714b45cd1b 472 /* Processing is complete.
xorjoep 1:24714b45cd1b 473 ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
xorjoep 1:24714b45cd1b 474 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 475
xorjoep 1:24714b45cd1b 476 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 477 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 478
xorjoep 1:24714b45cd1b 479 i = (uint32_t) phaseLen - 1U;
xorjoep 1:24714b45cd1b 480
xorjoep 1:24714b45cd1b 481 while (i > 0U)
xorjoep 1:24714b45cd1b 482 {
xorjoep 1:24714b45cd1b 483 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 484
xorjoep 1:24714b45cd1b 485 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 486 i--;
xorjoep 1:24714b45cd1b 487 }
xorjoep 1:24714b45cd1b 488
xorjoep 1:24714b45cd1b 489 }
xorjoep 1:24714b45cd1b 490
xorjoep 1:24714b45cd1b 491 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 492
xorjoep 1:24714b45cd1b 493
xorjoep 1:24714b45cd1b 494 /**
xorjoep 1:24714b45cd1b 495 * @} end of FIR_Interpolate group
xorjoep 1:24714b45cd1b 496 */