Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

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

functions/FilteringFunctions/arm_fir_interpolate_f32.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_f32.c
xorjoep 1:24714b45cd1b 4 * Description: Floating-point FIR interpolation sequences
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 * @defgroup FIR_Interpolate Finite Impulse Response (FIR) Interpolator
xorjoep 1:24714b45cd1b 33 *
xorjoep 1:24714b45cd1b 34 * These functions combine an upsampler (zero stuffer) and an FIR filter.
xorjoep 1:24714b45cd1b 35 * They are used in multirate systems for increasing the sample rate of a signal without introducing high frequency images.
xorjoep 1:24714b45cd1b 36 * Conceptually, the functions are equivalent to the block diagram below:
xorjoep 1:24714b45cd1b 37 * \image html FIRInterpolator.gif "Components included in the FIR Interpolator functions"
xorjoep 1:24714b45cd1b 38 * After upsampling by a factor of <code>L</code>, the signal should be filtered by a lowpass filter with a normalized
xorjoep 1:24714b45cd1b 39 * cutoff frequency of <code>1/L</code> in order to eliminate high frequency copies of the spectrum.
xorjoep 1:24714b45cd1b 40 * The user of the function is responsible for providing the filter coefficients.
xorjoep 1:24714b45cd1b 41 *
xorjoep 1:24714b45cd1b 42 * The FIR interpolator functions provided in the CMSIS DSP Library combine the upsampler and FIR filter in an efficient manner.
xorjoep 1:24714b45cd1b 43 * The upsampler inserts <code>L-1</code> zeros between each sample.
xorjoep 1:24714b45cd1b 44 * Instead of multiplying by these zero values, the FIR filter is designed to skip them.
xorjoep 1:24714b45cd1b 45 * This leads to an efficient implementation without any wasted effort.
xorjoep 1:24714b45cd1b 46 * The functions operate on blocks of input and output data.
xorjoep 1:24714b45cd1b 47 * <code>pSrc</code> points to an array of <code>blockSize</code> input values and
xorjoep 1:24714b45cd1b 48 * <code>pDst</code> points to an array of <code>blockSize*L</code> output values.
xorjoep 1:24714b45cd1b 49 *
xorjoep 1:24714b45cd1b 50 * The library provides separate functions for Q15, Q31, and floating-point data types.
xorjoep 1:24714b45cd1b 51 *
xorjoep 1:24714b45cd1b 52 * \par Algorithm:
xorjoep 1:24714b45cd1b 53 * The functions use a polyphase filter structure:
xorjoep 1:24714b45cd1b 54 * <pre>
xorjoep 1:24714b45cd1b 55 * y[n] = b[0] * x[n] + b[L] * x[n-1] + ... + b[L*(phaseLength-1)] * x[n-phaseLength+1]
xorjoep 1:24714b45cd1b 56 * y[n+1] = b[1] * x[n] + b[L+1] * x[n-1] + ... + b[L*(phaseLength-1)+1] * x[n-phaseLength+1]
xorjoep 1:24714b45cd1b 57 * ...
xorjoep 1:24714b45cd1b 58 * y[n+(L-1)] = b[L-1] * x[n] + b[2*L-1] * x[n-1] + ....+ b[L*(phaseLength-1)+(L-1)] * x[n-phaseLength+1]
xorjoep 1:24714b45cd1b 59 * </pre>
xorjoep 1:24714b45cd1b 60 * This approach is more efficient than straightforward upsample-then-filter algorithms.
xorjoep 1:24714b45cd1b 61 * With this method the computation is reduced by a factor of <code>1/L</code> when compared to using a standard FIR filter.
xorjoep 1:24714b45cd1b 62 * \par
xorjoep 1:24714b45cd1b 63 * <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
xorjoep 1:24714b45cd1b 64 * <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code> and this is checked by the
xorjoep 1:24714b45cd1b 65 * initialization functions.
xorjoep 1:24714b45cd1b 66 * Internally, the function divides the FIR filter's impulse response into shorter filters of length
xorjoep 1:24714b45cd1b 67 * <code>phaseLength=numTaps/L</code>.
xorjoep 1:24714b45cd1b 68 * Coefficients are stored in time reversed order.
xorjoep 1:24714b45cd1b 69 * \par
xorjoep 1:24714b45cd1b 70 * <pre>
xorjoep 1:24714b45cd1b 71 * {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
xorjoep 1:24714b45cd1b 72 * </pre>
xorjoep 1:24714b45cd1b 73 * \par
xorjoep 1:24714b45cd1b 74 * <code>pState</code> points to a state array of size <code>blockSize + phaseLength - 1</code>.
xorjoep 1:24714b45cd1b 75 * Samples in the state buffer are stored in the order:
xorjoep 1:24714b45cd1b 76 * \par
xorjoep 1:24714b45cd1b 77 * <pre>
xorjoep 1:24714b45cd1b 78 * {x[n-phaseLength+1], x[n-phaseLength], x[n-phaseLength-1], x[n-phaseLength-2]....x[0], x[1], ..., x[blockSize-1]}
xorjoep 1:24714b45cd1b 79 * </pre>
xorjoep 1:24714b45cd1b 80 * The state variables are updated after each block of data is processed, the coefficients are untouched.
xorjoep 1:24714b45cd1b 81 *
xorjoep 1:24714b45cd1b 82 * \par Instance Structure
xorjoep 1:24714b45cd1b 83 * The coefficients and state variables for a filter are stored together in an instance data structure.
xorjoep 1:24714b45cd1b 84 * A separate instance structure must be defined for each filter.
xorjoep 1:24714b45cd1b 85 * Coefficient arrays may be shared among several instances while state variable array should be allocated separately.
xorjoep 1:24714b45cd1b 86 * There are separate instance structure declarations for each of the 3 supported data types.
xorjoep 1:24714b45cd1b 87 *
xorjoep 1:24714b45cd1b 88 * \par Initialization Functions
xorjoep 1:24714b45cd1b 89 * There is also an associated initialization function for each data type.
xorjoep 1:24714b45cd1b 90 * The initialization function performs the following operations:
xorjoep 1:24714b45cd1b 91 * - Sets the values of the internal structure fields.
xorjoep 1:24714b45cd1b 92 * - Zeros out the values in the state buffer.
xorjoep 1:24714b45cd1b 93 * - Checks to make sure that the length of the filter is a multiple of the interpolation factor.
xorjoep 1:24714b45cd1b 94 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
xorjoep 1:24714b45cd1b 95 * L (interpolation factor), pCoeffs, phaseLength (numTaps / L), pState. Also set all of the values in pState to zero.
xorjoep 1:24714b45cd1b 96 *
xorjoep 1:24714b45cd1b 97 * \par
xorjoep 1:24714b45cd1b 98 * Use of the initialization function is optional.
xorjoep 1:24714b45cd1b 99 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
xorjoep 1:24714b45cd1b 100 * To place an instance structure into a const data section, the instance structure must be manually initialized.
xorjoep 1:24714b45cd1b 101 * The code below statically initializes each of the 3 different data type filter instance structures
xorjoep 1:24714b45cd1b 102 * <pre>
xorjoep 1:24714b45cd1b 103 * arm_fir_interpolate_instance_f32 S = {L, phaseLength, pCoeffs, pState};
xorjoep 1:24714b45cd1b 104 * arm_fir_interpolate_instance_q31 S = {L, phaseLength, pCoeffs, pState};
xorjoep 1:24714b45cd1b 105 * arm_fir_interpolate_instance_q15 S = {L, phaseLength, pCoeffs, pState};
xorjoep 1:24714b45cd1b 106 * </pre>
xorjoep 1:24714b45cd1b 107 * where <code>L</code> is the interpolation factor; <code>phaseLength=numTaps/L</code> is the
xorjoep 1:24714b45cd1b 108 * length of each of the shorter FIR filters used internally,
xorjoep 1:24714b45cd1b 109 * <code>pCoeffs</code> is the address of the coefficient buffer;
xorjoep 1:24714b45cd1b 110 * <code>pState</code> is the address of the state buffer.
xorjoep 1:24714b45cd1b 111 * Be sure to set the values in the state buffer to zeros when doing static initialization.
xorjoep 1:24714b45cd1b 112 *
xorjoep 1:24714b45cd1b 113 * \par Fixed-Point Behavior
xorjoep 1:24714b45cd1b 114 * Care must be taken when using the fixed-point versions of the FIR interpolate filter functions.
xorjoep 1:24714b45cd1b 115 * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
xorjoep 1:24714b45cd1b 116 * Refer to the function specific documentation below for usage guidelines.
xorjoep 1:24714b45cd1b 117 */
xorjoep 1:24714b45cd1b 118
xorjoep 1:24714b45cd1b 119 /**
xorjoep 1:24714b45cd1b 120 * @addtogroup FIR_Interpolate
xorjoep 1:24714b45cd1b 121 * @{
xorjoep 1:24714b45cd1b 122 */
xorjoep 1:24714b45cd1b 123
xorjoep 1:24714b45cd1b 124 /**
xorjoep 1:24714b45cd1b 125 * @brief Processing function for the floating-point FIR interpolator.
xorjoep 1:24714b45cd1b 126 * @param[in] *S points to an instance of the floating-point FIR interpolator structure.
xorjoep 1:24714b45cd1b 127 * @param[in] *pSrc points to the block of input data.
xorjoep 1:24714b45cd1b 128 * @param[out] *pDst points to the block of output data.
xorjoep 1:24714b45cd1b 129 * @param[in] blockSize number of input samples to process per call.
xorjoep 1:24714b45cd1b 130 * @return none.
xorjoep 1:24714b45cd1b 131 */
xorjoep 1:24714b45cd1b 132 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 133
xorjoep 1:24714b45cd1b 134 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 135
xorjoep 1:24714b45cd1b 136 void arm_fir_interpolate_f32(
xorjoep 1:24714b45cd1b 137 const arm_fir_interpolate_instance_f32 * S,
xorjoep 1:24714b45cd1b 138 float32_t * pSrc,
xorjoep 1:24714b45cd1b 139 float32_t * pDst,
xorjoep 1:24714b45cd1b 140 uint32_t blockSize)
xorjoep 1:24714b45cd1b 141 {
xorjoep 1:24714b45cd1b 142 float32_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 143 float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 144 float32_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 145 float32_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
xorjoep 1:24714b45cd1b 146 float32_t sum0; /* Accumulators */
xorjoep 1:24714b45cd1b 147 float32_t x0, c0; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 148 uint32_t i, blkCnt, j; /* Loop counters */
xorjoep 1:24714b45cd1b 149 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */
xorjoep 1:24714b45cd1b 150 float32_t acc0, acc1, acc2, acc3;
xorjoep 1:24714b45cd1b 151 float32_t x1, x2, x3;
xorjoep 1:24714b45cd1b 152 uint32_t blkCntN4;
xorjoep 1:24714b45cd1b 153 float32_t c1, c2, c3;
xorjoep 1:24714b45cd1b 154
xorjoep 1:24714b45cd1b 155 /* S->pState buffer contains previous frame (phaseLen - 1) samples */
xorjoep 1:24714b45cd1b 156 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 157 pStateCurnt = S->pState + (phaseLen - 1U);
xorjoep 1:24714b45cd1b 158
xorjoep 1:24714b45cd1b 159 /* Initialise blkCnt */
xorjoep 1:24714b45cd1b 160 blkCnt = blockSize / 4;
xorjoep 1:24714b45cd1b 161 blkCntN4 = blockSize - (4 * blkCnt);
xorjoep 1:24714b45cd1b 162
xorjoep 1:24714b45cd1b 163 /* Samples loop unrolled by 4 */
xorjoep 1:24714b45cd1b 164 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 165 {
xorjoep 1:24714b45cd1b 166 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 167 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 168 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 169 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 170 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 171
xorjoep 1:24714b45cd1b 172 /* Address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 173 j = 1U;
xorjoep 1:24714b45cd1b 174
xorjoep 1:24714b45cd1b 175 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 176 i = (S->L);
xorjoep 1:24714b45cd1b 177
xorjoep 1:24714b45cd1b 178 while (i > 0U)
xorjoep 1:24714b45cd1b 179 {
xorjoep 1:24714b45cd1b 180 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 181 acc0 = 0.0f;
xorjoep 1:24714b45cd1b 182 acc1 = 0.0f;
xorjoep 1:24714b45cd1b 183 acc2 = 0.0f;
xorjoep 1:24714b45cd1b 184 acc3 = 0.0f;
xorjoep 1:24714b45cd1b 185
xorjoep 1:24714b45cd1b 186 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 187 ptr1 = pState;
xorjoep 1:24714b45cd1b 188
xorjoep 1:24714b45cd1b 189 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 190 ptr2 = pCoeffs + (S->L - j);
xorjoep 1:24714b45cd1b 191
xorjoep 1:24714b45cd1b 192 /* Loop over the polyPhase length. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 193 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
xorjoep 1:24714b45cd1b 194 tapCnt = phaseLen >> 2U;
xorjoep 1:24714b45cd1b 195
xorjoep 1:24714b45cd1b 196 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 197 x1 = *(ptr1++);
xorjoep 1:24714b45cd1b 198 x2 = *(ptr1++);
xorjoep 1:24714b45cd1b 199
xorjoep 1:24714b45cd1b 200 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 201 {
xorjoep 1:24714b45cd1b 202
xorjoep 1:24714b45cd1b 203 /* Read the input sample */
xorjoep 1:24714b45cd1b 204 x3 = *(ptr1++);
xorjoep 1:24714b45cd1b 205
xorjoep 1:24714b45cd1b 206 /* Read the coefficient */
xorjoep 1:24714b45cd1b 207 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 208
xorjoep 1:24714b45cd1b 209 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 210 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 211 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 212 acc2 += x2 * c0;
xorjoep 1:24714b45cd1b 213 acc3 += x3 * c0;
xorjoep 1:24714b45cd1b 214
xorjoep 1:24714b45cd1b 215 /* Read the coefficient */
xorjoep 1:24714b45cd1b 216 c1 = *(ptr2 + S->L);
xorjoep 1:24714b45cd1b 217
xorjoep 1:24714b45cd1b 218 /* Read the input sample */
xorjoep 1:24714b45cd1b 219 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 220
xorjoep 1:24714b45cd1b 221 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 222 acc0 += x1 * c1;
xorjoep 1:24714b45cd1b 223 acc1 += x2 * c1;
xorjoep 1:24714b45cd1b 224 acc2 += x3 * c1;
xorjoep 1:24714b45cd1b 225 acc3 += x0 * c1;
xorjoep 1:24714b45cd1b 226
xorjoep 1:24714b45cd1b 227 /* Read the coefficient */
xorjoep 1:24714b45cd1b 228 c2 = *(ptr2 + S->L * 2);
xorjoep 1:24714b45cd1b 229
xorjoep 1:24714b45cd1b 230 /* Read the input sample */
xorjoep 1:24714b45cd1b 231 x1 = *(ptr1++);
xorjoep 1:24714b45cd1b 232
xorjoep 1:24714b45cd1b 233 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 234 acc0 += x2 * c2;
xorjoep 1:24714b45cd1b 235 acc1 += x3 * c2;
xorjoep 1:24714b45cd1b 236 acc2 += x0 * c2;
xorjoep 1:24714b45cd1b 237 acc3 += x1 * c2;
xorjoep 1:24714b45cd1b 238
xorjoep 1:24714b45cd1b 239 /* Read the coefficient */
xorjoep 1:24714b45cd1b 240 c3 = *(ptr2 + S->L * 3);
xorjoep 1:24714b45cd1b 241
xorjoep 1:24714b45cd1b 242 /* Read the input sample */
xorjoep 1:24714b45cd1b 243 x2 = *(ptr1++);
xorjoep 1:24714b45cd1b 244
xorjoep 1:24714b45cd1b 245 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 246 acc0 += x3 * c3;
xorjoep 1:24714b45cd1b 247 acc1 += x0 * c3;
xorjoep 1:24714b45cd1b 248 acc2 += x1 * c3;
xorjoep 1:24714b45cd1b 249 acc3 += x2 * c3;
xorjoep 1:24714b45cd1b 250
xorjoep 1:24714b45cd1b 251
xorjoep 1:24714b45cd1b 252 /* Upsampling is done by stuffing L-1 zeros between each sample.
xorjoep 1:24714b45cd1b 253 * So instead of multiplying zeros with coefficients,
xorjoep 1:24714b45cd1b 254 * Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 255 ptr2 += 4 * S->L;
xorjoep 1:24714b45cd1b 256
xorjoep 1:24714b45cd1b 257 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 258 tapCnt--;
xorjoep 1:24714b45cd1b 259 }
xorjoep 1:24714b45cd1b 260
xorjoep 1:24714b45cd1b 261 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 262 tapCnt = phaseLen % 0x4U;
xorjoep 1:24714b45cd1b 263
xorjoep 1:24714b45cd1b 264 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 265 {
xorjoep 1:24714b45cd1b 266
xorjoep 1:24714b45cd1b 267 /* Read the input sample */
xorjoep 1:24714b45cd1b 268 x3 = *(ptr1++);
xorjoep 1:24714b45cd1b 269
xorjoep 1:24714b45cd1b 270 /* Read the coefficient */
xorjoep 1:24714b45cd1b 271 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 272
xorjoep 1:24714b45cd1b 273 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 274 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 275 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 276 acc2 += x2 * c0;
xorjoep 1:24714b45cd1b 277 acc3 += x3 * c0;
xorjoep 1:24714b45cd1b 278
xorjoep 1:24714b45cd1b 279 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 280 ptr2 += S->L;
xorjoep 1:24714b45cd1b 281
xorjoep 1:24714b45cd1b 282 /* update states for next sample processing */
xorjoep 1:24714b45cd1b 283 x0 = x1;
xorjoep 1:24714b45cd1b 284 x1 = x2;
xorjoep 1:24714b45cd1b 285 x2 = x3;
xorjoep 1:24714b45cd1b 286
xorjoep 1:24714b45cd1b 287 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 288 tapCnt--;
xorjoep 1:24714b45cd1b 289 }
xorjoep 1:24714b45cd1b 290
xorjoep 1:24714b45cd1b 291 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 292 *pDst = acc0;
xorjoep 1:24714b45cd1b 293 *(pDst + S->L) = acc1;
xorjoep 1:24714b45cd1b 294 *(pDst + 2 * S->L) = acc2;
xorjoep 1:24714b45cd1b 295 *(pDst + 3 * S->L) = acc3;
xorjoep 1:24714b45cd1b 296
xorjoep 1:24714b45cd1b 297 pDst++;
xorjoep 1:24714b45cd1b 298
xorjoep 1:24714b45cd1b 299 /* Increment the address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 300 j++;
xorjoep 1:24714b45cd1b 301
xorjoep 1:24714b45cd1b 302 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 303 i--;
xorjoep 1:24714b45cd1b 304 }
xorjoep 1:24714b45cd1b 305
xorjoep 1:24714b45cd1b 306 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 307 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 308 pState = pState + 4;
xorjoep 1:24714b45cd1b 309
xorjoep 1:24714b45cd1b 310 pDst += S->L * 3;
xorjoep 1:24714b45cd1b 311
xorjoep 1:24714b45cd1b 312 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 313 blkCnt--;
xorjoep 1:24714b45cd1b 314 }
xorjoep 1:24714b45cd1b 315
xorjoep 1:24714b45cd1b 316 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 317 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 318
xorjoep 1:24714b45cd1b 319 while (blkCntN4 > 0U)
xorjoep 1:24714b45cd1b 320 {
xorjoep 1:24714b45cd1b 321 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 322 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 323
xorjoep 1:24714b45cd1b 324 /* Address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 325 j = 1U;
xorjoep 1:24714b45cd1b 326
xorjoep 1:24714b45cd1b 327 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 328 i = S->L;
xorjoep 1:24714b45cd1b 329 while (i > 0U)
xorjoep 1:24714b45cd1b 330 {
xorjoep 1:24714b45cd1b 331 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 332 sum0 = 0.0f;
xorjoep 1:24714b45cd1b 333
xorjoep 1:24714b45cd1b 334 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 335 ptr1 = pState;
xorjoep 1:24714b45cd1b 336
xorjoep 1:24714b45cd1b 337 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 338 ptr2 = pCoeffs + (S->L - j);
xorjoep 1:24714b45cd1b 339
xorjoep 1:24714b45cd1b 340 /* Loop over the polyPhase length. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 341 ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
xorjoep 1:24714b45cd1b 342 tapCnt = phaseLen >> 2U;
xorjoep 1:24714b45cd1b 343 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 344 {
xorjoep 1:24714b45cd1b 345
xorjoep 1:24714b45cd1b 346 /* Read the coefficient */
xorjoep 1:24714b45cd1b 347 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 348
xorjoep 1:24714b45cd1b 349 /* Upsampling is done by stuffing L-1 zeros between each sample.
xorjoep 1:24714b45cd1b 350 * So instead of multiplying zeros with coefficients,
xorjoep 1:24714b45cd1b 351 * Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 352 ptr2 += S->L;
xorjoep 1:24714b45cd1b 353
xorjoep 1:24714b45cd1b 354 /* Read the input sample */
xorjoep 1:24714b45cd1b 355 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 356
xorjoep 1:24714b45cd1b 357 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 358 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 359
xorjoep 1:24714b45cd1b 360 /* Read the coefficient */
xorjoep 1:24714b45cd1b 361 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 362
xorjoep 1:24714b45cd1b 363 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 364 ptr2 += S->L;
xorjoep 1:24714b45cd1b 365
xorjoep 1:24714b45cd1b 366 /* Read the input sample */
xorjoep 1:24714b45cd1b 367 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 368
xorjoep 1:24714b45cd1b 369 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 370 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 371
xorjoep 1:24714b45cd1b 372 /* Read the coefficient */
xorjoep 1:24714b45cd1b 373 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 374
xorjoep 1:24714b45cd1b 375 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 376 ptr2 += S->L;
xorjoep 1:24714b45cd1b 377
xorjoep 1:24714b45cd1b 378 /* Read the input sample */
xorjoep 1:24714b45cd1b 379 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 380
xorjoep 1:24714b45cd1b 381 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 382 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 383
xorjoep 1:24714b45cd1b 384 /* Read the coefficient */
xorjoep 1:24714b45cd1b 385 c0 = *(ptr2);
xorjoep 1:24714b45cd1b 386
xorjoep 1:24714b45cd1b 387 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 388 ptr2 += S->L;
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 /* Read the input sample */
xorjoep 1:24714b45cd1b 391 x0 = *(ptr1++);
xorjoep 1:24714b45cd1b 392
xorjoep 1:24714b45cd1b 393 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 394 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 395
xorjoep 1:24714b45cd1b 396 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 397 tapCnt--;
xorjoep 1:24714b45cd1b 398 }
xorjoep 1:24714b45cd1b 399
xorjoep 1:24714b45cd1b 400 /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 401 tapCnt = phaseLen % 0x4U;
xorjoep 1:24714b45cd1b 402
xorjoep 1:24714b45cd1b 403 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 404 {
xorjoep 1:24714b45cd1b 405 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 406 sum0 += *(ptr1++) * (*ptr2);
xorjoep 1:24714b45cd1b 407
xorjoep 1:24714b45cd1b 408 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 409 ptr2 += S->L;
xorjoep 1:24714b45cd1b 410
xorjoep 1:24714b45cd1b 411 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 412 tapCnt--;
xorjoep 1:24714b45cd1b 413 }
xorjoep 1:24714b45cd1b 414
xorjoep 1:24714b45cd1b 415 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 416 *pDst++ = sum0;
xorjoep 1:24714b45cd1b 417
xorjoep 1:24714b45cd1b 418 /* Increment the address modifier index of coefficient buffer */
xorjoep 1:24714b45cd1b 419 j++;
xorjoep 1:24714b45cd1b 420
xorjoep 1:24714b45cd1b 421 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 422 i--;
xorjoep 1:24714b45cd1b 423 }
xorjoep 1:24714b45cd1b 424
xorjoep 1:24714b45cd1b 425 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 426 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 427 pState = pState + 1;
xorjoep 1:24714b45cd1b 428
xorjoep 1:24714b45cd1b 429 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 430 blkCntN4--;
xorjoep 1:24714b45cd1b 431 }
xorjoep 1:24714b45cd1b 432
xorjoep 1:24714b45cd1b 433 /* Processing is complete.
xorjoep 1:24714b45cd1b 434 ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 435 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 436
xorjoep 1:24714b45cd1b 437 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 438 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 439
xorjoep 1:24714b45cd1b 440 tapCnt = (phaseLen - 1U) >> 2U;
xorjoep 1:24714b45cd1b 441
xorjoep 1:24714b45cd1b 442 /* copy data */
xorjoep 1:24714b45cd1b 443 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 444 {
xorjoep 1:24714b45cd1b 445 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 446 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 447 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 448 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 449
xorjoep 1:24714b45cd1b 450 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 451 tapCnt--;
xorjoep 1:24714b45cd1b 452 }
xorjoep 1:24714b45cd1b 453
xorjoep 1:24714b45cd1b 454 tapCnt = (phaseLen - 1U) % 0x04U;
xorjoep 1:24714b45cd1b 455
xorjoep 1:24714b45cd1b 456 /* copy data */
xorjoep 1:24714b45cd1b 457 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 458 {
xorjoep 1:24714b45cd1b 459 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 460
xorjoep 1:24714b45cd1b 461 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 462 tapCnt--;
xorjoep 1:24714b45cd1b 463 }
xorjoep 1:24714b45cd1b 464 }
xorjoep 1:24714b45cd1b 465
xorjoep 1:24714b45cd1b 466 #else
xorjoep 1:24714b45cd1b 467
xorjoep 1:24714b45cd1b 468 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 469
xorjoep 1:24714b45cd1b 470 void arm_fir_interpolate_f32(
xorjoep 1:24714b45cd1b 471 const arm_fir_interpolate_instance_f32 * S,
xorjoep 1:24714b45cd1b 472 float32_t * pSrc,
xorjoep 1:24714b45cd1b 473 float32_t * pDst,
xorjoep 1:24714b45cd1b 474 uint32_t blockSize)
xorjoep 1:24714b45cd1b 475 {
xorjoep 1:24714b45cd1b 476 float32_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 477 float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 478 float32_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 479 float32_t *ptr1, *ptr2; /* Temporary pointers for state and coefficient buffers */
xorjoep 1:24714b45cd1b 480
xorjoep 1:24714b45cd1b 481
xorjoep 1:24714b45cd1b 482 float32_t sum; /* Accumulator */
xorjoep 1:24714b45cd1b 483 uint32_t i, blkCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 484 uint16_t phaseLen = S->phaseLength, tapCnt; /* Length of each polyphase filter component */
xorjoep 1:24714b45cd1b 485
xorjoep 1:24714b45cd1b 486
xorjoep 1:24714b45cd1b 487 /* S->pState buffer contains previous frame (phaseLen - 1) samples */
xorjoep 1:24714b45cd1b 488 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 489 pStateCurnt = S->pState + (phaseLen - 1U);
xorjoep 1:24714b45cd1b 490
xorjoep 1:24714b45cd1b 491 /* Total number of intput samples */
xorjoep 1:24714b45cd1b 492 blkCnt = blockSize;
xorjoep 1:24714b45cd1b 493
xorjoep 1:24714b45cd1b 494 /* Loop over the blockSize. */
xorjoep 1:24714b45cd1b 495 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 496 {
xorjoep 1:24714b45cd1b 497 /* Copy new input sample into the state buffer */
xorjoep 1:24714b45cd1b 498 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 499
xorjoep 1:24714b45cd1b 500 /* Loop over the Interpolation factor. */
xorjoep 1:24714b45cd1b 501 i = S->L;
xorjoep 1:24714b45cd1b 502
xorjoep 1:24714b45cd1b 503 while (i > 0U)
xorjoep 1:24714b45cd1b 504 {
xorjoep 1:24714b45cd1b 505 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 506 sum = 0.0f;
xorjoep 1:24714b45cd1b 507
xorjoep 1:24714b45cd1b 508 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 509 ptr1 = pState;
xorjoep 1:24714b45cd1b 510
xorjoep 1:24714b45cd1b 511 /* Initialize coefficient pointer */
xorjoep 1:24714b45cd1b 512 ptr2 = pCoeffs + (i - 1U);
xorjoep 1:24714b45cd1b 513
xorjoep 1:24714b45cd1b 514 /* Loop over the polyPhase length */
xorjoep 1:24714b45cd1b 515 tapCnt = phaseLen;
xorjoep 1:24714b45cd1b 516
xorjoep 1:24714b45cd1b 517 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 518 {
xorjoep 1:24714b45cd1b 519 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 520 sum += *ptr1++ * *ptr2;
xorjoep 1:24714b45cd1b 521
xorjoep 1:24714b45cd1b 522 /* Increment the coefficient pointer by interpolation factor times. */
xorjoep 1:24714b45cd1b 523 ptr2 += S->L;
xorjoep 1:24714b45cd1b 524
xorjoep 1:24714b45cd1b 525 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 526 tapCnt--;
xorjoep 1:24714b45cd1b 527 }
xorjoep 1:24714b45cd1b 528
xorjoep 1:24714b45cd1b 529 /* The result is in the accumulator, store in the destination buffer. */
xorjoep 1:24714b45cd1b 530 *pDst++ = sum;
xorjoep 1:24714b45cd1b 531
xorjoep 1:24714b45cd1b 532 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 533 i--;
xorjoep 1:24714b45cd1b 534 }
xorjoep 1:24714b45cd1b 535
xorjoep 1:24714b45cd1b 536 /* Advance the state pointer by 1
xorjoep 1:24714b45cd1b 537 * to process the next group of interpolation factor number samples */
xorjoep 1:24714b45cd1b 538 pState = pState + 1;
xorjoep 1:24714b45cd1b 539
xorjoep 1:24714b45cd1b 540 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 541 blkCnt--;
xorjoep 1:24714b45cd1b 542 }
xorjoep 1:24714b45cd1b 543
xorjoep 1:24714b45cd1b 544 /* Processing is complete.
xorjoep 1:24714b45cd1b 545 ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
xorjoep 1:24714b45cd1b 546 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 547
xorjoep 1:24714b45cd1b 548 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 549 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 550
xorjoep 1:24714b45cd1b 551 tapCnt = phaseLen - 1U;
xorjoep 1:24714b45cd1b 552
xorjoep 1:24714b45cd1b 553 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 554 {
xorjoep 1:24714b45cd1b 555 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 556
xorjoep 1:24714b45cd1b 557 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 558 tapCnt--;
xorjoep 1:24714b45cd1b 559 }
xorjoep 1:24714b45cd1b 560
xorjoep 1:24714b45cd1b 561 }
xorjoep 1:24714b45cd1b 562
xorjoep 1:24714b45cd1b 563 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 564
xorjoep 1:24714b45cd1b 565
xorjoep 1:24714b45cd1b 566
xorjoep 1:24714b45cd1b 567 /**
xorjoep 1:24714b45cd1b 568 * @} end of FIR_Interpolate group
xorjoep 1:24714b45cd1b 569 */