Joep D / CMSIS_DSP_5

The CMSIS DSP 5 library

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

functions/FilteringFunctions/arm_fir_lattice_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_lattice_q15.c
xorjoep 1:24714b45cd1b 4 * Description: Q15 FIR lattice 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_Lattice
xorjoep 1:24714b45cd1b 37 * @{
xorjoep 1:24714b45cd1b 38 */
xorjoep 1:24714b45cd1b 39
xorjoep 1:24714b45cd1b 40
xorjoep 1:24714b45cd1b 41 /**
xorjoep 1:24714b45cd1b 42 * @brief Processing function for the Q15 FIR lattice filter.
xorjoep 1:24714b45cd1b 43 * @param[in] *S points to an instance of the Q15 FIR lattice structure.
xorjoep 1:24714b45cd1b 44 * @param[in] *pSrc points to the block of input data.
xorjoep 1:24714b45cd1b 45 * @param[out] *pDst points to the block of output data
xorjoep 1:24714b45cd1b 46 * @param[in] blockSize number of samples to process.
xorjoep 1:24714b45cd1b 47 * @return none.
xorjoep 1:24714b45cd1b 48 */
xorjoep 1:24714b45cd1b 49
xorjoep 1:24714b45cd1b 50 void arm_fir_lattice_q15(
xorjoep 1:24714b45cd1b 51 const arm_fir_lattice_instance_q15 * S,
xorjoep 1:24714b45cd1b 52 q15_t * pSrc,
xorjoep 1:24714b45cd1b 53 q15_t * pDst,
xorjoep 1:24714b45cd1b 54 uint32_t blockSize)
xorjoep 1:24714b45cd1b 55 {
xorjoep 1:24714b45cd1b 56 q15_t *pState; /* State pointer */
xorjoep 1:24714b45cd1b 57 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
xorjoep 1:24714b45cd1b 58 q15_t *px; /* temporary state pointer */
xorjoep 1:24714b45cd1b 59 q15_t *pk; /* temporary coefficient pointer */
xorjoep 1:24714b45cd1b 60
xorjoep 1:24714b45cd1b 61
xorjoep 1:24714b45cd1b 62 #if defined (ARM_MATH_DSP)
xorjoep 1:24714b45cd1b 63
xorjoep 1:24714b45cd1b 64 /* Run the below code for Cortex-M4 and Cortex-M3 */
xorjoep 1:24714b45cd1b 65
xorjoep 1:24714b45cd1b 66 q31_t fcurnt1, fnext1, gcurnt1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */
xorjoep 1:24714b45cd1b 67 q31_t fcurnt2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */
xorjoep 1:24714b45cd1b 68 q31_t fcurnt3, fnext3, gnext3; /* temporary variables for third sample in loop unrolling */
xorjoep 1:24714b45cd1b 69 q31_t fcurnt4, fnext4, gnext4; /* temporary variables for fourth sample in loop unrolling */
xorjoep 1:24714b45cd1b 70 uint32_t numStages = S->numStages; /* Number of stages in the filter */
xorjoep 1:24714b45cd1b 71 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
xorjoep 1:24714b45cd1b 72
xorjoep 1:24714b45cd1b 73 pState = &S->pState[0];
xorjoep 1:24714b45cd1b 74
xorjoep 1:24714b45cd1b 75 blkCnt = blockSize >> 2U;
xorjoep 1:24714b45cd1b 76
xorjoep 1:24714b45cd1b 77 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
xorjoep 1:24714b45cd1b 78 ** a second loop below computes the remaining 1 to 3 samples. */
xorjoep 1:24714b45cd1b 79 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 80 {
xorjoep 1:24714b45cd1b 81
xorjoep 1:24714b45cd1b 82 /* Read two samples from input buffer */
xorjoep 1:24714b45cd1b 83 /* f0(n) = x(n) */
xorjoep 1:24714b45cd1b 84 fcurnt1 = *pSrc++;
xorjoep 1:24714b45cd1b 85 fcurnt2 = *pSrc++;
xorjoep 1:24714b45cd1b 86
xorjoep 1:24714b45cd1b 87 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 88 pk = (pCoeffs);
xorjoep 1:24714b45cd1b 89
xorjoep 1:24714b45cd1b 90 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 91 px = pState;
xorjoep 1:24714b45cd1b 92
xorjoep 1:24714b45cd1b 93 /* Read g0(n-1) from state */
xorjoep 1:24714b45cd1b 94 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 95
xorjoep 1:24714b45cd1b 96 /* Process first sample for first tap */
xorjoep 1:24714b45cd1b 97 /* f1(n) = f0(n) + K1 * g0(n-1) */
xorjoep 1:24714b45cd1b 98 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 99 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 100
xorjoep 1:24714b45cd1b 101 /* g1(n) = f0(n) * K1 + g0(n-1) */
xorjoep 1:24714b45cd1b 102 gnext1 = (q31_t) ((fcurnt1 * (*pk)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 103 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 104
xorjoep 1:24714b45cd1b 105 /* Process second sample for first tap */
xorjoep 1:24714b45cd1b 106 /* for sample 2 processing */
xorjoep 1:24714b45cd1b 107 fnext2 = (q31_t) ((fcurnt1 * (*pk)) >> 15U) + fcurnt2;
xorjoep 1:24714b45cd1b 108 fnext2 = __SSAT(fnext2, 16);
xorjoep 1:24714b45cd1b 109
xorjoep 1:24714b45cd1b 110 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 111 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 112
xorjoep 1:24714b45cd1b 113
xorjoep 1:24714b45cd1b 114 /* Read next two samples from input buffer */
xorjoep 1:24714b45cd1b 115 /* f0(n+2) = x(n+2) */
xorjoep 1:24714b45cd1b 116 fcurnt3 = *pSrc++;
xorjoep 1:24714b45cd1b 117 fcurnt4 = *pSrc++;
xorjoep 1:24714b45cd1b 118
xorjoep 1:24714b45cd1b 119 /* Copy only last input samples into the state buffer
xorjoep 1:24714b45cd1b 120 which is used for next four samples processing */
xorjoep 1:24714b45cd1b 121 *px++ = (q15_t) fcurnt4;
xorjoep 1:24714b45cd1b 122
xorjoep 1:24714b45cd1b 123 /* Process third sample for first tap */
xorjoep 1:24714b45cd1b 124 fnext3 = (q31_t) ((fcurnt2 * (*pk)) >> 15U) + fcurnt3;
xorjoep 1:24714b45cd1b 125 fnext3 = __SSAT(fnext3, 16);
xorjoep 1:24714b45cd1b 126 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15U) + fcurnt2;
xorjoep 1:24714b45cd1b 127 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 128
xorjoep 1:24714b45cd1b 129 /* Process fourth sample for first tap */
xorjoep 1:24714b45cd1b 130 fnext4 = (q31_t) ((fcurnt3 * (*pk)) >> 15U) + fcurnt4;
xorjoep 1:24714b45cd1b 131 fnext4 = __SSAT(fnext4, 16);
xorjoep 1:24714b45cd1b 132 gnext4 = (q31_t) ((fcurnt4 * (*pk++)) >> 15U) + fcurnt3;
xorjoep 1:24714b45cd1b 133 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 134
xorjoep 1:24714b45cd1b 135 /* Update of f values for next coefficient set processing */
xorjoep 1:24714b45cd1b 136 fcurnt1 = fnext1;
xorjoep 1:24714b45cd1b 137 fcurnt2 = fnext2;
xorjoep 1:24714b45cd1b 138 fcurnt3 = fnext3;
xorjoep 1:24714b45cd1b 139 fcurnt4 = fnext4;
xorjoep 1:24714b45cd1b 140
xorjoep 1:24714b45cd1b 141
xorjoep 1:24714b45cd1b 142 /* Loop unrolling. Process 4 taps at a time . */
xorjoep 1:24714b45cd1b 143 stageCnt = (numStages - 1U) >> 2;
xorjoep 1:24714b45cd1b 144
xorjoep 1:24714b45cd1b 145
xorjoep 1:24714b45cd1b 146 /* Loop over the number of taps. Unroll by a factor of 4.
xorjoep 1:24714b45cd1b 147 ** Repeat until we've computed numStages-3 coefficients. */
xorjoep 1:24714b45cd1b 148
xorjoep 1:24714b45cd1b 149 /* Process 2nd, 3rd, 4th and 5th taps ... here */
xorjoep 1:24714b45cd1b 150 while (stageCnt > 0U)
xorjoep 1:24714b45cd1b 151 {
xorjoep 1:24714b45cd1b 152 /* Read g1(n-1), g3(n-1) .... from state */
xorjoep 1:24714b45cd1b 153 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 154
xorjoep 1:24714b45cd1b 155 /* save g1(n) in state buffer */
xorjoep 1:24714b45cd1b 156 *px++ = (q15_t) gnext4;
xorjoep 1:24714b45cd1b 157
xorjoep 1:24714b45cd1b 158 /* Process first sample for 2nd, 6th .. tap */
xorjoep 1:24714b45cd1b 159 /* Sample processing for K2, K6.... */
xorjoep 1:24714b45cd1b 160 /* f1(n) = f0(n) + K1 * g0(n-1) */
xorjoep 1:24714b45cd1b 161 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 162 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 163
xorjoep 1:24714b45cd1b 164
xorjoep 1:24714b45cd1b 165 /* Process second sample for 2nd, 6th .. tap */
xorjoep 1:24714b45cd1b 166 /* for sample 2 processing */
xorjoep 1:24714b45cd1b 167 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurnt2;
xorjoep 1:24714b45cd1b 168 fnext2 = __SSAT(fnext2, 16);
xorjoep 1:24714b45cd1b 169 /* Process third sample for 2nd, 6th .. tap */
xorjoep 1:24714b45cd1b 170 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurnt3;
xorjoep 1:24714b45cd1b 171 fnext3 = __SSAT(fnext3, 16);
xorjoep 1:24714b45cd1b 172 /* Process fourth sample for 2nd, 6th .. tap */
xorjoep 1:24714b45cd1b 173 /* fnext4 = fcurnt4 + (*pk) * gnext3; */
xorjoep 1:24714b45cd1b 174 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15U) + fcurnt4;
xorjoep 1:24714b45cd1b 175 fnext4 = __SSAT(fnext4, 16);
xorjoep 1:24714b45cd1b 176
xorjoep 1:24714b45cd1b 177 /* g1(n) = f0(n) * K1 + g0(n-1) */
xorjoep 1:24714b45cd1b 178 /* Calculation of state values for next stage */
xorjoep 1:24714b45cd1b 179 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15U) + gnext3;
xorjoep 1:24714b45cd1b 180 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 181 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15U) + gnext2;
xorjoep 1:24714b45cd1b 182 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 183
xorjoep 1:24714b45cd1b 184 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15U) + gnext1;
xorjoep 1:24714b45cd1b 185 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 186
xorjoep 1:24714b45cd1b 187 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 188 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 189
xorjoep 1:24714b45cd1b 190
xorjoep 1:24714b45cd1b 191 /* Read g2(n-1), g4(n-1) .... from state */
xorjoep 1:24714b45cd1b 192 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 193
xorjoep 1:24714b45cd1b 194 /* save g1(n) in state buffer */
xorjoep 1:24714b45cd1b 195 *px++ = (q15_t) gnext4;
xorjoep 1:24714b45cd1b 196
xorjoep 1:24714b45cd1b 197 /* Sample processing for K3, K7.... */
xorjoep 1:24714b45cd1b 198 /* Process first sample for 3rd, 7th .. tap */
xorjoep 1:24714b45cd1b 199 /* f3(n) = f2(n) + K3 * g2(n-1) */
xorjoep 1:24714b45cd1b 200 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fnext1;
xorjoep 1:24714b45cd1b 201 fcurnt1 = __SSAT(fcurnt1, 16);
xorjoep 1:24714b45cd1b 202
xorjoep 1:24714b45cd1b 203 /* Process second sample for 3rd, 7th .. tap */
xorjoep 1:24714b45cd1b 204 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fnext2;
xorjoep 1:24714b45cd1b 205 fcurnt2 = __SSAT(fcurnt2, 16);
xorjoep 1:24714b45cd1b 206
xorjoep 1:24714b45cd1b 207 /* Process third sample for 3rd, 7th .. tap */
xorjoep 1:24714b45cd1b 208 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fnext3;
xorjoep 1:24714b45cd1b 209 fcurnt3 = __SSAT(fcurnt3, 16);
xorjoep 1:24714b45cd1b 210
xorjoep 1:24714b45cd1b 211 /* Process fourth sample for 3rd, 7th .. tap */
xorjoep 1:24714b45cd1b 212 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15U) + fnext4;
xorjoep 1:24714b45cd1b 213 fcurnt4 = __SSAT(fcurnt4, 16);
xorjoep 1:24714b45cd1b 214
xorjoep 1:24714b45cd1b 215 /* Calculation of state values for next stage */
xorjoep 1:24714b45cd1b 216 /* g3(n) = f2(n) * K3 + g2(n-1) */
xorjoep 1:24714b45cd1b 217 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15U) + gnext3;
xorjoep 1:24714b45cd1b 218 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 219
xorjoep 1:24714b45cd1b 220 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15U) + gnext2;
xorjoep 1:24714b45cd1b 221 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 222
xorjoep 1:24714b45cd1b 223 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15U) + gnext1;
xorjoep 1:24714b45cd1b 224 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 225
xorjoep 1:24714b45cd1b 226 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 227 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 228
xorjoep 1:24714b45cd1b 229 /* Read g1(n-1), g3(n-1) .... from state */
xorjoep 1:24714b45cd1b 230 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 231
xorjoep 1:24714b45cd1b 232 /* save g1(n) in state buffer */
xorjoep 1:24714b45cd1b 233 *px++ = (q15_t) gnext4;
xorjoep 1:24714b45cd1b 234
xorjoep 1:24714b45cd1b 235 /* Sample processing for K4, K8.... */
xorjoep 1:24714b45cd1b 236 /* Process first sample for 4th, 8th .. tap */
xorjoep 1:24714b45cd1b 237 /* f4(n) = f3(n) + K4 * g3(n-1) */
xorjoep 1:24714b45cd1b 238 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 239 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 240
xorjoep 1:24714b45cd1b 241 /* Process second sample for 4th, 8th .. tap */
xorjoep 1:24714b45cd1b 242 /* for sample 2 processing */
xorjoep 1:24714b45cd1b 243 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurnt2;
xorjoep 1:24714b45cd1b 244 fnext2 = __SSAT(fnext2, 16);
xorjoep 1:24714b45cd1b 245
xorjoep 1:24714b45cd1b 246 /* Process third sample for 4th, 8th .. tap */
xorjoep 1:24714b45cd1b 247 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurnt3;
xorjoep 1:24714b45cd1b 248 fnext3 = __SSAT(fnext3, 16);
xorjoep 1:24714b45cd1b 249
xorjoep 1:24714b45cd1b 250 /* Process fourth sample for 4th, 8th .. tap */
xorjoep 1:24714b45cd1b 251 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15U) + fcurnt4;
xorjoep 1:24714b45cd1b 252 fnext4 = __SSAT(fnext4, 16);
xorjoep 1:24714b45cd1b 253
xorjoep 1:24714b45cd1b 254 /* g4(n) = f3(n) * K4 + g3(n-1) */
xorjoep 1:24714b45cd1b 255 /* Calculation of state values for next stage */
xorjoep 1:24714b45cd1b 256 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15U) + gnext3;
xorjoep 1:24714b45cd1b 257 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 258
xorjoep 1:24714b45cd1b 259 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15U) + gnext2;
xorjoep 1:24714b45cd1b 260 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 261
xorjoep 1:24714b45cd1b 262 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15U) + gnext1;
xorjoep 1:24714b45cd1b 263 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 264 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 265 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 266
xorjoep 1:24714b45cd1b 267
xorjoep 1:24714b45cd1b 268 /* Read g2(n-1), g4(n-1) .... from state */
xorjoep 1:24714b45cd1b 269 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 270
xorjoep 1:24714b45cd1b 271 /* save g4(n) in state buffer */
xorjoep 1:24714b45cd1b 272 *px++ = (q15_t) gnext4;
xorjoep 1:24714b45cd1b 273
xorjoep 1:24714b45cd1b 274 /* Sample processing for K5, K9.... */
xorjoep 1:24714b45cd1b 275 /* Process first sample for 5th, 9th .. tap */
xorjoep 1:24714b45cd1b 276 /* f5(n) = f4(n) + K5 * g4(n-1) */
xorjoep 1:24714b45cd1b 277 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fnext1;
xorjoep 1:24714b45cd1b 278 fcurnt1 = __SSAT(fcurnt1, 16);
xorjoep 1:24714b45cd1b 279
xorjoep 1:24714b45cd1b 280 /* Process second sample for 5th, 9th .. tap */
xorjoep 1:24714b45cd1b 281 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fnext2;
xorjoep 1:24714b45cd1b 282 fcurnt2 = __SSAT(fcurnt2, 16);
xorjoep 1:24714b45cd1b 283
xorjoep 1:24714b45cd1b 284 /* Process third sample for 5th, 9th .. tap */
xorjoep 1:24714b45cd1b 285 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fnext3;
xorjoep 1:24714b45cd1b 286 fcurnt3 = __SSAT(fcurnt3, 16);
xorjoep 1:24714b45cd1b 287
xorjoep 1:24714b45cd1b 288 /* Process fourth sample for 5th, 9th .. tap */
xorjoep 1:24714b45cd1b 289 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15U) + fnext4;
xorjoep 1:24714b45cd1b 290 fcurnt4 = __SSAT(fcurnt4, 16);
xorjoep 1:24714b45cd1b 291
xorjoep 1:24714b45cd1b 292 /* Calculation of state values for next stage */
xorjoep 1:24714b45cd1b 293 /* g5(n) = f4(n) * K5 + g4(n-1) */
xorjoep 1:24714b45cd1b 294 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15U) + gnext3;
xorjoep 1:24714b45cd1b 295 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 296 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15U) + gnext2;
xorjoep 1:24714b45cd1b 297 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 298 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15U) + gnext1;
xorjoep 1:24714b45cd1b 299 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 300 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 301 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 302
xorjoep 1:24714b45cd1b 303 stageCnt--;
xorjoep 1:24714b45cd1b 304 }
xorjoep 1:24714b45cd1b 305
xorjoep 1:24714b45cd1b 306 /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
xorjoep 1:24714b45cd1b 307 stageCnt = (numStages - 1U) % 0x4U;
xorjoep 1:24714b45cd1b 308
xorjoep 1:24714b45cd1b 309 while (stageCnt > 0U)
xorjoep 1:24714b45cd1b 310 {
xorjoep 1:24714b45cd1b 311 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 312
xorjoep 1:24714b45cd1b 313 /* save g value in state buffer */
xorjoep 1:24714b45cd1b 314 *px++ = (q15_t) gnext4;
xorjoep 1:24714b45cd1b 315
xorjoep 1:24714b45cd1b 316 /* Process four samples for last three taps here */
xorjoep 1:24714b45cd1b 317 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 318 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 319 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15U) + fcurnt2;
xorjoep 1:24714b45cd1b 320 fnext2 = __SSAT(fnext2, 16);
xorjoep 1:24714b45cd1b 321
xorjoep 1:24714b45cd1b 322 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15U) + fcurnt3;
xorjoep 1:24714b45cd1b 323 fnext3 = __SSAT(fnext3, 16);
xorjoep 1:24714b45cd1b 324
xorjoep 1:24714b45cd1b 325 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15U) + fcurnt4;
xorjoep 1:24714b45cd1b 326 fnext4 = __SSAT(fnext4, 16);
xorjoep 1:24714b45cd1b 327
xorjoep 1:24714b45cd1b 328 /* g1(n) = f0(n) * K1 + g0(n-1) */
xorjoep 1:24714b45cd1b 329 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15U) + gnext3;
xorjoep 1:24714b45cd1b 330 gnext4 = __SSAT(gnext4, 16);
xorjoep 1:24714b45cd1b 331 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15U) + gnext2;
xorjoep 1:24714b45cd1b 332 gnext3 = __SSAT(gnext3, 16);
xorjoep 1:24714b45cd1b 333 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15U) + gnext1;
xorjoep 1:24714b45cd1b 334 gnext2 = __SSAT(gnext2, 16);
xorjoep 1:24714b45cd1b 335 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 336 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 337
xorjoep 1:24714b45cd1b 338 /* Update of f values for next coefficient set processing */
xorjoep 1:24714b45cd1b 339 fcurnt1 = fnext1;
xorjoep 1:24714b45cd1b 340 fcurnt2 = fnext2;
xorjoep 1:24714b45cd1b 341 fcurnt3 = fnext3;
xorjoep 1:24714b45cd1b 342 fcurnt4 = fnext4;
xorjoep 1:24714b45cd1b 343
xorjoep 1:24714b45cd1b 344 stageCnt--;
xorjoep 1:24714b45cd1b 345
xorjoep 1:24714b45cd1b 346 }
xorjoep 1:24714b45cd1b 347
xorjoep 1:24714b45cd1b 348 /* The results in the 4 accumulators, store in the destination buffer. */
xorjoep 1:24714b45cd1b 349 /* y(n) = fN(n) */
xorjoep 1:24714b45cd1b 350
xorjoep 1:24714b45cd1b 351 #ifndef ARM_MATH_BIG_ENDIAN
xorjoep 1:24714b45cd1b 352
xorjoep 1:24714b45cd1b 353 *__SIMD32(pDst)++ = __PKHBT(fcurnt1, fcurnt2, 16);
xorjoep 1:24714b45cd1b 354 *__SIMD32(pDst)++ = __PKHBT(fcurnt3, fcurnt4, 16);
xorjoep 1:24714b45cd1b 355
xorjoep 1:24714b45cd1b 356 #else
xorjoep 1:24714b45cd1b 357
xorjoep 1:24714b45cd1b 358 *__SIMD32(pDst)++ = __PKHBT(fcurnt2, fcurnt1, 16);
xorjoep 1:24714b45cd1b 359 *__SIMD32(pDst)++ = __PKHBT(fcurnt4, fcurnt3, 16);
xorjoep 1:24714b45cd1b 360
xorjoep 1:24714b45cd1b 361 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
xorjoep 1:24714b45cd1b 362
xorjoep 1:24714b45cd1b 363 blkCnt--;
xorjoep 1:24714b45cd1b 364 }
xorjoep 1:24714b45cd1b 365
xorjoep 1:24714b45cd1b 366 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
xorjoep 1:24714b45cd1b 367 ** No loop unrolling is used. */
xorjoep 1:24714b45cd1b 368 blkCnt = blockSize % 0x4U;
xorjoep 1:24714b45cd1b 369
xorjoep 1:24714b45cd1b 370 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 371 {
xorjoep 1:24714b45cd1b 372 /* f0(n) = x(n) */
xorjoep 1:24714b45cd1b 373 fcurnt1 = *pSrc++;
xorjoep 1:24714b45cd1b 374
xorjoep 1:24714b45cd1b 375 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 376 pk = (pCoeffs);
xorjoep 1:24714b45cd1b 377
xorjoep 1:24714b45cd1b 378 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 379 px = pState;
xorjoep 1:24714b45cd1b 380
xorjoep 1:24714b45cd1b 381 /* read g2(n) from state buffer */
xorjoep 1:24714b45cd1b 382 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 383
xorjoep 1:24714b45cd1b 384 /* for sample 1 processing */
xorjoep 1:24714b45cd1b 385 /* f1(n) = f0(n) + K1 * g0(n-1) */
xorjoep 1:24714b45cd1b 386 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 387 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 388
xorjoep 1:24714b45cd1b 389
xorjoep 1:24714b45cd1b 390 /* g1(n) = f0(n) * K1 + g0(n-1) */
xorjoep 1:24714b45cd1b 391 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 392 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 393
xorjoep 1:24714b45cd1b 394 /* save g1(n) in state buffer */
xorjoep 1:24714b45cd1b 395 *px++ = (q15_t) fcurnt1;
xorjoep 1:24714b45cd1b 396
xorjoep 1:24714b45cd1b 397 /* f1(n) is saved in fcurnt1
xorjoep 1:24714b45cd1b 398 for next stage processing */
xorjoep 1:24714b45cd1b 399 fcurnt1 = fnext1;
xorjoep 1:24714b45cd1b 400
xorjoep 1:24714b45cd1b 401 stageCnt = (numStages - 1U);
xorjoep 1:24714b45cd1b 402
xorjoep 1:24714b45cd1b 403 /* stage loop */
xorjoep 1:24714b45cd1b 404 while (stageCnt > 0U)
xorjoep 1:24714b45cd1b 405 {
xorjoep 1:24714b45cd1b 406 /* read g2(n) from state buffer */
xorjoep 1:24714b45cd1b 407 gcurnt1 = *px;
xorjoep 1:24714b45cd1b 408
xorjoep 1:24714b45cd1b 409 /* save g1(n) in state buffer */
xorjoep 1:24714b45cd1b 410 *px++ = (q15_t) gnext1;
xorjoep 1:24714b45cd1b 411
xorjoep 1:24714b45cd1b 412 /* Sample processing for K2, K3.... */
xorjoep 1:24714b45cd1b 413 /* f2(n) = f1(n) + K2 * g1(n-1) */
xorjoep 1:24714b45cd1b 414 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15U) + fcurnt1;
xorjoep 1:24714b45cd1b 415 fnext1 = __SSAT(fnext1, 16);
xorjoep 1:24714b45cd1b 416
xorjoep 1:24714b45cd1b 417 /* g2(n) = f1(n) * K2 + g1(n-1) */
xorjoep 1:24714b45cd1b 418 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15U) + gcurnt1;
xorjoep 1:24714b45cd1b 419 gnext1 = __SSAT(gnext1, 16);
xorjoep 1:24714b45cd1b 420
xorjoep 1:24714b45cd1b 421
xorjoep 1:24714b45cd1b 422 /* f1(n) is saved in fcurnt1
xorjoep 1:24714b45cd1b 423 for next stage processing */
xorjoep 1:24714b45cd1b 424 fcurnt1 = fnext1;
xorjoep 1:24714b45cd1b 425
xorjoep 1:24714b45cd1b 426 stageCnt--;
xorjoep 1:24714b45cd1b 427
xorjoep 1:24714b45cd1b 428 }
xorjoep 1:24714b45cd1b 429
xorjoep 1:24714b45cd1b 430 /* y(n) = fN(n) */
xorjoep 1:24714b45cd1b 431 *pDst++ = __SSAT(fcurnt1, 16);
xorjoep 1:24714b45cd1b 432
xorjoep 1:24714b45cd1b 433
xorjoep 1:24714b45cd1b 434 blkCnt--;
xorjoep 1:24714b45cd1b 435
xorjoep 1:24714b45cd1b 436 }
xorjoep 1:24714b45cd1b 437
xorjoep 1:24714b45cd1b 438 #else
xorjoep 1:24714b45cd1b 439
xorjoep 1:24714b45cd1b 440 /* Run the below code for Cortex-M0 */
xorjoep 1:24714b45cd1b 441
xorjoep 1:24714b45cd1b 442 q31_t fcurnt, fnext, gcurnt, gnext; /* temporary variables */
xorjoep 1:24714b45cd1b 443 uint32_t numStages = S->numStages; /* Length of the filter */
xorjoep 1:24714b45cd1b 444 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
xorjoep 1:24714b45cd1b 445
xorjoep 1:24714b45cd1b 446 pState = &S->pState[0];
xorjoep 1:24714b45cd1b 447
xorjoep 1:24714b45cd1b 448 blkCnt = blockSize;
xorjoep 1:24714b45cd1b 449
xorjoep 1:24714b45cd1b 450 while (blkCnt > 0U)
xorjoep 1:24714b45cd1b 451 {
xorjoep 1:24714b45cd1b 452 /* f0(n) = x(n) */
xorjoep 1:24714b45cd1b 453 fcurnt = *pSrc++;
xorjoep 1:24714b45cd1b 454
xorjoep 1:24714b45cd1b 455 /* Initialize coeff pointer */
xorjoep 1:24714b45cd1b 456 pk = (pCoeffs);
xorjoep 1:24714b45cd1b 457
xorjoep 1:24714b45cd1b 458 /* Initialize state pointer */
xorjoep 1:24714b45cd1b 459 px = pState;
xorjoep 1:24714b45cd1b 460
xorjoep 1:24714b45cd1b 461 /* read g0(n-1) from state buffer */
xorjoep 1:24714b45cd1b 462 gcurnt = *px;
xorjoep 1:24714b45cd1b 463
xorjoep 1:24714b45cd1b 464 /* for sample 1 processing */
xorjoep 1:24714b45cd1b 465 /* f1(n) = f0(n) + K1 * g0(n-1) */
xorjoep 1:24714b45cd1b 466 fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
xorjoep 1:24714b45cd1b 467 fnext = __SSAT(fnext, 16);
xorjoep 1:24714b45cd1b 468
xorjoep 1:24714b45cd1b 469
xorjoep 1:24714b45cd1b 470 /* g1(n) = f0(n) * K1 + g0(n-1) */
xorjoep 1:24714b45cd1b 471 gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
xorjoep 1:24714b45cd1b 472 gnext = __SSAT(gnext, 16);
xorjoep 1:24714b45cd1b 473
xorjoep 1:24714b45cd1b 474 /* save f0(n) in state buffer */
xorjoep 1:24714b45cd1b 475 *px++ = (q15_t) fcurnt;
xorjoep 1:24714b45cd1b 476
xorjoep 1:24714b45cd1b 477 /* f1(n) is saved in fcurnt
xorjoep 1:24714b45cd1b 478 for next stage processing */
xorjoep 1:24714b45cd1b 479 fcurnt = fnext;
xorjoep 1:24714b45cd1b 480
xorjoep 1:24714b45cd1b 481 stageCnt = (numStages - 1U);
xorjoep 1:24714b45cd1b 482
xorjoep 1:24714b45cd1b 483 /* stage loop */
xorjoep 1:24714b45cd1b 484 while (stageCnt > 0U)
xorjoep 1:24714b45cd1b 485 {
xorjoep 1:24714b45cd1b 486 /* read g1(n-1) from state buffer */
xorjoep 1:24714b45cd1b 487 gcurnt = *px;
xorjoep 1:24714b45cd1b 488
xorjoep 1:24714b45cd1b 489 /* save g0(n-1) in state buffer */
xorjoep 1:24714b45cd1b 490 *px++ = (q15_t) gnext;
xorjoep 1:24714b45cd1b 491
xorjoep 1:24714b45cd1b 492 /* Sample processing for K2, K3.... */
xorjoep 1:24714b45cd1b 493 /* f2(n) = f1(n) + K2 * g1(n-1) */
xorjoep 1:24714b45cd1b 494 fnext = ((gcurnt * (*pk)) >> 15U) + fcurnt;
xorjoep 1:24714b45cd1b 495 fnext = __SSAT(fnext, 16);
xorjoep 1:24714b45cd1b 496
xorjoep 1:24714b45cd1b 497 /* g2(n) = f1(n) * K2 + g1(n-1) */
xorjoep 1:24714b45cd1b 498 gnext = ((fcurnt * (*pk++)) >> 15U) + gcurnt;
xorjoep 1:24714b45cd1b 499 gnext = __SSAT(gnext, 16);
xorjoep 1:24714b45cd1b 500
xorjoep 1:24714b45cd1b 501
xorjoep 1:24714b45cd1b 502 /* f1(n) is saved in fcurnt
xorjoep 1:24714b45cd1b 503 for next stage processing */
xorjoep 1:24714b45cd1b 504 fcurnt = fnext;
xorjoep 1:24714b45cd1b 505
xorjoep 1:24714b45cd1b 506 stageCnt--;
xorjoep 1:24714b45cd1b 507
xorjoep 1:24714b45cd1b 508 }
xorjoep 1:24714b45cd1b 509
xorjoep 1:24714b45cd1b 510 /* y(n) = fN(n) */
xorjoep 1:24714b45cd1b 511 *pDst++ = __SSAT(fcurnt, 16);
xorjoep 1:24714b45cd1b 512
xorjoep 1:24714b45cd1b 513
xorjoep 1:24714b45cd1b 514 blkCnt--;
xorjoep 1:24714b45cd1b 515
xorjoep 1:24714b45cd1b 516 }
xorjoep 1:24714b45cd1b 517
xorjoep 1:24714b45cd1b 518 #endif /* #if defined (ARM_MATH_DSP) */
xorjoep 1:24714b45cd1b 519
xorjoep 1:24714b45cd1b 520 }
xorjoep 1:24714b45cd1b 521
xorjoep 1:24714b45cd1b 522 /**
xorjoep 1:24714b45cd1b 523 * @} end of FIR_Lattice group
xorjoep 1:24714b45cd1b 524 */