mbed official / mbed-dsp

CMSIS DSP library

Dependents:   performance_timer Surfboard_ gps2rtty Capstone ... more

Legacy Warning

This is an mbed 2 library. To learn more about mbed OS 5, visit the docs.

cmsis_dsp/FilteringFunctions/arm_fir_lattice_q15.c@3:7a284390b0ce, 2013-11-08 (annotated)

Committer:
mbed_official
Date:
Fri Nov 08 13:45:10 2013 +0000
Revision:
3:7a284390b0ce
Parent:
2:da51fb522205
Child:
5:3762170b6d4d
Synchronized with git revision e69956aba2f68a2a26ac26b051f8d349deaa1ce8

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
mbed_official 3:7a284390b0ce 2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
mbed_official 3:7a284390b0ce 4 * $Date: 17. January 2013
mbed_official 3:7a284390b0ce 5 * $Revision: V1.4.1
emilmont 1:fdd22bb7aa52 6 *
emilmont 2:da51fb522205 7 * Project: CMSIS DSP Library
emilmont 2:da51fb522205 8 * Title: arm_fir_lattice_q15.c
emilmont 1:fdd22bb7aa52 9 *
emilmont 2:da51fb522205 10 * Description: Q15 FIR lattice filter processing function.
emilmont 1:fdd22bb7aa52 11 *
emilmont 1:fdd22bb7aa52 12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
emilmont 1:fdd22bb7aa52 13 *
mbed_official 3:7a284390b0ce 14 * Redistribution and use in source and binary forms, with or without
mbed_official 3:7a284390b0ce 15 * modification, are permitted provided that the following conditions
mbed_official 3:7a284390b0ce 16 * are met:
mbed_official 3:7a284390b0ce 17 * - Redistributions of source code must retain the above copyright
mbed_official 3:7a284390b0ce 18 * notice, this list of conditions and the following disclaimer.
mbed_official 3:7a284390b0ce 19 * - Redistributions in binary form must reproduce the above copyright
mbed_official 3:7a284390b0ce 20 * notice, this list of conditions and the following disclaimer in
mbed_official 3:7a284390b0ce 21 * the documentation and/or other materials provided with the
mbed_official 3:7a284390b0ce 22 * distribution.
mbed_official 3:7a284390b0ce 23 * - Neither the name of ARM LIMITED nor the names of its contributors
mbed_official 3:7a284390b0ce 24 * may be used to endorse or promote products derived from this
mbed_official 3:7a284390b0ce 25 * software without specific prior written permission.
mbed_official 3:7a284390b0ce 26 *
mbed_official 3:7a284390b0ce 27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
mbed_official 3:7a284390b0ce 28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
mbed_official 3:7a284390b0ce 29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
mbed_official 3:7a284390b0ce 30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
mbed_official 3:7a284390b0ce 31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
mbed_official 3:7a284390b0ce 32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
mbed_official 3:7a284390b0ce 33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
mbed_official 3:7a284390b0ce 34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
mbed_official 3:7a284390b0ce 35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
mbed_official 3:7a284390b0ce 36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
mbed_official 3:7a284390b0ce 37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 3:7a284390b0ce 38 * POSSIBILITY OF SUCH DAMAGE.
emilmont 1:fdd22bb7aa52 39 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 40
emilmont 1:fdd22bb7aa52 41 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 42
emilmont 1:fdd22bb7aa52 43 /**
emilmont 1:fdd22bb7aa52 44 * @ingroup groupFilters
emilmont 1:fdd22bb7aa52 45 */
emilmont 1:fdd22bb7aa52 46
emilmont 1:fdd22bb7aa52 47 /**
emilmont 1:fdd22bb7aa52 48 * @addtogroup FIR_Lattice
emilmont 1:fdd22bb7aa52 49 * @{
emilmont 1:fdd22bb7aa52 50 */
emilmont 1:fdd22bb7aa52 51
emilmont 1:fdd22bb7aa52 52
emilmont 1:fdd22bb7aa52 53 /**
emilmont 1:fdd22bb7aa52 54 * @brief Processing function for the Q15 FIR lattice filter.
emilmont 1:fdd22bb7aa52 55 * @param[in] *S points to an instance of the Q15 FIR lattice structure.
emilmont 1:fdd22bb7aa52 56 * @param[in] *pSrc points to the block of input data.
emilmont 1:fdd22bb7aa52 57 * @param[out] *pDst points to the block of output data
emilmont 1:fdd22bb7aa52 58 * @param[in] blockSize number of samples to process.
emilmont 1:fdd22bb7aa52 59 * @return none.
emilmont 1:fdd22bb7aa52 60 */
emilmont 1:fdd22bb7aa52 61
emilmont 1:fdd22bb7aa52 62 void arm_fir_lattice_q15(
emilmont 1:fdd22bb7aa52 63 const arm_fir_lattice_instance_q15 * S,
emilmont 1:fdd22bb7aa52 64 q15_t * pSrc,
emilmont 1:fdd22bb7aa52 65 q15_t * pDst,
emilmont 1:fdd22bb7aa52 66 uint32_t blockSize)
emilmont 1:fdd22bb7aa52 67 {
emilmont 1:fdd22bb7aa52 68 q15_t *pState; /* State pointer */
emilmont 1:fdd22bb7aa52 69 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
emilmont 1:fdd22bb7aa52 70 q15_t *px; /* temporary state pointer */
emilmont 1:fdd22bb7aa52 71 q15_t *pk; /* temporary coefficient pointer */
emilmont 1:fdd22bb7aa52 72
emilmont 1:fdd22bb7aa52 73
mbed_official 3:7a284390b0ce 74 #ifndef ARM_MATH_CM0_FAMILY
emilmont 1:fdd22bb7aa52 75
emilmont 1:fdd22bb7aa52 76 /* Run the below code for Cortex-M4 and Cortex-M3 */
emilmont 1:fdd22bb7aa52 77
emilmont 1:fdd22bb7aa52 78 q31_t fcurnt1, fnext1, gcurnt1 = 0, gnext1; /* temporary variables for first sample in loop unrolling */
emilmont 1:fdd22bb7aa52 79 q31_t fcurnt2, fnext2, gnext2; /* temporary variables for second sample in loop unrolling */
emilmont 1:fdd22bb7aa52 80 q31_t fcurnt3, fnext3, gnext3; /* temporary variables for third sample in loop unrolling */
emilmont 1:fdd22bb7aa52 81 q31_t fcurnt4, fnext4, gnext4; /* temporary variables for fourth sample in loop unrolling */
emilmont 1:fdd22bb7aa52 82 uint32_t numStages = S->numStages; /* Number of stages in the filter */
emilmont 1:fdd22bb7aa52 83 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
emilmont 1:fdd22bb7aa52 84
emilmont 1:fdd22bb7aa52 85 pState = &S->pState[0];
emilmont 1:fdd22bb7aa52 86
emilmont 1:fdd22bb7aa52 87 blkCnt = blockSize >> 2u;
emilmont 1:fdd22bb7aa52 88
emilmont 1:fdd22bb7aa52 89 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
emilmont 1:fdd22bb7aa52 90 ** a second loop below computes the remaining 1 to 3 samples. */
emilmont 1:fdd22bb7aa52 91 while(blkCnt > 0u)
emilmont 1:fdd22bb7aa52 92 {
emilmont 1:fdd22bb7aa52 93
emilmont 1:fdd22bb7aa52 94 /* Read two samples from input buffer */
emilmont 1:fdd22bb7aa52 95 /* f0(n) = x(n) */
emilmont 1:fdd22bb7aa52 96 fcurnt1 = *pSrc++;
emilmont 1:fdd22bb7aa52 97 fcurnt2 = *pSrc++;
emilmont 1:fdd22bb7aa52 98
emilmont 1:fdd22bb7aa52 99 /* Initialize coeff pointer */
emilmont 1:fdd22bb7aa52 100 pk = (pCoeffs);
emilmont 1:fdd22bb7aa52 101
emilmont 1:fdd22bb7aa52 102 /* Initialize state pointer */
emilmont 1:fdd22bb7aa52 103 px = pState;
emilmont 1:fdd22bb7aa52 104
emilmont 1:fdd22bb7aa52 105 /* Read g0(n-1) from state */
emilmont 1:fdd22bb7aa52 106 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 107
emilmont 1:fdd22bb7aa52 108 /* Process first sample for first tap */
emilmont 1:fdd22bb7aa52 109 /* f1(n) = f0(n) + K1 * g0(n-1) */
emilmont 1:fdd22bb7aa52 110 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 111 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 112
emilmont 1:fdd22bb7aa52 113 /* g1(n) = f0(n) * K1 + g0(n-1) */
emilmont 1:fdd22bb7aa52 114 gnext1 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 115 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 116
emilmont 1:fdd22bb7aa52 117 /* Process second sample for first tap */
emilmont 1:fdd22bb7aa52 118 /* for sample 2 processing */
emilmont 1:fdd22bb7aa52 119 fnext2 = (q31_t) ((fcurnt1 * (*pk)) >> 15u) + fcurnt2;
emilmont 1:fdd22bb7aa52 120 fnext2 = __SSAT(fnext2, 16);
emilmont 1:fdd22bb7aa52 121
emilmont 1:fdd22bb7aa52 122 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 123 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 124
emilmont 1:fdd22bb7aa52 125
emilmont 1:fdd22bb7aa52 126 /* Read next two samples from input buffer */
emilmont 1:fdd22bb7aa52 127 /* f0(n+2) = x(n+2) */
emilmont 1:fdd22bb7aa52 128 fcurnt3 = *pSrc++;
emilmont 1:fdd22bb7aa52 129 fcurnt4 = *pSrc++;
emilmont 1:fdd22bb7aa52 130
emilmont 1:fdd22bb7aa52 131 /* Copy only last input samples into the state buffer
emilmont 1:fdd22bb7aa52 132 which is used for next four samples processing */
emilmont 1:fdd22bb7aa52 133 *px++ = (q15_t) fcurnt4;
emilmont 1:fdd22bb7aa52 134
emilmont 1:fdd22bb7aa52 135 /* Process third sample for first tap */
emilmont 1:fdd22bb7aa52 136 fnext3 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + fcurnt3;
emilmont 1:fdd22bb7aa52 137 fnext3 = __SSAT(fnext3, 16);
emilmont 1:fdd22bb7aa52 138 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt2;
emilmont 1:fdd22bb7aa52 139 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 140
emilmont 1:fdd22bb7aa52 141 /* Process fourth sample for first tap */
emilmont 1:fdd22bb7aa52 142 fnext4 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + fcurnt4;
emilmont 1:fdd22bb7aa52 143 fnext4 = __SSAT(fnext4, 16);
emilmont 1:fdd22bb7aa52 144 gnext4 = (q31_t) ((fcurnt4 * (*pk++)) >> 15u) + fcurnt3;
emilmont 1:fdd22bb7aa52 145 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 146
emilmont 1:fdd22bb7aa52 147 /* Update of f values for next coefficient set processing */
emilmont 1:fdd22bb7aa52 148 fcurnt1 = fnext1;
emilmont 1:fdd22bb7aa52 149 fcurnt2 = fnext2;
emilmont 1:fdd22bb7aa52 150 fcurnt3 = fnext3;
emilmont 1:fdd22bb7aa52 151 fcurnt4 = fnext4;
emilmont 1:fdd22bb7aa52 152
emilmont 1:fdd22bb7aa52 153
emilmont 1:fdd22bb7aa52 154 /* Loop unrolling. Process 4 taps at a time . */
emilmont 1:fdd22bb7aa52 155 stageCnt = (numStages - 1u) >> 2;
emilmont 1:fdd22bb7aa52 156
emilmont 1:fdd22bb7aa52 157
emilmont 1:fdd22bb7aa52 158 /* Loop over the number of taps. Unroll by a factor of 4.
emilmont 1:fdd22bb7aa52 159 ** Repeat until we've computed numStages-3 coefficients. */
emilmont 1:fdd22bb7aa52 160
emilmont 1:fdd22bb7aa52 161 /* Process 2nd, 3rd, 4th and 5th taps ... here */
emilmont 1:fdd22bb7aa52 162 while(stageCnt > 0u)
emilmont 1:fdd22bb7aa52 163 {
emilmont 1:fdd22bb7aa52 164 /* Read g1(n-1), g3(n-1) .... from state */
emilmont 1:fdd22bb7aa52 165 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 166
emilmont 1:fdd22bb7aa52 167 /* save g1(n) in state buffer */
emilmont 1:fdd22bb7aa52 168 *px++ = (q15_t) gnext4;
emilmont 1:fdd22bb7aa52 169
emilmont 1:fdd22bb7aa52 170 /* Process first sample for 2nd, 6th .. tap */
emilmont 1:fdd22bb7aa52 171 /* Sample processing for K2, K6.... */
emilmont 1:fdd22bb7aa52 172 /* f1(n) = f0(n) + K1 * g0(n-1) */
emilmont 1:fdd22bb7aa52 173 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 174 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 175
emilmont 1:fdd22bb7aa52 176
emilmont 1:fdd22bb7aa52 177 /* Process second sample for 2nd, 6th .. tap */
emilmont 1:fdd22bb7aa52 178 /* for sample 2 processing */
emilmont 1:fdd22bb7aa52 179 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
emilmont 1:fdd22bb7aa52 180 fnext2 = __SSAT(fnext2, 16);
emilmont 1:fdd22bb7aa52 181 /* Process third sample for 2nd, 6th .. tap */
emilmont 1:fdd22bb7aa52 182 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
emilmont 1:fdd22bb7aa52 183 fnext3 = __SSAT(fnext3, 16);
emilmont 1:fdd22bb7aa52 184 /* Process fourth sample for 2nd, 6th .. tap */
emilmont 1:fdd22bb7aa52 185 /* fnext4 = fcurnt4 + (*pk) * gnext3; */
emilmont 1:fdd22bb7aa52 186 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
emilmont 1:fdd22bb7aa52 187 fnext4 = __SSAT(fnext4, 16);
emilmont 1:fdd22bb7aa52 188
emilmont 1:fdd22bb7aa52 189 /* g1(n) = f0(n) * K1 + g0(n-1) */
emilmont 1:fdd22bb7aa52 190 /* Calculation of state values for next stage */
emilmont 1:fdd22bb7aa52 191 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
emilmont 1:fdd22bb7aa52 192 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 193 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
emilmont 1:fdd22bb7aa52 194 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 195
emilmont 1:fdd22bb7aa52 196 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
emilmont 1:fdd22bb7aa52 197 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 198
emilmont 1:fdd22bb7aa52 199 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 200 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 201
emilmont 1:fdd22bb7aa52 202
emilmont 1:fdd22bb7aa52 203 /* Read g2(n-1), g4(n-1) .... from state */
emilmont 1:fdd22bb7aa52 204 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 205
emilmont 1:fdd22bb7aa52 206 /* save g1(n) in state buffer */
emilmont 1:fdd22bb7aa52 207 *px++ = (q15_t) gnext4;
emilmont 1:fdd22bb7aa52 208
emilmont 1:fdd22bb7aa52 209 /* Sample processing for K3, K7.... */
emilmont 1:fdd22bb7aa52 210 /* Process first sample for 3rd, 7th .. tap */
emilmont 1:fdd22bb7aa52 211 /* f3(n) = f2(n) + K3 * g2(n-1) */
emilmont 1:fdd22bb7aa52 212 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
emilmont 1:fdd22bb7aa52 213 fcurnt1 = __SSAT(fcurnt1, 16);
emilmont 1:fdd22bb7aa52 214
emilmont 1:fdd22bb7aa52 215 /* Process second sample for 3rd, 7th .. tap */
emilmont 1:fdd22bb7aa52 216 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
emilmont 1:fdd22bb7aa52 217 fcurnt2 = __SSAT(fcurnt2, 16);
emilmont 1:fdd22bb7aa52 218
emilmont 1:fdd22bb7aa52 219 /* Process third sample for 3rd, 7th .. tap */
emilmont 1:fdd22bb7aa52 220 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
emilmont 1:fdd22bb7aa52 221 fcurnt3 = __SSAT(fcurnt3, 16);
emilmont 1:fdd22bb7aa52 222
emilmont 1:fdd22bb7aa52 223 /* Process fourth sample for 3rd, 7th .. tap */
emilmont 1:fdd22bb7aa52 224 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
emilmont 1:fdd22bb7aa52 225 fcurnt4 = __SSAT(fcurnt4, 16);
emilmont 1:fdd22bb7aa52 226
emilmont 1:fdd22bb7aa52 227 /* Calculation of state values for next stage */
emilmont 1:fdd22bb7aa52 228 /* g3(n) = f2(n) * K3 + g2(n-1) */
emilmont 1:fdd22bb7aa52 229 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
emilmont 1:fdd22bb7aa52 230 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 231
emilmont 1:fdd22bb7aa52 232 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
emilmont 1:fdd22bb7aa52 233 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 234
emilmont 1:fdd22bb7aa52 235 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
emilmont 1:fdd22bb7aa52 236 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 237
emilmont 1:fdd22bb7aa52 238 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 239 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 240
emilmont 1:fdd22bb7aa52 241 /* Read g1(n-1), g3(n-1) .... from state */
emilmont 1:fdd22bb7aa52 242 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 243
emilmont 1:fdd22bb7aa52 244 /* save g1(n) in state buffer */
emilmont 1:fdd22bb7aa52 245 *px++ = (q15_t) gnext4;
emilmont 1:fdd22bb7aa52 246
emilmont 1:fdd22bb7aa52 247 /* Sample processing for K4, K8.... */
emilmont 1:fdd22bb7aa52 248 /* Process first sample for 4th, 8th .. tap */
emilmont 1:fdd22bb7aa52 249 /* f4(n) = f3(n) + K4 * g3(n-1) */
emilmont 1:fdd22bb7aa52 250 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 251 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 252
emilmont 1:fdd22bb7aa52 253 /* Process second sample for 4th, 8th .. tap */
emilmont 1:fdd22bb7aa52 254 /* for sample 2 processing */
emilmont 1:fdd22bb7aa52 255 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
emilmont 1:fdd22bb7aa52 256 fnext2 = __SSAT(fnext2, 16);
emilmont 1:fdd22bb7aa52 257
emilmont 1:fdd22bb7aa52 258 /* Process third sample for 4th, 8th .. tap */
emilmont 1:fdd22bb7aa52 259 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
emilmont 1:fdd22bb7aa52 260 fnext3 = __SSAT(fnext3, 16);
emilmont 1:fdd22bb7aa52 261
emilmont 1:fdd22bb7aa52 262 /* Process fourth sample for 4th, 8th .. tap */
emilmont 1:fdd22bb7aa52 263 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
emilmont 1:fdd22bb7aa52 264 fnext4 = __SSAT(fnext4, 16);
emilmont 1:fdd22bb7aa52 265
emilmont 1:fdd22bb7aa52 266 /* g4(n) = f3(n) * K4 + g3(n-1) */
emilmont 1:fdd22bb7aa52 267 /* Calculation of state values for next stage */
emilmont 1:fdd22bb7aa52 268 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
emilmont 1:fdd22bb7aa52 269 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 270
emilmont 1:fdd22bb7aa52 271 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
emilmont 1:fdd22bb7aa52 272 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 273
emilmont 1:fdd22bb7aa52 274 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
emilmont 1:fdd22bb7aa52 275 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 276 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 277 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 278
emilmont 1:fdd22bb7aa52 279
emilmont 1:fdd22bb7aa52 280 /* Read g2(n-1), g4(n-1) .... from state */
emilmont 1:fdd22bb7aa52 281 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 282
emilmont 1:fdd22bb7aa52 283 /* save g4(n) in state buffer */
emilmont 1:fdd22bb7aa52 284 *px++ = (q15_t) gnext4;
emilmont 1:fdd22bb7aa52 285
emilmont 1:fdd22bb7aa52 286 /* Sample processing for K5, K9.... */
emilmont 1:fdd22bb7aa52 287 /* Process first sample for 5th, 9th .. tap */
emilmont 1:fdd22bb7aa52 288 /* f5(n) = f4(n) + K5 * g4(n-1) */
emilmont 1:fdd22bb7aa52 289 fcurnt1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fnext1;
emilmont 1:fdd22bb7aa52 290 fcurnt1 = __SSAT(fcurnt1, 16);
emilmont 1:fdd22bb7aa52 291
emilmont 1:fdd22bb7aa52 292 /* Process second sample for 5th, 9th .. tap */
emilmont 1:fdd22bb7aa52 293 fcurnt2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fnext2;
emilmont 1:fdd22bb7aa52 294 fcurnt2 = __SSAT(fcurnt2, 16);
emilmont 1:fdd22bb7aa52 295
emilmont 1:fdd22bb7aa52 296 /* Process third sample for 5th, 9th .. tap */
emilmont 1:fdd22bb7aa52 297 fcurnt3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fnext3;
emilmont 1:fdd22bb7aa52 298 fcurnt3 = __SSAT(fcurnt3, 16);
emilmont 1:fdd22bb7aa52 299
emilmont 1:fdd22bb7aa52 300 /* Process fourth sample for 5th, 9th .. tap */
emilmont 1:fdd22bb7aa52 301 fcurnt4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fnext4;
emilmont 1:fdd22bb7aa52 302 fcurnt4 = __SSAT(fcurnt4, 16);
emilmont 1:fdd22bb7aa52 303
emilmont 1:fdd22bb7aa52 304 /* Calculation of state values for next stage */
emilmont 1:fdd22bb7aa52 305 /* g5(n) = f4(n) * K5 + g4(n-1) */
emilmont 1:fdd22bb7aa52 306 gnext4 = (q31_t) ((fnext4 * (*pk)) >> 15u) + gnext3;
emilmont 1:fdd22bb7aa52 307 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 308 gnext3 = (q31_t) ((fnext3 * (*pk)) >> 15u) + gnext2;
emilmont 1:fdd22bb7aa52 309 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 310 gnext2 = (q31_t) ((fnext2 * (*pk)) >> 15u) + gnext1;
emilmont 1:fdd22bb7aa52 311 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 312 gnext1 = (q31_t) ((fnext1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 313 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 314
emilmont 1:fdd22bb7aa52 315 stageCnt--;
emilmont 1:fdd22bb7aa52 316 }
emilmont 1:fdd22bb7aa52 317
emilmont 1:fdd22bb7aa52 318 /* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */
emilmont 1:fdd22bb7aa52 319 stageCnt = (numStages - 1u) % 0x4u;
emilmont 1:fdd22bb7aa52 320
emilmont 1:fdd22bb7aa52 321 while(stageCnt > 0u)
emilmont 1:fdd22bb7aa52 322 {
emilmont 1:fdd22bb7aa52 323 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 324
emilmont 1:fdd22bb7aa52 325 /* save g value in state buffer */
emilmont 1:fdd22bb7aa52 326 *px++ = (q15_t) gnext4;
emilmont 1:fdd22bb7aa52 327
emilmont 1:fdd22bb7aa52 328 /* Process four samples for last three taps here */
emilmont 1:fdd22bb7aa52 329 fnext1 = (q31_t) ((gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 330 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 331 fnext2 = (q31_t) ((gnext1 * (*pk)) >> 15u) + fcurnt2;
emilmont 1:fdd22bb7aa52 332 fnext2 = __SSAT(fnext2, 16);
emilmont 1:fdd22bb7aa52 333
emilmont 1:fdd22bb7aa52 334 fnext3 = (q31_t) ((gnext2 * (*pk)) >> 15u) + fcurnt3;
emilmont 1:fdd22bb7aa52 335 fnext3 = __SSAT(fnext3, 16);
emilmont 1:fdd22bb7aa52 336
emilmont 1:fdd22bb7aa52 337 fnext4 = (q31_t) ((gnext3 * (*pk)) >> 15u) + fcurnt4;
emilmont 1:fdd22bb7aa52 338 fnext4 = __SSAT(fnext4, 16);
emilmont 1:fdd22bb7aa52 339
emilmont 1:fdd22bb7aa52 340 /* g1(n) = f0(n) * K1 + g0(n-1) */
emilmont 1:fdd22bb7aa52 341 gnext4 = (q31_t) ((fcurnt4 * (*pk)) >> 15u) + gnext3;
emilmont 1:fdd22bb7aa52 342 gnext4 = __SSAT(gnext4, 16);
emilmont 1:fdd22bb7aa52 343 gnext3 = (q31_t) ((fcurnt3 * (*pk)) >> 15u) + gnext2;
emilmont 1:fdd22bb7aa52 344 gnext3 = __SSAT(gnext3, 16);
emilmont 1:fdd22bb7aa52 345 gnext2 = (q31_t) ((fcurnt2 * (*pk)) >> 15u) + gnext1;
emilmont 1:fdd22bb7aa52 346 gnext2 = __SSAT(gnext2, 16);
emilmont 1:fdd22bb7aa52 347 gnext1 = (q31_t) ((fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 348 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 349
emilmont 1:fdd22bb7aa52 350 /* Update of f values for next coefficient set processing */
emilmont 1:fdd22bb7aa52 351 fcurnt1 = fnext1;
emilmont 1:fdd22bb7aa52 352 fcurnt2 = fnext2;
emilmont 1:fdd22bb7aa52 353 fcurnt3 = fnext3;
emilmont 1:fdd22bb7aa52 354 fcurnt4 = fnext4;
emilmont 1:fdd22bb7aa52 355
emilmont 1:fdd22bb7aa52 356 stageCnt--;
emilmont 1:fdd22bb7aa52 357
emilmont 1:fdd22bb7aa52 358 }
emilmont 1:fdd22bb7aa52 359
emilmont 1:fdd22bb7aa52 360 /* The results in the 4 accumulators, store in the destination buffer. */
emilmont 1:fdd22bb7aa52 361 /* y(n) = fN(n) */
emilmont 1:fdd22bb7aa52 362
emilmont 1:fdd22bb7aa52 363 #ifndef ARM_MATH_BIG_ENDIAN
emilmont 1:fdd22bb7aa52 364
emilmont 1:fdd22bb7aa52 365 *__SIMD32(pDst)++ = __PKHBT(fcurnt1, fcurnt2, 16);
emilmont 1:fdd22bb7aa52 366 *__SIMD32(pDst)++ = __PKHBT(fcurnt3, fcurnt4, 16);
emilmont 1:fdd22bb7aa52 367
emilmont 1:fdd22bb7aa52 368 #else
emilmont 1:fdd22bb7aa52 369
emilmont 1:fdd22bb7aa52 370 *__SIMD32(pDst)++ = __PKHBT(fcurnt2, fcurnt1, 16);
emilmont 1:fdd22bb7aa52 371 *__SIMD32(pDst)++ = __PKHBT(fcurnt4, fcurnt3, 16);
emilmont 1:fdd22bb7aa52 372
emilmont 1:fdd22bb7aa52 373 #endif /* #ifndef ARM_MATH_BIG_ENDIAN */
emilmont 1:fdd22bb7aa52 374
emilmont 1:fdd22bb7aa52 375 blkCnt--;
emilmont 1:fdd22bb7aa52 376 }
emilmont 1:fdd22bb7aa52 377
emilmont 1:fdd22bb7aa52 378 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
emilmont 1:fdd22bb7aa52 379 ** No loop unrolling is used. */
emilmont 1:fdd22bb7aa52 380 blkCnt = blockSize % 0x4u;
emilmont 1:fdd22bb7aa52 381
emilmont 1:fdd22bb7aa52 382 while(blkCnt > 0u)
emilmont 1:fdd22bb7aa52 383 {
emilmont 1:fdd22bb7aa52 384 /* f0(n) = x(n) */
emilmont 1:fdd22bb7aa52 385 fcurnt1 = *pSrc++;
emilmont 1:fdd22bb7aa52 386
emilmont 1:fdd22bb7aa52 387 /* Initialize coeff pointer */
emilmont 1:fdd22bb7aa52 388 pk = (pCoeffs);
emilmont 1:fdd22bb7aa52 389
emilmont 1:fdd22bb7aa52 390 /* Initialize state pointer */
emilmont 1:fdd22bb7aa52 391 px = pState;
emilmont 1:fdd22bb7aa52 392
emilmont 1:fdd22bb7aa52 393 /* read g2(n) from state buffer */
emilmont 1:fdd22bb7aa52 394 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 395
emilmont 1:fdd22bb7aa52 396 /* for sample 1 processing */
emilmont 1:fdd22bb7aa52 397 /* f1(n) = f0(n) + K1 * g0(n-1) */
emilmont 1:fdd22bb7aa52 398 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 399 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 400
emilmont 1:fdd22bb7aa52 401
emilmont 1:fdd22bb7aa52 402 /* g1(n) = f0(n) * K1 + g0(n-1) */
emilmont 1:fdd22bb7aa52 403 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 404 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 405
emilmont 1:fdd22bb7aa52 406 /* save g1(n) in state buffer */
emilmont 1:fdd22bb7aa52 407 *px++ = (q15_t) fcurnt1;
emilmont 1:fdd22bb7aa52 408
emilmont 1:fdd22bb7aa52 409 /* f1(n) is saved in fcurnt1
emilmont 1:fdd22bb7aa52 410 for next stage processing */
emilmont 1:fdd22bb7aa52 411 fcurnt1 = fnext1;
emilmont 1:fdd22bb7aa52 412
emilmont 1:fdd22bb7aa52 413 stageCnt = (numStages - 1u);
emilmont 1:fdd22bb7aa52 414
emilmont 1:fdd22bb7aa52 415 /* stage loop */
emilmont 1:fdd22bb7aa52 416 while(stageCnt > 0u)
emilmont 1:fdd22bb7aa52 417 {
emilmont 1:fdd22bb7aa52 418 /* read g2(n) from state buffer */
emilmont 1:fdd22bb7aa52 419 gcurnt1 = *px;
emilmont 1:fdd22bb7aa52 420
emilmont 1:fdd22bb7aa52 421 /* save g1(n) in state buffer */
emilmont 1:fdd22bb7aa52 422 *px++ = (q15_t) gnext1;
emilmont 1:fdd22bb7aa52 423
emilmont 1:fdd22bb7aa52 424 /* Sample processing for K2, K3.... */
emilmont 1:fdd22bb7aa52 425 /* f2(n) = f1(n) + K2 * g1(n-1) */
emilmont 1:fdd22bb7aa52 426 fnext1 = (((q31_t) gcurnt1 * (*pk)) >> 15u) + fcurnt1;
emilmont 1:fdd22bb7aa52 427 fnext1 = __SSAT(fnext1, 16);
emilmont 1:fdd22bb7aa52 428
emilmont 1:fdd22bb7aa52 429 /* g2(n) = f1(n) * K2 + g1(n-1) */
emilmont 1:fdd22bb7aa52 430 gnext1 = (((q31_t) fcurnt1 * (*pk++)) >> 15u) + gcurnt1;
emilmont 1:fdd22bb7aa52 431 gnext1 = __SSAT(gnext1, 16);
emilmont 1:fdd22bb7aa52 432
emilmont 1:fdd22bb7aa52 433
emilmont 1:fdd22bb7aa52 434 /* f1(n) is saved in fcurnt1
emilmont 1:fdd22bb7aa52 435 for next stage processing */
emilmont 1:fdd22bb7aa52 436 fcurnt1 = fnext1;
emilmont 1:fdd22bb7aa52 437
emilmont 1:fdd22bb7aa52 438 stageCnt--;
emilmont 1:fdd22bb7aa52 439
emilmont 1:fdd22bb7aa52 440 }
emilmont 1:fdd22bb7aa52 441
emilmont 1:fdd22bb7aa52 442 /* y(n) = fN(n) */
emilmont 1:fdd22bb7aa52 443 *pDst++ = __SSAT(fcurnt1, 16);
emilmont 1:fdd22bb7aa52 444
emilmont 1:fdd22bb7aa52 445
emilmont 1:fdd22bb7aa52 446 blkCnt--;
emilmont 1:fdd22bb7aa52 447
emilmont 1:fdd22bb7aa52 448 }
emilmont 1:fdd22bb7aa52 449
emilmont 1:fdd22bb7aa52 450 #else
emilmont 1:fdd22bb7aa52 451
emilmont 1:fdd22bb7aa52 452 /* Run the below code for Cortex-M0 */
emilmont 1:fdd22bb7aa52 453
emilmont 1:fdd22bb7aa52 454 q31_t fcurnt, fnext, gcurnt, gnext; /* temporary variables */
emilmont 1:fdd22bb7aa52 455 uint32_t numStages = S->numStages; /* Length of the filter */
emilmont 1:fdd22bb7aa52 456 uint32_t blkCnt, stageCnt; /* temporary variables for counts */
emilmont 1:fdd22bb7aa52 457
emilmont 1:fdd22bb7aa52 458 pState = &S->pState[0];
emilmont 1:fdd22bb7aa52 459
emilmont 1:fdd22bb7aa52 460 blkCnt = blockSize;
emilmont 1:fdd22bb7aa52 461
emilmont 1:fdd22bb7aa52 462 while(blkCnt > 0u)
emilmont 1:fdd22bb7aa52 463 {
emilmont 1:fdd22bb7aa52 464 /* f0(n) = x(n) */
emilmont 1:fdd22bb7aa52 465 fcurnt = *pSrc++;
emilmont 1:fdd22bb7aa52 466
emilmont 1:fdd22bb7aa52 467 /* Initialize coeff pointer */
emilmont 1:fdd22bb7aa52 468 pk = (pCoeffs);
emilmont 1:fdd22bb7aa52 469
emilmont 1:fdd22bb7aa52 470 /* Initialize state pointer */
emilmont 1:fdd22bb7aa52 471 px = pState;
emilmont 1:fdd22bb7aa52 472
emilmont 1:fdd22bb7aa52 473 /* read g0(n-1) from state buffer */
emilmont 1:fdd22bb7aa52 474 gcurnt = *px;
emilmont 1:fdd22bb7aa52 475
emilmont 1:fdd22bb7aa52 476 /* for sample 1 processing */
emilmont 1:fdd22bb7aa52 477 /* f1(n) = f0(n) + K1 * g0(n-1) */
emilmont 1:fdd22bb7aa52 478 fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
emilmont 1:fdd22bb7aa52 479 fnext = __SSAT(fnext, 16);
emilmont 1:fdd22bb7aa52 480
emilmont 1:fdd22bb7aa52 481
emilmont 1:fdd22bb7aa52 482 /* g1(n) = f0(n) * K1 + g0(n-1) */
emilmont 1:fdd22bb7aa52 483 gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
emilmont 1:fdd22bb7aa52 484 gnext = __SSAT(gnext, 16);
emilmont 1:fdd22bb7aa52 485
emilmont 1:fdd22bb7aa52 486 /* save f0(n) in state buffer */
emilmont 1:fdd22bb7aa52 487 *px++ = (q15_t) fcurnt;
emilmont 1:fdd22bb7aa52 488
emilmont 1:fdd22bb7aa52 489 /* f1(n) is saved in fcurnt
emilmont 1:fdd22bb7aa52 490 for next stage processing */
emilmont 1:fdd22bb7aa52 491 fcurnt = fnext;
emilmont 1:fdd22bb7aa52 492
emilmont 1:fdd22bb7aa52 493 stageCnt = (numStages - 1u);
emilmont 1:fdd22bb7aa52 494
emilmont 1:fdd22bb7aa52 495 /* stage loop */
emilmont 1:fdd22bb7aa52 496 while(stageCnt > 0u)
emilmont 1:fdd22bb7aa52 497 {
emilmont 1:fdd22bb7aa52 498 /* read g1(n-1) from state buffer */
emilmont 1:fdd22bb7aa52 499 gcurnt = *px;
emilmont 1:fdd22bb7aa52 500
emilmont 1:fdd22bb7aa52 501 /* save g0(n-1) in state buffer */
emilmont 1:fdd22bb7aa52 502 *px++ = (q15_t) gnext;
emilmont 1:fdd22bb7aa52 503
emilmont 1:fdd22bb7aa52 504 /* Sample processing for K2, K3.... */
emilmont 1:fdd22bb7aa52 505 /* f2(n) = f1(n) + K2 * g1(n-1) */
emilmont 1:fdd22bb7aa52 506 fnext = ((gcurnt * (*pk)) >> 15u) + fcurnt;
emilmont 1:fdd22bb7aa52 507 fnext = __SSAT(fnext, 16);
emilmont 1:fdd22bb7aa52 508
emilmont 1:fdd22bb7aa52 509 /* g2(n) = f1(n) * K2 + g1(n-1) */
emilmont 1:fdd22bb7aa52 510 gnext = ((fcurnt * (*pk++)) >> 15u) + gcurnt;
emilmont 1:fdd22bb7aa52 511 gnext = __SSAT(gnext, 16);
emilmont 1:fdd22bb7aa52 512
emilmont 1:fdd22bb7aa52 513
emilmont 1:fdd22bb7aa52 514 /* f1(n) is saved in fcurnt
emilmont 1:fdd22bb7aa52 515 for next stage processing */
emilmont 1:fdd22bb7aa52 516 fcurnt = fnext;
emilmont 1:fdd22bb7aa52 517
emilmont 1:fdd22bb7aa52 518 stageCnt--;
emilmont 1:fdd22bb7aa52 519
emilmont 1:fdd22bb7aa52 520 }
emilmont 1:fdd22bb7aa52 521
emilmont 1:fdd22bb7aa52 522 /* y(n) = fN(n) */
emilmont 1:fdd22bb7aa52 523 *pDst++ = __SSAT(fcurnt, 16);
emilmont 1:fdd22bb7aa52 524
emilmont 1:fdd22bb7aa52 525
emilmont 1:fdd22bb7aa52 526 blkCnt--;
emilmont 1:fdd22bb7aa52 527
emilmont 1:fdd22bb7aa52 528 }
emilmont 1:fdd22bb7aa52 529
mbed_official 3:7a284390b0ce 530 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
emilmont 1:fdd22bb7aa52 531
emilmont 1:fdd22bb7aa52 532 }
emilmont 1:fdd22bb7aa52 533
emilmont 1:fdd22bb7aa52 534 /**
emilmont 1:fdd22bb7aa52 535 * @} end of FIR_Lattice group
emilmont 1:fdd22bb7aa52 536 */