Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

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

functions/FilteringFunctions/arm_fir_decimate_fast_q15.c@1:24714b45cd1b, 2018-06-20 (annotated)

Committer:
xorjoep
Date:
Wed Jun 20 11:21:31 2018 +0000
Revision:
1:24714b45cd1b
The newest version of the CMSIS 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_decimate_fast_q15.c
xorjoep 1:24714b45cd1b 4 * Description: Fast Q15 FIR Decimator
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_decimate
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 decimator (fast variant) for Cortex-M3 and Cortex-M4.
xorjoep 1:24714b45cd1b 42 * @param[in] *S points to an instance of the Q15 FIR decimator 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 * \par Restrictions
xorjoep 1:24714b45cd1b 49 * If the silicon does not support unaligned memory access enable the macro UNALIGNED_SUPPORT_DISABLE
xorjoep 1:24714b45cd1b 50 * In this case input, output, state buffers should be aligned by 32-bit
xorjoep 1:24714b45cd1b 51 *
xorjoep 1:24714b45cd1b 52 * <b>Scaling and Overflow Behavior:</b>
xorjoep 1:24714b45cd1b 53 * \par
xorjoep 1:24714b45cd1b 54 * This fast version uses a 32-bit accumulator with 2.30 format.
xorjoep 1:24714b45cd1b 55 * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
xorjoep 1:24714b45cd1b 56 * Thus, if the accumulator result overflows it wraps around and distorts the result.
xorjoep 1:24714b45cd1b 57 * In order to avoid overflows completely the input signal must be scaled down by log2(numTaps) bits (log2 is read as log to the base 2).
xorjoep 1:24714b45cd1b 58 * The 2.30 accumulator is then truncated to 2.15 format and saturated to yield the 1.15 result.
xorjoep 1:24714b45cd1b 59 *
xorjoep 1:24714b45cd1b 60 * \par
xorjoep 1:24714b45cd1b 61 * Refer to the function <code>arm_fir_decimate_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.
xorjoep 1:24714b45cd1b 62 * Both the slow and the fast versions use the same instance structure.
xorjoep 1:24714b45cd1b 63 * Use the function <code>arm_fir_decimate_init_q15()</code> to initialize the filter structure.
xorjoep 1:24714b45cd1b 64 */
xorjoep 1:24714b45cd1b 65
xorjoep 1:24714b45cd1b 66 #ifndef UNALIGNED_SUPPORT_DISABLE
xorjoep 1:24714b45cd1b 67
xorjoep 1:24714b45cd1b 68 void arm_fir_decimate_fast_q15(
xorjoep 1:24714b45cd1b 69 const arm_fir_decimate_instance_q15 * S,
xorjoep 1:24714b45cd1b 70 q15_t * pSrc,
xorjoep 1:24714b45cd1b 71 q15_t * pDst,
xorjoep 1:24714b45cd1b 72 uint32_t blockSize)
xorjoep 1:24714b45cd1b 73 {
xorjoep 1:24714b45cd1b 74 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 75 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 76 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 77 q15_t *px; /* Temporary pointer for state buffer */
xorjoep 1:24714b45cd1b 78 q15_t *pb; /* Temporary pointer coefficient buffer */
xorjoep 1:24714b45cd1b 79 q31_t x0, x1, c0, c1; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 80 q31_t sum0; /* Accumulators */
xorjoep 1:24714b45cd1b 81 q31_t acc0, acc1;
xorjoep 1:24714b45cd1b 82 q15_t *px0, *px1;
xorjoep 1:24714b45cd1b 83 uint32_t blkCntN3;
xorjoep 1:24714b45cd1b 84 uint32_t numTaps = S->numTaps; /* Number of taps */
xorjoep 1:24714b45cd1b 85 uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
xorjoep 1:24714b45cd1b 86
xorjoep 1:24714b45cd1b 87
xorjoep 1:24714b45cd1b 88 /* S->pState buffer contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 89 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 90 pStateCurnt = S->pState + (numTaps - 1U);
xorjoep 1:24714b45cd1b 91
xorjoep 1:24714b45cd1b 92
xorjoep 1:24714b45cd1b 93 /* Total number of output samples to be computed */
xorjoep 1:24714b45cd1b 94 blkCnt = outBlockSize / 2;
xorjoep 1:24714b45cd1b 95 blkCntN3 = outBlockSize - (2 * blkCnt);
xorjoep 1:24714b45cd1b 96
xorjoep 1:24714b45cd1b 97
xorjoep 1:24714b45cd1b 98 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 99 {
xorjoep 1:24714b45cd1b 100 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 101 i = 2 * S->M;
xorjoep 1:24714b45cd1b 102
xorjoep 1:24714b45cd1b 103 do
xorjoep 1:24714b45cd1b 104 {
xorjoep 1:24714b45cd1b 105 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 106
xorjoep 1:24714b45cd1b 107 } while (--i);
xorjoep 1:24714b45cd1b 108
xorjoep 1:24714b45cd1b 109 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 110 acc0 = 0;
xorjoep 1:24714b45cd1b 111 acc1 = 0;
xorjoep 1:24714b45cd1b 112
xorjoep 1:24714b45cd1b 113 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 114 px0 = pState;
xorjoep 1:24714b45cd1b 115
xorjoep 1:24714b45cd1b 116 px1 = pState + S->M;
xorjoep 1:24714b45cd1b 117
xorjoep 1:24714b45cd1b 118
xorjoep 1:24714b45cd1b 119 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 120 pb = pCoeffs;
xorjoep 1:24714b45cd1b 121
xorjoep 1:24714b45cd1b 122 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 123 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 124
xorjoep 1:24714b45cd1b 125 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 126 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 127 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 128 {
xorjoep 1:24714b45cd1b 129 /* Read the Read b[numTaps-1] and b[numTaps-2] coefficients */
xorjoep 1:24714b45cd1b 130 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 131
xorjoep 1:24714b45cd1b 132 /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
xorjoep 1:24714b45cd1b 133 x0 = *__SIMD32(px0)++;
xorjoep 1:24714b45cd1b 134
xorjoep 1:24714b45cd1b 135 x1 = *__SIMD32(px1)++;
xorjoep 1:24714b45cd1b 136
xorjoep 1:24714b45cd1b 137 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 138 acc0 = __SMLAD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 139
xorjoep 1:24714b45cd1b 140 acc1 = __SMLAD(x1, c0, acc1);
xorjoep 1:24714b45cd1b 141
xorjoep 1:24714b45cd1b 142 /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
xorjoep 1:24714b45cd1b 143 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 144
xorjoep 1:24714b45cd1b 145 /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
xorjoep 1:24714b45cd1b 146 x0 = *__SIMD32(px0)++;
xorjoep 1:24714b45cd1b 147
xorjoep 1:24714b45cd1b 148 x1 = *__SIMD32(px1)++;
xorjoep 1:24714b45cd1b 149
xorjoep 1:24714b45cd1b 150 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 151 acc0 = __SMLAD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 152
xorjoep 1:24714b45cd1b 153 acc1 = __SMLAD(x1, c0, acc1);
xorjoep 1:24714b45cd1b 154
xorjoep 1:24714b45cd1b 155 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 156 tapCnt--;
xorjoep 1:24714b45cd1b 157 }
xorjoep 1:24714b45cd1b 158
xorjoep 1:24714b45cd1b 159 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 160 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 161
xorjoep 1:24714b45cd1b 162 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 163 {
xorjoep 1:24714b45cd1b 164 /* Read coefficients */
xorjoep 1:24714b45cd1b 165 c0 = *pb++;
xorjoep 1:24714b45cd1b 166
xorjoep 1:24714b45cd1b 167 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 168 x0 = *px0++;
xorjoep 1:24714b45cd1b 169
xorjoep 1:24714b45cd1b 170 x1 = *px1++;
xorjoep 1:24714b45cd1b 171
xorjoep 1:24714b45cd1b 172 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 173 acc0 = __SMLAD(x0, c0, acc0);
xorjoep 1:24714b45cd1b 174 acc1 = __SMLAD(x1, c0, acc1);
xorjoep 1:24714b45cd1b 175
xorjoep 1:24714b45cd1b 176 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 177 tapCnt--;
xorjoep 1:24714b45cd1b 178 }
xorjoep 1:24714b45cd1b 179
xorjoep 1:24714b45cd1b 180 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 181 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 182 pState = pState + S->M * 2;
xorjoep 1:24714b45cd1b 183
xorjoep 1:24714b45cd1b 184 /* Store filter output, smlad returns the values in 2.14 format */
xorjoep 1:24714b45cd1b 185 /* so downsacle by 15 to get output in 1.15 */
xorjoep 1:24714b45cd1b 186 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
xorjoep 1:24714b45cd1b 187 *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
xorjoep 1:24714b45cd1b 188
xorjoep 1:24714b45cd1b 189 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 190 blkCnt--;
xorjoep 1:24714b45cd1b 191 }
xorjoep 1:24714b45cd1b 192
xorjoep 1:24714b45cd1b 193
xorjoep 1:24714b45cd1b 194
xorjoep 1:24714b45cd1b 195 while (blkCntN3 > 0U)
xorjoep 1:24714b45cd1b 196 {
xorjoep 1:24714b45cd1b 197 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 198 i = S->M;
xorjoep 1:24714b45cd1b 199
xorjoep 1:24714b45cd1b 200 do
xorjoep 1:24714b45cd1b 201 {
xorjoep 1:24714b45cd1b 202 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 203
xorjoep 1:24714b45cd1b 204 } while (--i);
xorjoep 1:24714b45cd1b 205
xorjoep 1:24714b45cd1b 206 /*Set sum to zero */
xorjoep 1:24714b45cd1b 207 sum0 = 0;
xorjoep 1:24714b45cd1b 208
xorjoep 1:24714b45cd1b 209 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 210 px = pState;
xorjoep 1:24714b45cd1b 211
xorjoep 1:24714b45cd1b 212 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 213 pb = pCoeffs;
xorjoep 1:24714b45cd1b 214
xorjoep 1:24714b45cd1b 215 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 216 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 217
xorjoep 1:24714b45cd1b 218 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 219 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 220 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 221 {
xorjoep 1:24714b45cd1b 222 /* Read the Read b[numTaps-1] and b[numTaps-2] coefficients */
xorjoep 1:24714b45cd1b 223 c0 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 224
xorjoep 1:24714b45cd1b 225 /* Read x[n-numTaps-1] and x[n-numTaps-2]sample */
xorjoep 1:24714b45cd1b 226 x0 = *__SIMD32(px)++;
xorjoep 1:24714b45cd1b 227
xorjoep 1:24714b45cd1b 228 /* Read the b[numTaps-3] and b[numTaps-4] coefficient */
xorjoep 1:24714b45cd1b 229 c1 = *__SIMD32(pb)++;
xorjoep 1:24714b45cd1b 230
xorjoep 1:24714b45cd1b 231 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 232 sum0 = __SMLAD(x0, c0, sum0);
xorjoep 1:24714b45cd1b 233
xorjoep 1:24714b45cd1b 234 /* Read x[n-numTaps-2] and x[n-numTaps-3] sample */
xorjoep 1:24714b45cd1b 235 x0 = *__SIMD32(px)++;
xorjoep 1:24714b45cd1b 236
xorjoep 1:24714b45cd1b 237 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 238 sum0 = __SMLAD(x0, c1, sum0);
xorjoep 1:24714b45cd1b 239
xorjoep 1:24714b45cd1b 240 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 241 tapCnt--;
xorjoep 1:24714b45cd1b 242 }
xorjoep 1:24714b45cd1b 243
xorjoep 1:24714b45cd1b 244 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 245 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 246
xorjoep 1:24714b45cd1b 247 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 248 {
xorjoep 1:24714b45cd1b 249 /* Read coefficients */
xorjoep 1:24714b45cd1b 250 c0 = *pb++;
xorjoep 1:24714b45cd1b 251
xorjoep 1:24714b45cd1b 252 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 253 x0 = *px++;
xorjoep 1:24714b45cd1b 254
xorjoep 1:24714b45cd1b 255 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 256 sum0 = __SMLAD(x0, c0, sum0);
xorjoep 1:24714b45cd1b 257
xorjoep 1:24714b45cd1b 258 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 259 tapCnt--;
xorjoep 1:24714b45cd1b 260 }
xorjoep 1:24714b45cd1b 261
xorjoep 1:24714b45cd1b 262 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 263 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 264 pState = pState + S->M;
xorjoep 1:24714b45cd1b 265
xorjoep 1:24714b45cd1b 266 /* Store filter output, smlad returns the values in 2.14 format */
xorjoep 1:24714b45cd1b 267 /* so downsacle by 15 to get output in 1.15 */
xorjoep 1:24714b45cd1b 268 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
xorjoep 1:24714b45cd1b 269
xorjoep 1:24714b45cd1b 270 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 271 blkCntN3--;
xorjoep 1:24714b45cd1b 272 }
xorjoep 1:24714b45cd1b 273
xorjoep 1:24714b45cd1b 274 /* Processing is complete.
xorjoep 1:24714b45cd1b 275 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 276 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 277
xorjoep 1:24714b45cd1b 278 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 279 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 280
xorjoep 1:24714b45cd1b 281 i = (numTaps - 1U) >> 2U;
xorjoep 1:24714b45cd1b 282
xorjoep 1:24714b45cd1b 283 /* copy data */
xorjoep 1:24714b45cd1b 284 while (i > 0U)
xorjoep 1:24714b45cd1b 285 {
xorjoep 1:24714b45cd1b 286 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 287 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
xorjoep 1:24714b45cd1b 288
xorjoep 1:24714b45cd1b 289 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 290 i--;
xorjoep 1:24714b45cd1b 291 }
xorjoep 1:24714b45cd1b 292
xorjoep 1:24714b45cd1b 293 i = (numTaps - 1U) % 0x04U;
xorjoep 1:24714b45cd1b 294
xorjoep 1:24714b45cd1b 295 /* copy data */
xorjoep 1:24714b45cd1b 296 while (i > 0U)
xorjoep 1:24714b45cd1b 297 {
xorjoep 1:24714b45cd1b 298 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 299
xorjoep 1:24714b45cd1b 300 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 301 i--;
xorjoep 1:24714b45cd1b 302 }
xorjoep 1:24714b45cd1b 303 }
xorjoep 1:24714b45cd1b 304
xorjoep 1:24714b45cd1b 305 #else
xorjoep 1:24714b45cd1b 306
xorjoep 1:24714b45cd1b 307
xorjoep 1:24714b45cd1b 308 void arm_fir_decimate_fast_q15(
xorjoep 1:24714b45cd1b 309 const arm_fir_decimate_instance_q15 * S,
xorjoep 1:24714b45cd1b 310 q15_t * pSrc,
xorjoep 1:24714b45cd1b 311 q15_t * pDst,
xorjoep 1:24714b45cd1b 312 uint32_t blockSize)
xorjoep 1:24714b45cd1b 313 {
xorjoep 1:24714b45cd1b 314 q15_t *pState = S->pState; /* State pointer */
xorjoep 1:24714b45cd1b 315 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 316 q15_t *pStateCurnt; /* Points to the current sample of the state */
xorjoep 1:24714b45cd1b 317 q15_t *px; /* Temporary pointer for state buffer */
xorjoep 1:24714b45cd1b 318 q15_t *pb; /* Temporary pointer coefficient buffer */
xorjoep 1:24714b45cd1b 319 q15_t x0, x1, c0; /* Temporary variables to hold state and coefficient values */
xorjoep 1:24714b45cd1b 320 q31_t sum0; /* Accumulators */
xorjoep 1:24714b45cd1b 321 q31_t acc0, acc1;
xorjoep 1:24714b45cd1b 322 q15_t *px0, *px1;
xorjoep 1:24714b45cd1b 323 uint32_t blkCntN3;
xorjoep 1:24714b45cd1b 324 uint32_t numTaps = S->numTaps; /* Number of taps */
xorjoep 1:24714b45cd1b 325 uint32_t i, blkCnt, tapCnt, outBlockSize = blockSize / S->M; /* Loop counters */
xorjoep 1:24714b45cd1b 326
xorjoep 1:24714b45cd1b 327
xorjoep 1:24714b45cd1b 328 /* S->pState buffer contains previous frame (numTaps - 1) samples */
xorjoep 1:24714b45cd1b 329 /* pStateCurnt points to the location where the new input data should be written */
xorjoep 1:24714b45cd1b 330 pStateCurnt = S->pState + (numTaps - 1U);
xorjoep 1:24714b45cd1b 331
xorjoep 1:24714b45cd1b 332
xorjoep 1:24714b45cd1b 333 /* Total number of output samples to be computed */
xorjoep 1:24714b45cd1b 334 blkCnt = outBlockSize / 2;
xorjoep 1:24714b45cd1b 335 blkCntN3 = outBlockSize - (2 * blkCnt);
xorjoep 1:24714b45cd1b 336
xorjoep 1:24714b45cd1b 337 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 338 {
xorjoep 1:24714b45cd1b 339 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 340 i = 2 * S->M;
xorjoep 1:24714b45cd1b 341
xorjoep 1:24714b45cd1b 342 do
xorjoep 1:24714b45cd1b 343 {
xorjoep 1:24714b45cd1b 344 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 345
xorjoep 1:24714b45cd1b 346 } while (--i);
xorjoep 1:24714b45cd1b 347
xorjoep 1:24714b45cd1b 348 /* Set accumulator to zero */
xorjoep 1:24714b45cd1b 349 acc0 = 0;
xorjoep 1:24714b45cd1b 350 acc1 = 0;
xorjoep 1:24714b45cd1b 351
xorjoep 1:24714b45cd1b 352 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 353 px0 = pState;
xorjoep 1:24714b45cd1b 354
xorjoep 1:24714b45cd1b 355 px1 = pState + S->M;
xorjoep 1:24714b45cd1b 356
xorjoep 1:24714b45cd1b 357
xorjoep 1:24714b45cd1b 358 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 359 pb = pCoeffs;
xorjoep 1:24714b45cd1b 360
xorjoep 1:24714b45cd1b 361 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 362 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 363
xorjoep 1:24714b45cd1b 364 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 365 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 366 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 367 {
xorjoep 1:24714b45cd1b 368 /* Read the Read b[numTaps-1] coefficients */
xorjoep 1:24714b45cd1b 369 c0 = *pb++;
xorjoep 1:24714b45cd1b 370
xorjoep 1:24714b45cd1b 371 /* Read x[n-numTaps-1] for sample 0 and for sample 1 */
xorjoep 1:24714b45cd1b 372 x0 = *px0++;
xorjoep 1:24714b45cd1b 373 x1 = *px1++;
xorjoep 1:24714b45cd1b 374
xorjoep 1:24714b45cd1b 375 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 376 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 377 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 378
xorjoep 1:24714b45cd1b 379 /* Read the b[numTaps-2] coefficient */
xorjoep 1:24714b45cd1b 380 c0 = *pb++;
xorjoep 1:24714b45cd1b 381
xorjoep 1:24714b45cd1b 382 /* Read x[n-numTaps-2] for sample 0 and sample 1 */
xorjoep 1:24714b45cd1b 383 x0 = *px0++;
xorjoep 1:24714b45cd1b 384 x1 = *px1++;
xorjoep 1:24714b45cd1b 385
xorjoep 1:24714b45cd1b 386 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 387 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 388 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 /* Read the b[numTaps-3] coefficients */
xorjoep 1:24714b45cd1b 391 c0 = *pb++;
xorjoep 1:24714b45cd1b 392
xorjoep 1:24714b45cd1b 393 /* Read x[n-numTaps-3] for sample 0 and sample 1 */
xorjoep 1:24714b45cd1b 394 x0 = *px0++;
xorjoep 1:24714b45cd1b 395 x1 = *px1++;
xorjoep 1:24714b45cd1b 396
xorjoep 1:24714b45cd1b 397 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 398 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 399 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 400
xorjoep 1:24714b45cd1b 401 /* Read the b[numTaps-4] coefficient */
xorjoep 1:24714b45cd1b 402 c0 = *pb++;
xorjoep 1:24714b45cd1b 403
xorjoep 1:24714b45cd1b 404 /* Read x[n-numTaps-4] for sample 0 and sample 1 */
xorjoep 1:24714b45cd1b 405 x0 = *px0++;
xorjoep 1:24714b45cd1b 406 x1 = *px1++;
xorjoep 1:24714b45cd1b 407
xorjoep 1:24714b45cd1b 408 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 409 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 410 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 411
xorjoep 1:24714b45cd1b 412 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 413 tapCnt--;
xorjoep 1:24714b45cd1b 414 }
xorjoep 1:24714b45cd1b 415
xorjoep 1:24714b45cd1b 416 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 417 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 418
xorjoep 1:24714b45cd1b 419 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 420 {
xorjoep 1:24714b45cd1b 421 /* Read coefficients */
xorjoep 1:24714b45cd1b 422 c0 = *pb++;
xorjoep 1:24714b45cd1b 423
xorjoep 1:24714b45cd1b 424 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 425 x0 = *px0++;
xorjoep 1:24714b45cd1b 426 x1 = *px1++;
xorjoep 1:24714b45cd1b 427
xorjoep 1:24714b45cd1b 428 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 429 acc0 += x0 * c0;
xorjoep 1:24714b45cd1b 430 acc1 += x1 * c0;
xorjoep 1:24714b45cd1b 431
xorjoep 1:24714b45cd1b 432 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 433 tapCnt--;
xorjoep 1:24714b45cd1b 434 }
xorjoep 1:24714b45cd1b 435
xorjoep 1:24714b45cd1b 436 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 437 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 438 pState = pState + S->M * 2;
xorjoep 1:24714b45cd1b 439
xorjoep 1:24714b45cd1b 440 /* Store filter output, smlad returns the values in 2.14 format */
xorjoep 1:24714b45cd1b 441 /* so downsacle by 15 to get output in 1.15 */
xorjoep 1:24714b45cd1b 442
xorjoep 1:24714b45cd1b 443 *pDst++ = (q15_t) (__SSAT((acc0 >> 15), 16));
xorjoep 1:24714b45cd1b 444 *pDst++ = (q15_t) (__SSAT((acc1 >> 15), 16));
xorjoep 1:24714b45cd1b 445
xorjoep 1:24714b45cd1b 446
xorjoep 1:24714b45cd1b 447 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 448 blkCnt--;
xorjoep 1:24714b45cd1b 449 }
xorjoep 1:24714b45cd1b 450
xorjoep 1:24714b45cd1b 451 while (blkCntN3 > 0U)
xorjoep 1:24714b45cd1b 452 {
xorjoep 1:24714b45cd1b 453 /* Copy decimation factor number of new input samples into the state buffer */
xorjoep 1:24714b45cd1b 454 i = S->M;
xorjoep 1:24714b45cd1b 455
xorjoep 1:24714b45cd1b 456 do
xorjoep 1:24714b45cd1b 457 {
xorjoep 1:24714b45cd1b 458 *pStateCurnt++ = *pSrc++;
xorjoep 1:24714b45cd1b 459
xorjoep 1:24714b45cd1b 460 } while (--i);
xorjoep 1:24714b45cd1b 461
xorjoep 1:24714b45cd1b 462 /*Set sum to zero */
xorjoep 1:24714b45cd1b 463 sum0 = 0;
xorjoep 1:24714b45cd1b 464
xorjoep 1:24714b45cd1b 465 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 466 px = pState;
xorjoep 1:24714b45cd1b 467
xorjoep 1:24714b45cd1b 468 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 469 pb = pCoeffs;
xorjoep 1:24714b45cd1b 470
xorjoep 1:24714b45cd1b 471 /* Loop unrolling. Process 4 taps at a time. */
xorjoep 1:24714b45cd1b 472 tapCnt = numTaps >> 2;
xorjoep 1:24714b45cd1b 473
xorjoep 1:24714b45cd1b 474 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 475 ** Repeat until we've computed numTaps-4 coefficients. */
xorjoep 1:24714b45cd1b 476 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 477 {
xorjoep 1:24714b45cd1b 478 /* Read the Read b[numTaps-1] coefficients */
xorjoep 1:24714b45cd1b 479 c0 = *pb++;
xorjoep 1:24714b45cd1b 480
xorjoep 1:24714b45cd1b 481 /* Read x[n-numTaps-1] and sample */
xorjoep 1:24714b45cd1b 482 x0 = *px++;
xorjoep 1:24714b45cd1b 483
xorjoep 1:24714b45cd1b 484 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 485 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 486
xorjoep 1:24714b45cd1b 487 /* Read the b[numTaps-2] coefficient */
xorjoep 1:24714b45cd1b 488 c0 = *pb++;
xorjoep 1:24714b45cd1b 489
xorjoep 1:24714b45cd1b 490 /* Read x[n-numTaps-2] and sample */
xorjoep 1:24714b45cd1b 491 x0 = *px++;
xorjoep 1:24714b45cd1b 492
xorjoep 1:24714b45cd1b 493 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 494 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 495
xorjoep 1:24714b45cd1b 496 /* Read the b[numTaps-3] coefficients */
xorjoep 1:24714b45cd1b 497 c0 = *pb++;
xorjoep 1:24714b45cd1b 498
xorjoep 1:24714b45cd1b 499 /* Read x[n-numTaps-3] sample */
xorjoep 1:24714b45cd1b 500 x0 = *px++;
xorjoep 1:24714b45cd1b 501
xorjoep 1:24714b45cd1b 502 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 503 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 504
xorjoep 1:24714b45cd1b 505 /* Read the b[numTaps-4] coefficient */
xorjoep 1:24714b45cd1b 506 c0 = *pb++;
xorjoep 1:24714b45cd1b 507
xorjoep 1:24714b45cd1b 508 /* Read x[n-numTaps-4] sample */
xorjoep 1:24714b45cd1b 509 x0 = *px++;
xorjoep 1:24714b45cd1b 510
xorjoep 1:24714b45cd1b 511 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 512 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 513
xorjoep 1:24714b45cd1b 514 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 515 tapCnt--;
xorjoep 1:24714b45cd1b 516 }
xorjoep 1:24714b45cd1b 517
xorjoep 1:24714b45cd1b 518 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 519 tapCnt = numTaps % 0x4U;
xorjoep 1:24714b45cd1b 520
xorjoep 1:24714b45cd1b 521 while (tapCnt > 0U)
xorjoep 1:24714b45cd1b 522 {
xorjoep 1:24714b45cd1b 523 /* Read coefficients */
xorjoep 1:24714b45cd1b 524 c0 = *pb++;
xorjoep 1:24714b45cd1b 525
xorjoep 1:24714b45cd1b 526 /* Fetch 1 state variable */
xorjoep 1:24714b45cd1b 527 x0 = *px++;
xorjoep 1:24714b45cd1b 528
xorjoep 1:24714b45cd1b 529 /* Perform the multiply-accumulate */
xorjoep 1:24714b45cd1b 530 sum0 += x0 * c0;
xorjoep 1:24714b45cd1b 531
xorjoep 1:24714b45cd1b 532 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 533 tapCnt--;
xorjoep 1:24714b45cd1b 534 }
xorjoep 1:24714b45cd1b 535
xorjoep 1:24714b45cd1b 536 /* Advance the state pointer by the decimation factor
xorjoep 1:24714b45cd1b 537 * to process the next group of decimation factor number samples */
xorjoep 1:24714b45cd1b 538 pState = pState + S->M;
xorjoep 1:24714b45cd1b 539
xorjoep 1:24714b45cd1b 540 /* Store filter output, smlad returns the values in 2.14 format */
xorjoep 1:24714b45cd1b 541 /* so downsacle by 15 to get output in 1.15 */
xorjoep 1:24714b45cd1b 542 *pDst++ = (q15_t) (__SSAT((sum0 >> 15), 16));
xorjoep 1:24714b45cd1b 543
xorjoep 1:24714b45cd1b 544 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 545 blkCntN3--;
xorjoep 1:24714b45cd1b 546 }
xorjoep 1:24714b45cd1b 547
xorjoep 1:24714b45cd1b 548 /* Processing is complete.
xorjoep 1:24714b45cd1b 549 ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
xorjoep 1:24714b45cd1b 550 ** This prepares the state buffer for the next function call. */
xorjoep 1:24714b45cd1b 551
xorjoep 1:24714b45cd1b 552 /* Points to the start of the state buffer */
xorjoep 1:24714b45cd1b 553 pStateCurnt = S->pState;
xorjoep 1:24714b45cd1b 554
xorjoep 1:24714b45cd1b 555 i = (numTaps - 1U) >> 2U;
xorjoep 1:24714b45cd1b 556
xorjoep 1:24714b45cd1b 557 /* copy data */
xorjoep 1:24714b45cd1b 558 while (i > 0U)
xorjoep 1:24714b45cd1b 559 {
xorjoep 1:24714b45cd1b 560 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 561 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 562 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 563 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 564
xorjoep 1:24714b45cd1b 565 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 566 i--;
xorjoep 1:24714b45cd1b 567 }
xorjoep 1:24714b45cd1b 568
xorjoep 1:24714b45cd1b 569 i = (numTaps - 1U) % 0x04U;
xorjoep 1:24714b45cd1b 570
xorjoep 1:24714b45cd1b 571 /* copy data */
xorjoep 1:24714b45cd1b 572 while (i > 0U)
xorjoep 1:24714b45cd1b 573 {
xorjoep 1:24714b45cd1b 574 *pStateCurnt++ = *pState++;
xorjoep 1:24714b45cd1b 575
xorjoep 1:24714b45cd1b 576 /* Decrement the loop counter */
xorjoep 1:24714b45cd1b 577 i--;
xorjoep 1:24714b45cd1b 578 }
xorjoep 1:24714b45cd1b 579 }
xorjoep 1:24714b45cd1b 580
xorjoep 1:24714b45cd1b 581
xorjoep 1:24714b45cd1b 582 #endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
xorjoep 1:24714b45cd1b 583
xorjoep 1:24714b45cd1b 584 /**
xorjoep 1:24714b45cd1b 585 * @} end of FIR_decimate group
xorjoep 1:24714b45cd1b 586 */