Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

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

functions/FilteringFunctions/arm_fir_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_q15.c
xorjoep 1:24714b45cd1b 4 * Description: Q15 FIR filter processing function
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
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 filter.
xorjoep 1:24714b45cd1b 42 * @param[in] *S points to an instance of the Q15 FIR 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 samples to process per call.
xorjoep 1:24714b45cd1b 46 * @return none.
xorjoep 1:24714b45cd1b 47 *
xorjoep 1:24714b45cd1b 48 *
xorjoep 1:24714b45cd1b 49 * \par Restrictions
xorjoep 1:24714b45cd1b 50 * If the silicon does not support unaligned memory access enable the macro UNALIGNED_SUPPORT_DISABLE
xorjoep 1:24714b45cd1b 51 * In this case input, output, state buffers should be aligned by 32-bit
xorjoep 1:24714b45cd1b 52 *
xorjoep 1:24714b45cd1b 53 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 54 * \par
xorjoep 1:24714b45cd1b 55 * The function is implemented using a 64-bit internal accumulator.
xorjoep 1:24714b45cd1b 56 * Both coefficients and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
xorjoep 1:24714b45cd1b 57 * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
xorjoep 1:24714b45cd1b 58 * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
xorjoep 1:24714b45cd1b 59 * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
xorjoep 1:24714b45cd1b 60 * Lastly, the accumulator is saturated to yield a result in 1.15 format.
xorjoep 1:24714b45cd1b 61 *
xorjoep 1:24714b45cd1b 62 * \par
xorjoep 1:24714b45cd1b 63 * Refer to the function <code>arm_fir_fast_q15()</code> for a faster but less precise implementation of this function.
xorjoep 1:24714b45cd1b 64 */
xorjoep 1:24714b45cd1b 65
xorjoep 1:24714b45cd1b 66 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 67
xorjoep 1:24714b45cd1b 68 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 69
xorjoep 1:24714b45cd1b 70 #ifndef UNALIGNED_SUPPORT_DISABLE
xorjoep 1:24714b45cd1b 71
xorjoep 1:24714b45cd1b 72
xorjoep 1:24714b45cd1b 73 void arm_fir_q15(
xorjoep 1:24714b45cd1b 74 const arm_fir_instance_q15 * S,
xorjoep 1:24714b45cd1b 75 q15_t * pSrc,
xorjoep 1:24714b45cd1b 76 q15_t * pDst,
xorjoep 1:24714b45cd1b 77 uint32_t blockSize)
xorjoep 1:24714b45cd1b 78 {
xorjoep 1:24714b45cd1b 79 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 80 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 81 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 82 q15_t *px1; /* Temporary q15 pointer for state buffer */
xorjoep 1:24714b45cd1b 83 q15_t *pb; /* Temporary pointer for coefficient buffer */
xorjoep 1:24714b45cd1b 84 q31_t x0, x1, x2, x3, c0; /* Temporary variables to hold SIMD state and coefficient values */
xorjoep 1:24714b45cd1b 85 q63_t acc0, acc1, acc2, acc3; /* Accumulators */
xorjoep 1:24714b45cd1b 86 uint32_t numTaps = S->numTaps; /* Number of taps in the filter */
xorjoep 1:24714b45cd1b 87 uint32_t tapCnt, blkCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 88
xorjoep 1:24714b45cd1b 89
xorjoep 1:24714b45cd1b 90 /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 91 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 92 pStateCurnt = &(S->pState[(numTaps - 1U)]);
xorjoep 1:24714b45cd1b 93
xorjoep 1:24714b45cd1b 94 /* Apply loop unrolling and compute 4 output values simultaneously.
xorjoep 1:24714b45cd1b 95 * The variables acc0 ... acc3 hold output values that are being computed:
xorjoep 1:24714b45cd1b 96 *
xorjoep 1:24714b45cd1b 97 * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
xorjoep 1:24714b45cd1b 98 * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
xorjoep 1:24714b45cd1b 99 * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
xorjoep 1:24714b45cd1b 100 * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
xorjoep 1:24714b45cd1b 101 */
xorjoep 1:24714b45cd1b 102
xorjoep 1:24714b45cd1b 103 blkCnt = blockSize >> 2;
xorjoep 1:24714b45cd1b 104
xorjoep 1:24714b45cd1b 105 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
xorjoep 1:24714b45cd1b 106 ** a second loop below computes the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 107 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 108 {
xorjoep 1:24714b45cd1b 109 /* Copy four new input samples into the state buffer.
xorjoep 1:24714b45cd1b 110 ** Use 32-bit SIMD to move the 16-bit data. Only requires two copies. */
xorjoep 1:24714b45cd1b 111 *__SIMD32(pStateCurnt)++ = *__SIMD32(pSrc)++;
xorjoep 1:24714b45cd1b 112 *__SIMD32(pStateCurnt)++ = *__SIMD32(pSrc)++;
xorjoep 1:24714b45cd1b 113
xorjoep 1:24714b45cd1b 114 /* Set all accumulators to zero */
xorjoep 1:24714b45cd1b 115 acc0 = 0;
xorjoep 1:24714b45cd1b 116 acc1 = 0;
xorjoep 1:24714b45cd1b 117 acc2 = 0;
xorjoep 1:24714b45cd1b 118 acc3 = 0;
xorjoep 1:24714b45cd1b 119
xorjoep 1:24714b45cd1b 120 /* Initialize state pointer of type q15 */
xorjoep 1:24714b45cd1b 121 px1 = pState;
xorjoep 1:24714b45cd1b 122
xorjoep 1:24714b45cd1b 123 /* Initialize coeff pointer of type q31 */
xorjoep 1:24714b45cd1b 124 pb = pCoeffs;
xorjoep 1:24714b45cd1b 125
xorjoep 1:24714b45cd1b 126 /* Read the first two samples from the state buffer: x[n-N], x[n-N-1] */
xorjoep 1:24714b45cd1b 127 x0 = _SIMD32_OFFSET(px1);
xorjoep 1:24714b45cd1b 128
xorjoep 1:24714b45cd1b 129 /* Read the third and forth samples from the state buffer: x[n-N-1], x[n-N-2] */
xorjoep 1:24714b45cd1b 130 x1 = _SIMD32_OFFSET(px1 + 1U);
xorjoep 1:24714b45cd1b 131
xorjoep 1:24714b45cd1b 132 px1 += 2U;
xorjoep 1:24714b45cd1b 133
xorjoep 1:24714b45cd1b 134 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 135 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 136 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 137
xorjoep 1:24714b45cd1b 138 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 139 {
xorjoep 1:24714b45cd1b 140 /* Read the first two coefficients using SIMD: b[N] and b[N-1] coefficients */
xorjoep 1:24714b45cd1b 141 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 142
xorjoep 1:24714b45cd1b 143 /* acc0 += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
xorjoep 1:24714b45cd1b 144 acc0 = __SMLALD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 145
xorjoep 1:24714b45cd1b 146 /* acc1 += b[N] * x[n-N-1] + b[N-1] * x[n-N-2] */
xorjoep 1:24714b45cd1b 147 acc1 = __SMLALD(x1, c0, acc1);
xorjoep 1:24714b45cd1b 148
xorjoep 1:24714b45cd1b 149 /* Read state x[n-N-2], x[n-N-3] */
xorjoep 1:24714b45cd1b 150 x2 = _SIMD32_OFFSET(px1);
xorjoep 1:24714b45cd1b 151
xorjoep 1:24714b45cd1b 152 /* Read state x[n-N-3], x[n-N-4] */
xorjoep 1:24714b45cd1b 153 x3 = _SIMD32_OFFSET(px1 + 1U);
xorjoep 1:24714b45cd1b 154
xorjoep 1:24714b45cd1b 155 /* acc2 += b[N] * x[n-N-2] + b[N-1] * x[n-N-3] */
xorjoep 1:24714b45cd1b 156 acc2 = __SMLALD(x2, c0, acc2);
xorjoep 1:24714b45cd1b 157
xorjoep 1:24714b45cd1b 158 /* acc3 += b[N] * x[n-N-3] + b[N-1] * x[n-N-4] */
xorjoep 1:24714b45cd1b 159 acc3 = __SMLALD(x3, c0, acc3);
xorjoep 1:24714b45cd1b 160
xorjoep 1:24714b45cd1b 161 /* Read coefficients b[N-2], b[N-3] */
xorjoep 1:24714b45cd1b 162 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 163
xorjoep 1:24714b45cd1b 164 /* acc0 += b[N-2] * x[n-N-2] + b[N-3] * x[n-N-3] */
xorjoep 1:24714b45cd1b 165 acc0 = __SMLALD(x2, c0, acc0);
xorjoep 1:24714b45cd1b 166
xorjoep 1:24714b45cd1b 167 /* acc1 += b[N-2] * x[n-N-3] + b[N-3] * x[n-N-4] */
xorjoep 1:24714b45cd1b 168 acc1 = __SMLALD(x3, c0, acc1);
xorjoep 1:24714b45cd1b 169
xorjoep 1:24714b45cd1b 170 /* Read state x[n-N-4], x[n-N-5] */
xorjoep 1:24714b45cd1b 171 x0 = _SIMD32_OFFSET(px1 + 2U);
xorjoep 1:24714b45cd1b 172
xorjoep 1:24714b45cd1b 173 /* Read state x[n-N-5], x[n-N-6] */
xorjoep 1:24714b45cd1b 174 x1 = _SIMD32_OFFSET(px1 + 3U);
xorjoep 1:24714b45cd1b 175
xorjoep 1:24714b45cd1b 176 /* acc2 += b[N-2] * x[n-N-4] + b[N-3] * x[n-N-5] */
xorjoep 1:24714b45cd1b 177 acc2 = __SMLALD(x0, c0, acc2);
xorjoep 1:24714b45cd1b 178
xorjoep 1:24714b45cd1b 179 /* acc3 += b[N-2] * x[n-N-5] + b[N-3] * x[n-N-6] */
xorjoep 1:24714b45cd1b 180 acc3 = __SMLALD(x1, c0, acc3);
xorjoep 1:24714b45cd1b 181
xorjoep 1:24714b45cd1b 182 px1 += 4U;
xorjoep 1:24714b45cd1b 183
xorjoep 1:24714b45cd1b 184 tapCnt--;
xorjoep 1:24714b45cd1b 185
xorjoep 1:24714b45cd1b 186 }
xorjoep 1:24714b45cd1b 187
xorjoep 1:24714b45cd1b 188
xorjoep 1:24714b45cd1b 189 /* If the filter length is not a multiple of 4, compute the remaining filter taps.
xorjoep 1:24714b45cd1b 190 ** This is always be 2 taps since the filter length is even. */
xorjoep 1:24714b45cd1b 191 if ((numTaps & 0x3U) != 0U)
xorjoep 1:24714b45cd1b 192 {
xorjoep 1:24714b45cd1b 193 /* Read 2 coefficients */
xorjoep 1:24714b45cd1b 194 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 195
xorjoep 1:24714b45cd1b 196 /* Fetch 4 state variables */
xorjoep 1:24714b45cd1b 197 x2 = _SIMD32_OFFSET(px1);
xorjoep 1:24714b45cd1b 198
xorjoep 1:24714b45cd1b 199 x3 = _SIMD32_OFFSET(px1 + 1U);
xorjoep 1:24714b45cd1b 200
xorjoep 1:24714b45cd1b 201 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 202 acc0 = __SMLALD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 203
xorjoep 1:24714b45cd1b 204 px1 += 2U;
xorjoep 1:24714b45cd1b 205
xorjoep 1:24714b45cd1b 206 acc1 = __SMLALD(x1, c0, acc1);
xorjoep 1:24714b45cd1b 207 acc2 = __SMLALD(x2, c0, acc2);
xorjoep 1:24714b45cd1b 208 acc3 = __SMLALD(x3, c0, acc3);
xorjoep 1:24714b45cd1b 209 }
xorjoep 1:24714b45cd1b 210
xorjoep 1:24714b45cd1b 211 /* The results in the 4 accumulators are in 2.30 format. Convert to 1.15 with saturation.
xorjoep 1:24714b45cd1b 212 ** Then store the 4 outputs in the destination buffer. */
xorjoep 1:24714b45cd1b 213
xorjoep 1:24714b45cd1b 214 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 215
xorjoep 1:24714b45cd1b 216 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 217 __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 218 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 219 __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 220
xorjoep 1:24714b45cd1b 221 #else
xorjoep 1:24714b45cd1b 222
xorjoep 1:24714b45cd1b 223 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 224 __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 225 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 226 __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 227
xorjoep 1:24714b45cd1b 228 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 229
xorjoep 1:24714b45cd1b 230
xorjoep 1:24714b45cd1b 231
xorjoep 1:24714b45cd1b 232 /* Advance the state pointer by 4 to process the next group of 4 samples */
xorjoep 1:24714b45cd1b 233 pState = pState + 4;
xorjoep 1:24714b45cd1b 234
xorjoep 1:24714b45cd1b 235 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 236 blkCnt--;
xorjoep 1:24714b45cd1b 237 }
xorjoep 1:24714b45cd1b 238
xorjoep 1:24714b45cd1b 239 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 240 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 241 blkCnt = blockSize % 0x4U;
xorjoep 1:24714b45cd1b 242 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 243 {
xorjoep 1:24714b45cd1b 244 /* Copy two samples into state buffer */
xorjoep 1:24714b45cd1b 245 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 246
xorjoep 1:24714b45cd1b 247 /* Set the accumulator to zero */
xorjoep 1:24714b45cd1b 248 acc0 = 0;
xorjoep 1:24714b45cd1b 249
xorjoep 1:24714b45cd1b 250 /* Initialize state pointer of type q15 */
xorjoep 1:24714b45cd1b 251 px1 = pState;
xorjoep 1:24714b45cd1b 252
xorjoep 1:24714b45cd1b 253 /* Initialize coeff pointer of type q31 */
xorjoep 1:24714b45cd1b 254 pb = pCoeffs;
xorjoep 1:24714b45cd1b 255
xorjoep 1:24714b45cd1b 256 tapCnt = numTaps >> 1;
xorjoep 1:24714b45cd1b 257
xorjoep 1:24714b45cd1b 258 do
xorjoep 1:24714b45cd1b 259 {
xorjoep 1:24714b45cd1b 260
xorjoep 1:24714b45cd1b 261 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 262 x0 = *__SIMD32(px1)++;
xorjoep 1:24714b45cd1b 263
xorjoep 1:24714b45cd1b 264 acc0 = __SMLALD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 265 tapCnt--;
xorjoep 1:24714b45cd1b 266 }
xorjoep 1:24714b45cd1b 267 while (tapCnt > 0U);
xorjoep 1:24714b45cd1b 268
xorjoep 1:24714b45cd1b 269 /* The result is in 2.30 format. Convert to 1.15 with saturation.
xorjoep 1:24714b45cd1b 270 ** Then store the output in the destination buffer. */
xorjoep 1:24714b45cd1b 271 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
xorjoep 1:24714b45cd1b 272
xorjoep 1:24714b45cd1b 273 /* Advance state pointer by 1 for the next sample */
xorjoep 1:24714b45cd1b 274 pState = pState + 1;
xorjoep 1:24714b45cd1b 275
xorjoep 1:24714b45cd1b 276 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 277 blkCnt--;
xorjoep 1:24714b45cd1b 278 }
xorjoep 1:24714b45cd1b 279
xorjoep 1:24714b45cd1b 280 /* Processing is complete.
xorjoep 1:24714b45cd1b 281 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 282 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 283
xorjoep 1:24714b45cd1b 284 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 285 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 286
xorjoep 1:24714b45cd1b 287 /* Calculation of count for copying integer writes */
xorjoep 1:24714b45cd1b 288 tapCnt = (numTaps - 1U) >> 2;
xorjoep 1:24714b45cd1b 289
xorjoep 1:24714b45cd1b 290 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 291 {
xorjoep 1:24714b45cd1b 292
xorjoep 1:24714b45cd1b 293 /* Copy state values to start of state buffer */
xorjoep 1:24714b45cd1b 294 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 295 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 296
xorjoep 1:24714b45cd1b 297 tapCnt--;
xorjoep 1:24714b45cd1b 298
xorjoep 1:24714b45cd1b 299 }
xorjoep 1:24714b45cd1b 300
xorjoep 1:24714b45cd1b 301 /* Calculation of count for remaining q15_t data */
xorjoep 1:24714b45cd1b 302 tapCnt = (numTaps - 1U) % 0x4U;
xorjoep 1:24714b45cd1b 303
xorjoep 1:24714b45cd1b 304 /* copy remaining data */
xorjoep 1:24714b45cd1b 305 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 306 {
xorjoep 1:24714b45cd1b 307 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 308
xorjoep 1:24714b45cd1b 309 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 310 tapCnt--;
xorjoep 1:24714b45cd1b 311 }
xorjoep 1:24714b45cd1b 312 }
xorjoep 1:24714b45cd1b 313
xorjoep 1:24714b45cd1b 314 #else /* UNALIGNED_SUPPORT_DISABLE */
xorjoep 1:24714b45cd1b 315
xorjoep 1:24714b45cd1b 316 void arm_fir_q15(
xorjoep 1:24714b45cd1b 317 const arm_fir_instance_q15 * S,
xorjoep 1:24714b45cd1b 318 q15_t * pSrc,
xorjoep 1:24714b45cd1b 319 q15_t * pDst,
xorjoep 1:24714b45cd1b 320 uint32_t blockSize)
xorjoep 1:24714b45cd1b 321 {
xorjoep 1:24714b45cd1b 322 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 323 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 324 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 325 q63_t acc0, acc1, acc2, acc3; /* Accumulators */
xorjoep 1:24714b45cd1b 326 q15_t *pb; /* Temporary pointer for coefficient buffer */
xorjoep 1:24714b45cd1b 327 q15_t *px; /* Temporary q31 pointer for SIMD state buffer accesses */
xorjoep 1:24714b45cd1b 328 q31_t x0, x1, x2, c0; /* Temporary variables to hold SIMD state and coefficient values */
xorjoep 1:24714b45cd1b 329 uint32_t numTaps = S->numTaps; /* Number of taps in the filter */
xorjoep 1:24714b45cd1b 330 uint32_t tapCnt, blkCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 331
xorjoep 1:24714b45cd1b 332
xorjoep 1:24714b45cd1b 333 /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 334 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 335 pStateCurnt = &(S->pState[(numTaps - 1U)]);
xorjoep 1:24714b45cd1b 336
xorjoep 1:24714b45cd1b 337 /* Apply loop unrolling and compute 4 output values simultaneously.
xorjoep 1:24714b45cd1b 338 * The variables acc0 ... acc3 hold output values that are being computed:
xorjoep 1:24714b45cd1b 339 *
xorjoep 1:24714b45cd1b 340 * acc0 = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
xorjoep 1:24714b45cd1b 341 * acc1 = b[numTaps-1] * x[n-numTaps] + b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
xorjoep 1:24714b45cd1b 342 * acc2 = b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] + b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
xorjoep 1:24714b45cd1b 343 * acc3 = b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps] +...+ b[0] * x[3]
xorjoep 1:24714b45cd1b 344 */
xorjoep 1:24714b45cd1b 345
xorjoep 1:24714b45cd1b 346 blkCnt = blockSize >> 2;
xorjoep 1:24714b45cd1b 347
xorjoep 1:24714b45cd1b 348 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
xorjoep 1:24714b45cd1b 349 ** a second loop below computes the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 350 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 351 {
xorjoep 1:24714b45cd1b 352 /* Copy four new input samples into the state buffer.
xorjoep 1:24714b45cd1b 353 ** Use 32-bit SIMD to move the 16-bit data. Only requires two copies. */
xorjoep 1:24714b45cd1b 354 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 355 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 356 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 357 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 358
xorjoep 1:24714b45cd1b 359
xorjoep 1:24714b45cd1b 360 /* Set all accumulators to zero */
xorjoep 1:24714b45cd1b 361 acc0 = 0;
xorjoep 1:24714b45cd1b 362 acc1 = 0;
xorjoep 1:24714b45cd1b 363 acc2 = 0;
xorjoep 1:24714b45cd1b 364 acc3 = 0;
xorjoep 1:24714b45cd1b 365
xorjoep 1:24714b45cd1b 366 /* Typecast q15_t pointer to q31_t pointer for state reading in q31_t */
xorjoep 1:24714b45cd1b 367 px = pState;
xorjoep 1:24714b45cd1b 368
xorjoep 1:24714b45cd1b 369 /* Typecast q15_t pointer to q31_t pointer for coefficient reading in q31_t */
xorjoep 1:24714b45cd1b 370 pb = pCoeffs;
xorjoep 1:24714b45cd1b 371
xorjoep 1:24714b45cd1b 372 /* Read the first two samples from the state buffer: x[n-N], x[n-N-1] */
xorjoep 1:24714b45cd1b 373 x0 = *__SIMD32(px)++;
xorjoep 1:24714b45cd1b 374
xorjoep 1:24714b45cd1b 375 /* Read the third and forth samples from the state buffer: x[n-N-2], x[n-N-3] */
xorjoep 1:24714b45cd1b 376 x2 = *__SIMD32(px)++;
xorjoep 1:24714b45cd1b 377
xorjoep 1:24714b45cd1b 378 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 379 ** Repeat until we've computed numTaps-(numTaps%4) coefficients. */
xorjoep 1:24714b45cd1b 380 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 381
xorjoep 1:24714b45cd1b 382 while (tapCnt > 0)
xorjoep 1:24714b45cd1b 383 {
xorjoep 1:24714b45cd1b 384 /* Read the first two coefficients using SIMD: b[N] and b[N-1] coefficients */
xorjoep 1:24714b45cd1b 385 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 386
xorjoep 1:24714b45cd1b 387 /* acc0 += b[N] * x[n-N] + b[N-1] * x[n-N-1] */
xorjoep 1:24714b45cd1b 388 acc0 = __SMLALD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 /* acc2 += b[N] * x[n-N-2] + b[N-1] * x[n-N-3] */
xorjoep 1:24714b45cd1b 391 acc2 = __SMLALD(x2, c0, acc2);
xorjoep 1:24714b45cd1b 392
xorjoep 1:24714b45cd1b 393 /* pack x[n-N-1] and x[n-N-2] */
xorjoep 1:24714b45cd1b 394 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 395 x1 = __PKHBT(x2, x0, 0);
xorjoep 1:24714b45cd1b 396 #else
xorjoep 1:24714b45cd1b 397 x1 = __PKHBT(x0, x2, 0);
xorjoep 1:24714b45cd1b 398 #endif
xorjoep 1:24714b45cd1b 399
xorjoep 1:24714b45cd1b 400 /* Read state x[n-N-4], x[n-N-5] */
xorjoep 1:24714b45cd1b 401 x0 = _SIMD32_OFFSET(px);
xorjoep 1:24714b45cd1b 402
xorjoep 1:24714b45cd1b 403 /* acc1 += b[N] * x[n-N-1] + b[N-1] * x[n-N-2] */
xorjoep 1:24714b45cd1b 404 acc1 = __SMLALDX(x1, c0, acc1);
xorjoep 1:24714b45cd1b 405
xorjoep 1:24714b45cd1b 406 /* pack x[n-N-3] and x[n-N-4] */
xorjoep 1:24714b45cd1b 407 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 408 x1 = __PKHBT(x0, x2, 0);
xorjoep 1:24714b45cd1b 409 #else
xorjoep 1:24714b45cd1b 410 x1 = __PKHBT(x2, x0, 0);
xorjoep 1:24714b45cd1b 411 #endif
xorjoep 1:24714b45cd1b 412
xorjoep 1:24714b45cd1b 413 /* acc3 += b[N] * x[n-N-3] + b[N-1] * x[n-N-4] */
xorjoep 1:24714b45cd1b 414 acc3 = __SMLALDX(x1, c0, acc3);
xorjoep 1:24714b45cd1b 415
xorjoep 1:24714b45cd1b 416 /* Read coefficients b[N-2], b[N-3] */
xorjoep 1:24714b45cd1b 417 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 418
xorjoep 1:24714b45cd1b 419 /* acc0 += b[N-2] * x[n-N-2] + b[N-3] * x[n-N-3] */
xorjoep 1:24714b45cd1b 420 acc0 = __SMLALD(x2, c0, acc0);
xorjoep 1:24714b45cd1b 421
xorjoep 1:24714b45cd1b 422 /* Read state x[n-N-6], x[n-N-7] with offset */
xorjoep 1:24714b45cd1b 423 x2 = _SIMD32_OFFSET(px + 2U);
xorjoep 1:24714b45cd1b 424
xorjoep 1:24714b45cd1b 425 /* acc2 += b[N-2] * x[n-N-4] + b[N-3] * x[n-N-5] */
xorjoep 1:24714b45cd1b 426 acc2 = __SMLALD(x0, c0, acc2);
xorjoep 1:24714b45cd1b 427
xorjoep 1:24714b45cd1b 428 /* acc1 += b[N-2] * x[n-N-3] + b[N-3] * x[n-N-4] */
xorjoep 1:24714b45cd1b 429 acc1 = __SMLALDX(x1, c0, acc1);
xorjoep 1:24714b45cd1b 430
xorjoep 1:24714b45cd1b 431 /* pack x[n-N-5] and x[n-N-6] */
xorjoep 1:24714b45cd1b 432 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 433 x1 = __PKHBT(x2, x0, 0);
xorjoep 1:24714b45cd1b 434 #else
xorjoep 1:24714b45cd1b 435 x1 = __PKHBT(x0, x2, 0);
xorjoep 1:24714b45cd1b 436 #endif
xorjoep 1:24714b45cd1b 437
xorjoep 1:24714b45cd1b 438 /* acc3 += b[N-2] * x[n-N-5] + b[N-3] * x[n-N-6] */
xorjoep 1:24714b45cd1b 439 acc3 = __SMLALDX(x1, c0, acc3);
xorjoep 1:24714b45cd1b 440
xorjoep 1:24714b45cd1b 441 /* Update state pointer for next state reading */
xorjoep 1:24714b45cd1b 442 px += 4U;
xorjoep 1:24714b45cd1b 443
xorjoep 1:24714b45cd1b 444 /* Decrement tap count */
xorjoep 1:24714b45cd1b 445 tapCnt--;
xorjoep 1:24714b45cd1b 446
xorjoep 1:24714b45cd1b 447 }
xorjoep 1:24714b45cd1b 448
xorjoep 1:24714b45cd1b 449 /* If the filter length is not a multiple of 4, compute the remaining filter taps.
xorjoep 1:24714b45cd1b 450 ** This is always be 2 taps since the filter length is even. */
xorjoep 1:24714b45cd1b 451 if ((numTaps & 0x3U) != 0U)
xorjoep 1:24714b45cd1b 452 {
xorjoep 1:24714b45cd1b 453
xorjoep 1:24714b45cd1b 454 /* Read last two coefficients */
xorjoep 1:24714b45cd1b 455 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 456
xorjoep 1:24714b45cd1b 457 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 458 acc0 = __SMLALD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 459 acc2 = __SMLALD(x2, c0, acc2);
xorjoep 1:24714b45cd1b 460
xorjoep 1:24714b45cd1b 461 /* pack state variables */
xorjoep 1:24714b45cd1b 462 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 463 x1 = __PKHBT(x2, x0, 0);
xorjoep 1:24714b45cd1b 464 #else
xorjoep 1:24714b45cd1b 465 x1 = __PKHBT(x0, x2, 0);
xorjoep 1:24714b45cd1b 466 #endif
xorjoep 1:24714b45cd1b 467
xorjoep 1:24714b45cd1b 468 /* Read last state variables */
xorjoep 1:24714b45cd1b 469 x0 = *__SIMD32(px);
xorjoep 1:24714b45cd1b 470
xorjoep 1:24714b45cd1b 471 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 472 acc1 = __SMLALDX(x1, c0, acc1);
xorjoep 1:24714b45cd1b 473
xorjoep 1:24714b45cd1b 474 /* pack state variables */
xorjoep 1:24714b45cd1b 475 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 476 x1 = __PKHBT(x0, x2, 0);
xorjoep 1:24714b45cd1b 477 #else
xorjoep 1:24714b45cd1b 478 x1 = __PKHBT(x2, x0, 0);
xorjoep 1:24714b45cd1b 479 #endif
xorjoep 1:24714b45cd1b 480
xorjoep 1:24714b45cd1b 481 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 482 acc3 = __SMLALDX(x1, c0, acc3);
xorjoep 1:24714b45cd1b 483 }
xorjoep 1:24714b45cd1b 484
xorjoep 1:24714b45cd1b 485 /* The results in the 4 accumulators are in 2.30 format. Convert to 1.15 with saturation.
xorjoep 1:24714b45cd1b 486 ** Then store the 4 outputs in the destination buffer. */
xorjoep 1:24714b45cd1b 487
xorjoep 1:24714b45cd1b 488 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 489
xorjoep 1:24714b45cd1b 490 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 491 __PKHBT(__SSAT((acc0 >> 15), 16), __SSAT((acc1 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 492
xorjoep 1:24714b45cd1b 493 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 494 __PKHBT(__SSAT((acc2 >> 15), 16), __SSAT((acc3 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 495
xorjoep 1:24714b45cd1b 496 #else
xorjoep 1:24714b45cd1b 497
xorjoep 1:24714b45cd1b 498 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 499 __PKHBT(__SSAT((acc1 >> 15), 16), __SSAT((acc0 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 500
xorjoep 1:24714b45cd1b 501 *__SIMD32(pDst)++ =
xorjoep 1:24714b45cd1b 502 __PKHBT(__SSAT((acc3 >> 15), 16), __SSAT((acc2 >> 15), 16), 16);
xorjoep 1:24714b45cd1b 503
xorjoep 1:24714b45cd1b 504 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 505
xorjoep 1:24714b45cd1b 506 /* Advance the state pointer by 4 to process the next group of 4 samples */
xorjoep 1:24714b45cd1b 507 pState = pState + 4;
xorjoep 1:24714b45cd1b 508
xorjoep 1:24714b45cd1b 509 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 510 blkCnt--;
xorjoep 1:24714b45cd1b 511 }
xorjoep 1:24714b45cd1b 512
xorjoep 1:24714b45cd1b 513 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 514 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 515 blkCnt = blockSize % 0x4U;
xorjoep 1:24714b45cd1b 516 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 517 {
xorjoep 1:24714b45cd1b 518 /* Copy two samples into state buffer */
xorjoep 1:24714b45cd1b 519 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 520
xorjoep 1:24714b45cd1b 521 /* Set the accumulator to zero */
xorjoep 1:24714b45cd1b 522 acc0 = 0;
xorjoep 1:24714b45cd1b 523
xorjoep 1:24714b45cd1b 524 /* Use SIMD to hold states and coefficients */
xorjoep 1:24714b45cd1b 525 px = pState;
xorjoep 1:24714b45cd1b 526 pb = pCoeffs;
xorjoep 1:24714b45cd1b 527
xorjoep 1:24714b45cd1b 528 tapCnt = numTaps >> 1U;
xorjoep 1:24714b45cd1b 529
xorjoep 1:24714b45cd1b 530 do
xorjoep 1:24714b45cd1b 531 {
xorjoep 1:24714b45cd1b 532 acc0 += (q31_t) * px++ * *pb++;
xorjoep 1:24714b45cd1b 533 acc0 += (q31_t) * px++ * *pb++;
xorjoep 1:24714b45cd1b 534 tapCnt--;
xorjoep 1:24714b45cd1b 535 }
xorjoep 1:24714b45cd1b 536 while (tapCnt > 0U);
xorjoep 1:24714b45cd1b 537
xorjoep 1:24714b45cd1b 538 /* The result is in 2.30 format. Convert to 1.15 with saturation.
xorjoep 1:24714b45cd1b 539 ** Then store the output in the destination buffer. */
xorjoep 1:24714b45cd1b 540 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
xorjoep 1:24714b45cd1b 541
xorjoep 1:24714b45cd1b 542 /* Advance state pointer by 1 for the next sample */
xorjoep 1:24714b45cd1b 543 pState = pState + 1U;
xorjoep 1:24714b45cd1b 544
xorjoep 1:24714b45cd1b 545 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 546 blkCnt--;
xorjoep 1:24714b45cd1b 547 }
xorjoep 1:24714b45cd1b 548
xorjoep 1:24714b45cd1b 549 /* Processing is complete.
xorjoep 1:24714b45cd1b 550 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 551 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 552
xorjoep 1:24714b45cd1b 553 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 554 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 555
xorjoep 1:24714b45cd1b 556 /* Calculation of count for copying integer writes */
xorjoep 1:24714b45cd1b 557 tapCnt = (numTaps - 1U) >> 2;
xorjoep 1:24714b45cd1b 558
xorjoep 1:24714b45cd1b 559 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 560 {
xorjoep 1:24714b45cd1b 561 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 562 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 563 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 564 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 565
xorjoep 1:24714b45cd1b 566 tapCnt--;
xorjoep 1:24714b45cd1b 567
xorjoep 1:24714b45cd1b 568 }
xorjoep 1:24714b45cd1b 569
xorjoep 1:24714b45cd1b 570 /* Calculation of count for remaining q15_t data */
xorjoep 1:24714b45cd1b 571 tapCnt = (numTaps - 1U) % 0x4U;
xorjoep 1:24714b45cd1b 572
xorjoep 1:24714b45cd1b 573 /* copy remaining data */
xorjoep 1:24714b45cd1b 574 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 575 {
xorjoep 1:24714b45cd1b 576 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 577
xorjoep 1:24714b45cd1b 578 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 579 tapCnt--;
xorjoep 1:24714b45cd1b 580 }
xorjoep 1:24714b45cd1b 581 }
xorjoep 1:24714b45cd1b 582
xorjoep 1:24714b45cd1b 583
xorjoep 1:24714b45cd1b 584 #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
xorjoep 1:24714b45cd1b 585
xorjoep 1:24714b45cd1b 586 #else /* ARM_MATH_CM0_FAMILY */
xorjoep 1:24714b45cd1b 587
xorjoep 1:24714b45cd1b 588
xorjoep 1:24714b45cd1b 589 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 590
xorjoep 1:24714b45cd1b 591 void arm_fir_q15(
xorjoep 1:24714b45cd1b 592 const arm_fir_instance_q15 * S,
xorjoep 1:24714b45cd1b 593 q15_t * pSrc,
xorjoep 1:24714b45cd1b 594 q15_t * pDst,
xorjoep 1:24714b45cd1b 595 uint32_t blockSize)
xorjoep 1:24714b45cd1b 596 {
xorjoep 1:24714b45cd1b 597 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 598 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 599 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 600
xorjoep 1:24714b45cd1b 601
xorjoep 1:24714b45cd1b 602
xorjoep 1:24714b45cd1b 603 q15_t *px; /* Temporary pointer for state buffer */
xorjoep 1:24714b45cd1b 604 q15_t *pb; /* Temporary pointer for coefficient buffer */
xorjoep 1:24714b45cd1b 605 q63_t acc; /* Accumulator */
xorjoep 1:24714b45cd1b 606 uint32_t numTaps = S->numTaps; /* Number of nTaps in the filter */
xorjoep 1:24714b45cd1b 607 uint32_t tapCnt, blkCnt; /* Loop counters */
xorjoep 1:24714b45cd1b 608
xorjoep 1:24714b45cd1b 609 /* S->pState buffer contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 610 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 611 pStateCurnt = &(S->pState[(numTaps - 1U)]);
xorjoep 1:24714b45cd1b 612
xorjoep 1:24714b45cd1b 613 /* Initialize blkCnt with blockSize */
xorjoep 1:24714b45cd1b 614 blkCnt = blockSize;
xorjoep 1:24714b45cd1b 615
xorjoep 1:24714b45cd1b 616 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 617 {
xorjoep 1:24714b45cd1b 618 /* Copy one sample at a time into state buffer */
xorjoep 1:24714b45cd1b 619 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 620
xorjoep 1:24714b45cd1b 621 /* Set the accumulator to zero */
xorjoep 1:24714b45cd1b 622 acc = 0;
xorjoep 1:24714b45cd1b 623
xorjoep 1:24714b45cd1b 624 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 625 px = pState;
xorjoep 1:24714b45cd1b 626
xorjoep 1:24714b45cd1b 627 /* Initialize Coefficient pointer */
xorjoep 1:24714b45cd1b 628 pb = pCoeffs;
xorjoep 1:24714b45cd1b 629
xorjoep 1:24714b45cd1b 630 tapCnt = numTaps;
xorjoep 1:24714b45cd1b 631
xorjoep 1:24714b45cd1b 632 /* Perform the multiply-accumulates */
xorjoep 1:24714b45cd1b 633 do
xorjoep 1:24714b45cd1b 634 {
xorjoep 1:24714b45cd1b 635 /* acc = b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
xorjoep 1:24714b45cd1b 636 acc += (q31_t) * px++ * *pb++;
xorjoep 1:24714b45cd1b 637 tapCnt--;
xorjoep 1:24714b45cd1b 638 } while (tapCnt > 0U);
xorjoep 1:24714b45cd1b 639
xorjoep 1:24714b45cd1b 640 /* The result is in 2.30 format. Convert to 1.15
xorjoep 1:24714b45cd1b 641 ** Then store the output in the destination buffer. */
xorjoep 1:24714b45cd1b 642 *pDst++ = (q15_t) __SSAT((acc >> 15U), 16);
xorjoep 1:24714b45cd1b 643
xorjoep 1:24714b45cd1b 644 /* Advance state pointer by 1 for the next sample */
xorjoep 1:24714b45cd1b 645 pState = pState + 1;
xorjoep 1:24714b45cd1b 646
xorjoep 1:24714b45cd1b 647 /* Decrement the samples loop counter */
xorjoep 1:24714b45cd1b 648 blkCnt--;
xorjoep 1:24714b45cd1b 649 }
xorjoep 1:24714b45cd1b 650
xorjoep 1:24714b45cd1b 651 /* Processing is complete.
xorjoep 1:24714b45cd1b 652 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 653 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 654
xorjoep 1:24714b45cd1b 655 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 656 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 657
xorjoep 1:24714b45cd1b 658 /* Copy numTaps number of values */
xorjoep 1:24714b45cd1b 659 tapCnt = (numTaps - 1U);
xorjoep 1:24714b45cd1b 660
xorjoep 1:24714b45cd1b 661 /* copy data */
xorjoep 1:24714b45cd1b 662 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 663 {
xorjoep 1:24714b45cd1b 664 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 665
xorjoep 1:24714b45cd1b 666 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 667 tapCnt--;
xorjoep 1:24714b45cd1b 668 }
xorjoep 1:24714b45cd1b 669
xorjoep 1:24714b45cd1b 670 }
xorjoep 1:24714b45cd1b 671
xorjoep 1:24714b45cd1b 672 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 673
xorjoep 1:24714b45cd1b 674
xorjoep 1:24714b45cd1b 675
xorjoep 1:24714b45cd1b 676
xorjoep 1:24714b45cd1b 677 /**
xorjoep 1:24714b45cd1b 678 * @} end of FIR group
xorjoep 1:24714b45cd1b 679 */