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_biquad_cascade_stereo_df2T_f32.c@5:3762170b6d4d, 2015-11-20 (annotated)

Committer:
mbed_official
Date:
Fri Nov 20 08:45:18 2015 +0000
Revision:
5:3762170b6d4d
Synchronized with git revision 2eb940b9a73af188d3004a2575fdfbb05febe62b



Full URL: https://github.com/mbedmicro/mbed/commit/2eb940b9a73af188d3004a2575fdfbb05febe62b/



Added option to build rpc library. closes #1426

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 5:3762170b6d4d 1 /* ----------------------------------------------------------------------
mbed_official 5:3762170b6d4d 2 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.
mbed_official 5:3762170b6d4d 3 *
mbed_official 5:3762170b6d4d 4 * $Date: 19. March 2015
mbed_official 5:3762170b6d4d 5 * $Revision: V.1.4.5
mbed_official 5:3762170b6d4d 6 *
mbed_official 5:3762170b6d4d 7 * Project: CMSIS DSP Library
mbed_official 5:3762170b6d4d 8 * Title: arm_biquad_cascade_stereo_df2T_f32.c
mbed_official 5:3762170b6d4d 9 *
mbed_official 5:3762170b6d4d 10 * Description: Processing function for the floating-point transposed
mbed_official 5:3762170b6d4d 11 * direct form II Biquad cascade filter. 2 channels
mbed_official 5:3762170b6d4d 12 *
mbed_official 5:3762170b6d4d 13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
mbed_official 5:3762170b6d4d 14 *
mbed_official 5:3762170b6d4d 15 * Redistribution and use in source and binary forms, with or without
mbed_official 5:3762170b6d4d 16 * modification, are permitted provided that the following conditions
mbed_official 5:3762170b6d4d 17 * are met:
mbed_official 5:3762170b6d4d 18 * - Redistributions of source code must retain the above copyright
mbed_official 5:3762170b6d4d 19 * notice, this list of conditions and the following disclaimer.
mbed_official 5:3762170b6d4d 20 * - Redistributions in binary form must reproduce the above copyright
mbed_official 5:3762170b6d4d 21 * notice, this list of conditions and the following disclaimer in
mbed_official 5:3762170b6d4d 22 * the documentation and/or other materials provided with the
mbed_official 5:3762170b6d4d 23 * distribution.
mbed_official 5:3762170b6d4d 24 * - Neither the name of ARM LIMITED nor the names of its contributors
mbed_official 5:3762170b6d4d 25 * may be used to endorse or promote products derived from this
mbed_official 5:3762170b6d4d 26 * software without specific prior written permission.
mbed_official 5:3762170b6d4d 27 *
mbed_official 5:3762170b6d4d 28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
mbed_official 5:3762170b6d4d 29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
mbed_official 5:3762170b6d4d 30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
mbed_official 5:3762170b6d4d 31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
mbed_official 5:3762170b6d4d 32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
mbed_official 5:3762170b6d4d 33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
mbed_official 5:3762170b6d4d 34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
mbed_official 5:3762170b6d4d 35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
mbed_official 5:3762170b6d4d 36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
mbed_official 5:3762170b6d4d 37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
mbed_official 5:3762170b6d4d 38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
mbed_official 5:3762170b6d4d 39 * POSSIBILITY OF SUCH DAMAGE.
mbed_official 5:3762170b6d4d 40 * -------------------------------------------------------------------- */
mbed_official 5:3762170b6d4d 41
mbed_official 5:3762170b6d4d 42 #include "arm_math.h"
mbed_official 5:3762170b6d4d 43
mbed_official 5:3762170b6d4d 44 /**
mbed_official 5:3762170b6d4d 45 * @ingroup groupFilters
mbed_official 5:3762170b6d4d 46 */
mbed_official 5:3762170b6d4d 47
mbed_official 5:3762170b6d4d 48 /**
mbed_official 5:3762170b6d4d 49 * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure
mbed_official 5:3762170b6d4d 50 *
mbed_official 5:3762170b6d4d 51 * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.
mbed_official 5:3762170b6d4d 52 * The filters are implemented as a cascade of second order Biquad sections.
mbed_official 5:3762170b6d4d 53 * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.
mbed_official 5:3762170b6d4d 54 * Only floating-point data is supported.
mbed_official 5:3762170b6d4d 55 *
mbed_official 5:3762170b6d4d 56 * This function operate on blocks of input and output data and each call to the function
mbed_official 5:3762170b6d4d 57 * processes <code>blockSize</code> samples through the filter.
mbed_official 5:3762170b6d4d 58 * <code>pSrc</code> points to the array of input data and
mbed_official 5:3762170b6d4d 59 * <code>pDst</code> points to the array of output data.
mbed_official 5:3762170b6d4d 60 * Both arrays contain <code>blockSize</code> values.
mbed_official 5:3762170b6d4d 61 *
mbed_official 5:3762170b6d4d 62 * \par Algorithm
mbed_official 5:3762170b6d4d 63 * Each Biquad stage implements a second order filter using the difference equation:
mbed_official 5:3762170b6d4d 64 * <pre>
mbed_official 5:3762170b6d4d 65 * y[n] = b0 * x[n] + d1
mbed_official 5:3762170b6d4d 66 * d1 = b1 * x[n] + a1 * y[n] + d2
mbed_official 5:3762170b6d4d 67 * d2 = b2 * x[n] + a2 * y[n]
mbed_official 5:3762170b6d4d 68 * </pre>
mbed_official 5:3762170b6d4d 69 * where d1 and d2 represent the two state values.
mbed_official 5:3762170b6d4d 70 *
mbed_official 5:3762170b6d4d 71 * \par
mbed_official 5:3762170b6d4d 72 * A Biquad filter using a transposed Direct Form II structure is shown below.
mbed_official 5:3762170b6d4d 73 * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"
mbed_official 5:3762170b6d4d 74 * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
mbed_official 5:3762170b6d4d 75 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
mbed_official 5:3762170b6d4d 76 * Pay careful attention to the sign of the feedback coefficients.
mbed_official 5:3762170b6d4d 77 * Some design tools flip the sign of the feedback coefficients:
mbed_official 5:3762170b6d4d 78 * <pre>
mbed_official 5:3762170b6d4d 79 * y[n] = b0 * x[n] + d1;
mbed_official 5:3762170b6d4d 80 * d1 = b1 * x[n] - a1 * y[n] + d2;
mbed_official 5:3762170b6d4d 81 * d2 = b2 * x[n] - a2 * y[n];
mbed_official 5:3762170b6d4d 82 * </pre>
mbed_official 5:3762170b6d4d 83 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
mbed_official 5:3762170b6d4d 84 *
mbed_official 5:3762170b6d4d 85 * \par
mbed_official 5:3762170b6d4d 86 * Higher order filters are realized as a cascade of second order sections.
mbed_official 5:3762170b6d4d 87 * <code>numStages</code> refers to the number of second order stages used.
mbed_official 5:3762170b6d4d 88 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
mbed_official 5:3762170b6d4d 89 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the
mbed_official 5:3762170b6d4d 90 * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
mbed_official 5:3762170b6d4d 91 *
mbed_official 5:3762170b6d4d 92 * \par
mbed_official 5:3762170b6d4d 93 * <code>pState</code> points to the state variable array.
mbed_official 5:3762170b6d4d 94 * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.
mbed_official 5:3762170b6d4d 95 * The state variables are arranged in the <code>pState</code> array as:
mbed_official 5:3762170b6d4d 96 * <pre>
mbed_official 5:3762170b6d4d 97 * {d11, d12, d21, d22, ...}
mbed_official 5:3762170b6d4d 98 * </pre>
mbed_official 5:3762170b6d4d 99 * where <code>d1x</code> refers to the state variables for the first Biquad and
mbed_official 5:3762170b6d4d 100 * <code>d2x</code> refers to the state variables for the second Biquad.
mbed_official 5:3762170b6d4d 101 * The state array has a total length of <code>2*numStages</code> values.
mbed_official 5:3762170b6d4d 102 * The state variables are updated after each block of data is processed; the coefficients are untouched.
mbed_official 5:3762170b6d4d 103 *
mbed_official 5:3762170b6d4d 104 * \par
mbed_official 5:3762170b6d4d 105 * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.
mbed_official 5:3762170b6d4d 106 * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.
mbed_official 5:3762170b6d4d 107 * That is why the Direct Form I structure supports Q15 and Q31 data types.
mbed_official 5:3762170b6d4d 108 * The transposed Direct Form II structure, on the other hand, requires a wide dynamic range for the state variables <code>d1</code> and <code>d2</code>.
mbed_official 5:3762170b6d4d 109 * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.
mbed_official 5:3762170b6d4d 110 * The advantage of the Direct Form II Biquad is that it requires half the number of state variables, 2 rather than 4, per Biquad stage.
mbed_official 5:3762170b6d4d 111 *
mbed_official 5:3762170b6d4d 112 * \par Instance Structure
mbed_official 5:3762170b6d4d 113 * The coefficients and state variables for a filter are stored together in an instance data structure.
mbed_official 5:3762170b6d4d 114 * A separate instance structure must be defined for each filter.
mbed_official 5:3762170b6d4d 115 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
mbed_official 5:3762170b6d4d 116 *
mbed_official 5:3762170b6d4d 117 * \par Init Functions
mbed_official 5:3762170b6d4d 118 * There is also an associated initialization function.
mbed_official 5:3762170b6d4d 119 * The initialization function performs following operations:
mbed_official 5:3762170b6d4d 120 * - Sets the values of the internal structure fields.
mbed_official 5:3762170b6d4d 121 * - Zeros out the values in the state buffer.
mbed_official 5:3762170b6d4d 122 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
mbed_official 5:3762170b6d4d 123 * numStages, pCoeffs, pState. Also set all of the values in pState to zero.
mbed_official 5:3762170b6d4d 124 *
mbed_official 5:3762170b6d4d 125 * \par
mbed_official 5:3762170b6d4d 126 * Use of the initialization function is optional.
mbed_official 5:3762170b6d4d 127 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
mbed_official 5:3762170b6d4d 128 * To place an instance structure into a const data section, the instance structure must be manually initialized.
mbed_official 5:3762170b6d4d 129 * Set the values in the state buffer to zeros before static initialization.
mbed_official 5:3762170b6d4d 130 * For example, to statically initialize the instance structure use
mbed_official 5:3762170b6d4d 131 * <pre>
mbed_official 5:3762170b6d4d 132 * arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};
mbed_official 5:3762170b6d4d 133 * </pre>
mbed_official 5:3762170b6d4d 134 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer.
mbed_official 5:3762170b6d4d 135 * <code>pCoeffs</code> is the address of the coefficient buffer;
mbed_official 5:3762170b6d4d 136 *
mbed_official 5:3762170b6d4d 137 */
mbed_official 5:3762170b6d4d 138
mbed_official 5:3762170b6d4d 139 /**
mbed_official 5:3762170b6d4d 140 * @addtogroup BiquadCascadeDF2T
mbed_official 5:3762170b6d4d 141 * @{
mbed_official 5:3762170b6d4d 142 */
mbed_official 5:3762170b6d4d 143
mbed_official 5:3762170b6d4d 144 /**
mbed_official 5:3762170b6d4d 145 * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
mbed_official 5:3762170b6d4d 146 * @param[in] *S points to an instance of the filter data structure.
mbed_official 5:3762170b6d4d 147 * @param[in] *pSrc points to the block of input data.
mbed_official 5:3762170b6d4d 148 * @param[out] *pDst points to the block of output data
mbed_official 5:3762170b6d4d 149 * @param[in] blockSize number of samples to process.
mbed_official 5:3762170b6d4d 150 * @return none.
mbed_official 5:3762170b6d4d 151 */
mbed_official 5:3762170b6d4d 152
mbed_official 5:3762170b6d4d 153
mbed_official 5:3762170b6d4d 154 LOW_OPTIMIZATION_ENTER
mbed_official 5:3762170b6d4d 155 void arm_biquad_cascade_stereo_df2T_f32(
mbed_official 5:3762170b6d4d 156 const arm_biquad_cascade_stereo_df2T_instance_f32 * S,
mbed_official 5:3762170b6d4d 157 float32_t * pSrc,
mbed_official 5:3762170b6d4d 158 float32_t * pDst,
mbed_official 5:3762170b6d4d 159 uint32_t blockSize)
mbed_official 5:3762170b6d4d 160 {
mbed_official 5:3762170b6d4d 161
mbed_official 5:3762170b6d4d 162 float32_t *pIn = pSrc; /* source pointer */
mbed_official 5:3762170b6d4d 163 float32_t *pOut = pDst; /* destination pointer */
mbed_official 5:3762170b6d4d 164 float32_t *pState = S->pState; /* State pointer */
mbed_official 5:3762170b6d4d 165 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */
mbed_official 5:3762170b6d4d 166 float32_t acc1a, acc1b; /* accumulator */
mbed_official 5:3762170b6d4d 167 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */
mbed_official 5:3762170b6d4d 168 float32_t Xn1a, Xn1b; /* temporary input */
mbed_official 5:3762170b6d4d 169 float32_t d1a, d2a, d1b, d2b; /* state variables */
mbed_official 5:3762170b6d4d 170 uint32_t sample, stage = S->numStages; /* loop counters */
mbed_official 5:3762170b6d4d 171
mbed_official 5:3762170b6d4d 172 #if defined(ARM_MATH_CM7)
mbed_official 5:3762170b6d4d 173
mbed_official 5:3762170b6d4d 174 float32_t Xn2a, Xn3a, Xn4a, Xn5a, Xn6a, Xn7a, Xn8a; /* Input State variables */
mbed_official 5:3762170b6d4d 175 float32_t Xn2b, Xn3b, Xn4b, Xn5b, Xn6b, Xn7b, Xn8b; /* Input State variables */
mbed_official 5:3762170b6d4d 176 float32_t acc2a, acc3a, acc4a, acc5a, acc6a, acc7a, acc8a; /* Simulates the accumulator */
mbed_official 5:3762170b6d4d 177 float32_t acc2b, acc3b, acc4b, acc5b, acc6b, acc7b, acc8b; /* Simulates the accumulator */
mbed_official 5:3762170b6d4d 178
mbed_official 5:3762170b6d4d 179 do
mbed_official 5:3762170b6d4d 180 {
mbed_official 5:3762170b6d4d 181 /* Reading the coefficients */
mbed_official 5:3762170b6d4d 182 b0 = pCoeffs[0];
mbed_official 5:3762170b6d4d 183 b1 = pCoeffs[1];
mbed_official 5:3762170b6d4d 184 b2 = pCoeffs[2];
mbed_official 5:3762170b6d4d 185 a1 = pCoeffs[3];
mbed_official 5:3762170b6d4d 186 /* Apply loop unrolling and compute 8 output values simultaneously. */
mbed_official 5:3762170b6d4d 187 sample = blockSize >> 3u;
mbed_official 5:3762170b6d4d 188 a2 = pCoeffs[4];
mbed_official 5:3762170b6d4d 189
mbed_official 5:3762170b6d4d 190 /*Reading the state values */
mbed_official 5:3762170b6d4d 191 d1a = pState[0];
mbed_official 5:3762170b6d4d 192 d2a = pState[1];
mbed_official 5:3762170b6d4d 193 d1b = pState[2];
mbed_official 5:3762170b6d4d 194 d2b = pState[3];
mbed_official 5:3762170b6d4d 195
mbed_official 5:3762170b6d4d 196 pCoeffs += 5u;
mbed_official 5:3762170b6d4d 197
mbed_official 5:3762170b6d4d 198 /* First part of the processing with loop unrolling. Compute 8 outputs at a time.
mbed_official 5:3762170b6d4d 199 ** a second loop below computes the remaining 1 to 7 samples. */
mbed_official 5:3762170b6d4d 200 while(sample > 0u) {
mbed_official 5:3762170b6d4d 201
mbed_official 5:3762170b6d4d 202 /* y[n] = b0 * x[n] + d1 */
mbed_official 5:3762170b6d4d 203 /* d1 = b1 * x[n] + a1 * y[n] + d2 */
mbed_official 5:3762170b6d4d 204 /* d2 = b2 * x[n] + a2 * y[n] */
mbed_official 5:3762170b6d4d 205
mbed_official 5:3762170b6d4d 206 /* Read the first 2 inputs. 2 cycles */
mbed_official 5:3762170b6d4d 207 Xn1a = pIn[0 ];
mbed_official 5:3762170b6d4d 208 Xn1b = pIn[1 ];
mbed_official 5:3762170b6d4d 209
mbed_official 5:3762170b6d4d 210 /* Sample 1. 5 cycles */
mbed_official 5:3762170b6d4d 211 Xn2a = pIn[2 ];
mbed_official 5:3762170b6d4d 212 acc1a = b0 * Xn1a + d1a;
mbed_official 5:3762170b6d4d 213
mbed_official 5:3762170b6d4d 214 Xn2b = pIn[3 ];
mbed_official 5:3762170b6d4d 215 d1a = b1 * Xn1a + d2a;
mbed_official 5:3762170b6d4d 216
mbed_official 5:3762170b6d4d 217 Xn3a = pIn[4 ];
mbed_official 5:3762170b6d4d 218 d2a = b2 * Xn1a;
mbed_official 5:3762170b6d4d 219
mbed_official 5:3762170b6d4d 220 Xn3b = pIn[5 ];
mbed_official 5:3762170b6d4d 221 d1a += a1 * acc1a;
mbed_official 5:3762170b6d4d 222
mbed_official 5:3762170b6d4d 223 Xn4a = pIn[6 ];
mbed_official 5:3762170b6d4d 224 d2a += a2 * acc1a;
mbed_official 5:3762170b6d4d 225
mbed_official 5:3762170b6d4d 226 /* Sample 2. 5 cycles */
mbed_official 5:3762170b6d4d 227 Xn4b = pIn[7 ];
mbed_official 5:3762170b6d4d 228 acc1b = b0 * Xn1b + d1b;
mbed_official 5:3762170b6d4d 229
mbed_official 5:3762170b6d4d 230 Xn5a = pIn[8 ];
mbed_official 5:3762170b6d4d 231 d1b = b1 * Xn1b + d2b;
mbed_official 5:3762170b6d4d 232
mbed_official 5:3762170b6d4d 233 Xn5b = pIn[9 ];
mbed_official 5:3762170b6d4d 234 d2b = b2 * Xn1b;
mbed_official 5:3762170b6d4d 235
mbed_official 5:3762170b6d4d 236 Xn6a = pIn[10];
mbed_official 5:3762170b6d4d 237 d1b += a1 * acc1b;
mbed_official 5:3762170b6d4d 238
mbed_official 5:3762170b6d4d 239 Xn6b = pIn[11];
mbed_official 5:3762170b6d4d 240 d2b += a2 * acc1b;
mbed_official 5:3762170b6d4d 241
mbed_official 5:3762170b6d4d 242 /* Sample 3. 5 cycles */
mbed_official 5:3762170b6d4d 243 Xn7a = pIn[12];
mbed_official 5:3762170b6d4d 244 acc2a = b0 * Xn2a + d1a;
mbed_official 5:3762170b6d4d 245
mbed_official 5:3762170b6d4d 246 Xn7b = pIn[13];
mbed_official 5:3762170b6d4d 247 d1a = b1 * Xn2a + d2a;
mbed_official 5:3762170b6d4d 248
mbed_official 5:3762170b6d4d 249 Xn8a = pIn[14];
mbed_official 5:3762170b6d4d 250 d2a = b2 * Xn2a;
mbed_official 5:3762170b6d4d 251
mbed_official 5:3762170b6d4d 252 Xn8b = pIn[15];
mbed_official 5:3762170b6d4d 253 d1a += a1 * acc2a;
mbed_official 5:3762170b6d4d 254
mbed_official 5:3762170b6d4d 255 pIn += 16;
mbed_official 5:3762170b6d4d 256 d2a += a2 * acc2a;
mbed_official 5:3762170b6d4d 257
mbed_official 5:3762170b6d4d 258 /* Sample 4. 5 cycles */
mbed_official 5:3762170b6d4d 259 acc2b = b0 * Xn2b + d1b;
mbed_official 5:3762170b6d4d 260 d1b = b1 * Xn2b + d2b;
mbed_official 5:3762170b6d4d 261 d2b = b2 * Xn2b;
mbed_official 5:3762170b6d4d 262 d1b += a1 * acc2b;
mbed_official 5:3762170b6d4d 263 d2b += a2 * acc2b;
mbed_official 5:3762170b6d4d 264
mbed_official 5:3762170b6d4d 265 /* Sample 5. 5 cycles */
mbed_official 5:3762170b6d4d 266 acc3a = b0 * Xn3a + d1a;
mbed_official 5:3762170b6d4d 267 d1a = b1 * Xn3a + d2a;
mbed_official 5:3762170b6d4d 268 d2a = b2 * Xn3a;
mbed_official 5:3762170b6d4d 269 d1a += a1 * acc3a;
mbed_official 5:3762170b6d4d 270 d2a += a2 * acc3a;
mbed_official 5:3762170b6d4d 271
mbed_official 5:3762170b6d4d 272 /* Sample 6. 5 cycles */
mbed_official 5:3762170b6d4d 273 acc3b = b0 * Xn3b + d1b;
mbed_official 5:3762170b6d4d 274 d1b = b1 * Xn3b + d2b;
mbed_official 5:3762170b6d4d 275 d2b = b2 * Xn3b;
mbed_official 5:3762170b6d4d 276 d1b += a1 * acc3b;
mbed_official 5:3762170b6d4d 277 d2b += a2 * acc3b;
mbed_official 5:3762170b6d4d 278
mbed_official 5:3762170b6d4d 279 /* Sample 7. 5 cycles */
mbed_official 5:3762170b6d4d 280 acc4a = b0 * Xn4a + d1a;
mbed_official 5:3762170b6d4d 281 d1a = b1 * Xn4a + d2a;
mbed_official 5:3762170b6d4d 282 d2a = b2 * Xn4a;
mbed_official 5:3762170b6d4d 283 d1a += a1 * acc4a;
mbed_official 5:3762170b6d4d 284 d2a += a2 * acc4a;
mbed_official 5:3762170b6d4d 285
mbed_official 5:3762170b6d4d 286 /* Sample 8. 5 cycles */
mbed_official 5:3762170b6d4d 287 acc4b = b0 * Xn4b + d1b;
mbed_official 5:3762170b6d4d 288 d1b = b1 * Xn4b + d2b;
mbed_official 5:3762170b6d4d 289 d2b = b2 * Xn4b;
mbed_official 5:3762170b6d4d 290 d1b += a1 * acc4b;
mbed_official 5:3762170b6d4d 291 d2b += a2 * acc4b;
mbed_official 5:3762170b6d4d 292
mbed_official 5:3762170b6d4d 293 /* Sample 9. 5 cycles */
mbed_official 5:3762170b6d4d 294 acc5a = b0 * Xn5a + d1a;
mbed_official 5:3762170b6d4d 295 d1a = b1 * Xn5a + d2a;
mbed_official 5:3762170b6d4d 296 d2a = b2 * Xn5a;
mbed_official 5:3762170b6d4d 297 d1a += a1 * acc5a;
mbed_official 5:3762170b6d4d 298 d2a += a2 * acc5a;
mbed_official 5:3762170b6d4d 299
mbed_official 5:3762170b6d4d 300 /* Sample 10. 5 cycles */
mbed_official 5:3762170b6d4d 301 acc5b = b0 * Xn5b + d1b;
mbed_official 5:3762170b6d4d 302 d1b = b1 * Xn5b + d2b;
mbed_official 5:3762170b6d4d 303 d2b = b2 * Xn5b;
mbed_official 5:3762170b6d4d 304 d1b += a1 * acc5b;
mbed_official 5:3762170b6d4d 305 d2b += a2 * acc5b;
mbed_official 5:3762170b6d4d 306
mbed_official 5:3762170b6d4d 307 /* Sample 11. 5 cycles */
mbed_official 5:3762170b6d4d 308 acc6a = b0 * Xn6a + d1a;
mbed_official 5:3762170b6d4d 309 d1a = b1 * Xn6a + d2a;
mbed_official 5:3762170b6d4d 310 d2a = b2 * Xn6a;
mbed_official 5:3762170b6d4d 311 d1a += a1 * acc6a;
mbed_official 5:3762170b6d4d 312 d2a += a2 * acc6a;
mbed_official 5:3762170b6d4d 313
mbed_official 5:3762170b6d4d 314 /* Sample 12. 5 cycles */
mbed_official 5:3762170b6d4d 315 acc6b = b0 * Xn6b + d1b;
mbed_official 5:3762170b6d4d 316 d1b = b1 * Xn6b + d2b;
mbed_official 5:3762170b6d4d 317 d2b = b2 * Xn6b;
mbed_official 5:3762170b6d4d 318 d1b += a1 * acc6b;
mbed_official 5:3762170b6d4d 319 d2b += a2 * acc6b;
mbed_official 5:3762170b6d4d 320
mbed_official 5:3762170b6d4d 321 /* Sample 13. 5 cycles */
mbed_official 5:3762170b6d4d 322 acc7a = b0 * Xn7a + d1a;
mbed_official 5:3762170b6d4d 323 d1a = b1 * Xn7a + d2a;
mbed_official 5:3762170b6d4d 324
mbed_official 5:3762170b6d4d 325 pOut[0 ] = acc1a ;
mbed_official 5:3762170b6d4d 326 d2a = b2 * Xn7a;
mbed_official 5:3762170b6d4d 327
mbed_official 5:3762170b6d4d 328 pOut[1 ] = acc1b ;
mbed_official 5:3762170b6d4d 329 d1a += a1 * acc7a;
mbed_official 5:3762170b6d4d 330
mbed_official 5:3762170b6d4d 331 pOut[2 ] = acc2a ;
mbed_official 5:3762170b6d4d 332 d2a += a2 * acc7a;
mbed_official 5:3762170b6d4d 333
mbed_official 5:3762170b6d4d 334 /* Sample 14. 5 cycles */
mbed_official 5:3762170b6d4d 335 pOut[3 ] = acc2b ;
mbed_official 5:3762170b6d4d 336 acc7b = b0 * Xn7b + d1b;
mbed_official 5:3762170b6d4d 337
mbed_official 5:3762170b6d4d 338 pOut[4 ] = acc3a ;
mbed_official 5:3762170b6d4d 339 d1b = b1 * Xn7b + d2b;
mbed_official 5:3762170b6d4d 340
mbed_official 5:3762170b6d4d 341 pOut[5 ] = acc3b ;
mbed_official 5:3762170b6d4d 342 d2b = b2 * Xn7b;
mbed_official 5:3762170b6d4d 343
mbed_official 5:3762170b6d4d 344 pOut[6 ] = acc4a ;
mbed_official 5:3762170b6d4d 345 d1b += a1 * acc7b;
mbed_official 5:3762170b6d4d 346
mbed_official 5:3762170b6d4d 347 pOut[7 ] = acc4b ;
mbed_official 5:3762170b6d4d 348 d2b += a2 * acc7b;
mbed_official 5:3762170b6d4d 349
mbed_official 5:3762170b6d4d 350 /* Sample 15. 5 cycles */
mbed_official 5:3762170b6d4d 351 pOut[8 ] = acc5a ;
mbed_official 5:3762170b6d4d 352 acc8a = b0 * Xn8a + d1a;
mbed_official 5:3762170b6d4d 353
mbed_official 5:3762170b6d4d 354 pOut[9 ] = acc5b;
mbed_official 5:3762170b6d4d 355 d1a = b1 * Xn8a + d2a;
mbed_official 5:3762170b6d4d 356
mbed_official 5:3762170b6d4d 357 pOut[10] = acc6a;
mbed_official 5:3762170b6d4d 358 d2a = b2 * Xn8a;
mbed_official 5:3762170b6d4d 359
mbed_official 5:3762170b6d4d 360 pOut[11] = acc6b;
mbed_official 5:3762170b6d4d 361 d1a += a1 * acc8a;
mbed_official 5:3762170b6d4d 362
mbed_official 5:3762170b6d4d 363 pOut[12] = acc7a;
mbed_official 5:3762170b6d4d 364 d2a += a2 * acc8a;
mbed_official 5:3762170b6d4d 365
mbed_official 5:3762170b6d4d 366 /* Sample 16. 5 cycles */
mbed_official 5:3762170b6d4d 367 pOut[13] = acc7b;
mbed_official 5:3762170b6d4d 368 acc8b = b0 * Xn8b + d1b;
mbed_official 5:3762170b6d4d 369
mbed_official 5:3762170b6d4d 370 pOut[14] = acc8a;
mbed_official 5:3762170b6d4d 371 d1b = b1 * Xn8b + d2b;
mbed_official 5:3762170b6d4d 372
mbed_official 5:3762170b6d4d 373 pOut[15] = acc8b;
mbed_official 5:3762170b6d4d 374 d2b = b2 * Xn8b;
mbed_official 5:3762170b6d4d 375
mbed_official 5:3762170b6d4d 376 sample--;
mbed_official 5:3762170b6d4d 377 d1b += a1 * acc8b;
mbed_official 5:3762170b6d4d 378
mbed_official 5:3762170b6d4d 379 pOut += 16;
mbed_official 5:3762170b6d4d 380 d2b += a2 * acc8b;
mbed_official 5:3762170b6d4d 381 }
mbed_official 5:3762170b6d4d 382
mbed_official 5:3762170b6d4d 383 sample = blockSize & 0x7u;
mbed_official 5:3762170b6d4d 384 while(sample > 0u) {
mbed_official 5:3762170b6d4d 385 /* Read the input */
mbed_official 5:3762170b6d4d 386 Xn1a = *pIn++; //Channel a
mbed_official 5:3762170b6d4d 387 Xn1b = *pIn++; //Channel b
mbed_official 5:3762170b6d4d 388
mbed_official 5:3762170b6d4d 389 /* y[n] = b0 * x[n] + d1 */
mbed_official 5:3762170b6d4d 390 acc1a = (b0 * Xn1a) + d1a;
mbed_official 5:3762170b6d4d 391 acc1b = (b0 * Xn1b) + d1b;
mbed_official 5:3762170b6d4d 392
mbed_official 5:3762170b6d4d 393 /* Store the result in the accumulator in the destination buffer. */
mbed_official 5:3762170b6d4d 394 *pOut++ = acc1a;
mbed_official 5:3762170b6d4d 395 *pOut++ = acc1b;
mbed_official 5:3762170b6d4d 396
mbed_official 5:3762170b6d4d 397 /* Every time after the output is computed state should be updated. */
mbed_official 5:3762170b6d4d 398 /* d1 = b1 * x[n] + a1 * y[n] + d2 */
mbed_official 5:3762170b6d4d 399 d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
mbed_official 5:3762170b6d4d 400 d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
mbed_official 5:3762170b6d4d 401
mbed_official 5:3762170b6d4d 402 /* d2 = b2 * x[n] + a2 * y[n] */
mbed_official 5:3762170b6d4d 403 d2a = (b2 * Xn1a) + (a2 * acc1a);
mbed_official 5:3762170b6d4d 404 d2b = (b2 * Xn1b) + (a2 * acc1b);
mbed_official 5:3762170b6d4d 405
mbed_official 5:3762170b6d4d 406 sample--;
mbed_official 5:3762170b6d4d 407 }
mbed_official 5:3762170b6d4d 408
mbed_official 5:3762170b6d4d 409 /* Store the updated state variables back into the state array */
mbed_official 5:3762170b6d4d 410 pState[0] = d1a;
mbed_official 5:3762170b6d4d 411 pState[1] = d2a;
mbed_official 5:3762170b6d4d 412
mbed_official 5:3762170b6d4d 413 pState[2] = d1b;
mbed_official 5:3762170b6d4d 414 pState[3] = d2b;
mbed_official 5:3762170b6d4d 415
mbed_official 5:3762170b6d4d 416 /* The current stage input is given as the output to the next stage */
mbed_official 5:3762170b6d4d 417 pIn = pDst;
mbed_official 5:3762170b6d4d 418 /* decrement the loop counter */
mbed_official 5:3762170b6d4d 419 stage--;
mbed_official 5:3762170b6d4d 420
mbed_official 5:3762170b6d4d 421 pState += 4u;
mbed_official 5:3762170b6d4d 422 /*Reset the output working pointer */
mbed_official 5:3762170b6d4d 423 pOut = pDst;
mbed_official 5:3762170b6d4d 424
mbed_official 5:3762170b6d4d 425 } while(stage > 0u);
mbed_official 5:3762170b6d4d 426
mbed_official 5:3762170b6d4d 427 #elif defined(ARM_MATH_CM0_FAMILY)
mbed_official 5:3762170b6d4d 428
mbed_official 5:3762170b6d4d 429 /* Run the below code for Cortex-M0 */
mbed_official 5:3762170b6d4d 430
mbed_official 5:3762170b6d4d 431 do
mbed_official 5:3762170b6d4d 432 {
mbed_official 5:3762170b6d4d 433 /* Reading the coefficients */
mbed_official 5:3762170b6d4d 434 b0 = *pCoeffs++;
mbed_official 5:3762170b6d4d 435 b1 = *pCoeffs++;
mbed_official 5:3762170b6d4d 436 b2 = *pCoeffs++;
mbed_official 5:3762170b6d4d 437 a1 = *pCoeffs++;
mbed_official 5:3762170b6d4d 438 a2 = *pCoeffs++;
mbed_official 5:3762170b6d4d 439
mbed_official 5:3762170b6d4d 440 /*Reading the state values */
mbed_official 5:3762170b6d4d 441 d1a = pState[0];
mbed_official 5:3762170b6d4d 442 d2a = pState[1];
mbed_official 5:3762170b6d4d 443 d1b = pState[2];
mbed_official 5:3762170b6d4d 444 d2b = pState[3];
mbed_official 5:3762170b6d4d 445
mbed_official 5:3762170b6d4d 446
mbed_official 5:3762170b6d4d 447 sample = blockSize;
mbed_official 5:3762170b6d4d 448
mbed_official 5:3762170b6d4d 449 while(sample > 0u)
mbed_official 5:3762170b6d4d 450 {
mbed_official 5:3762170b6d4d 451 /* Read the input */
mbed_official 5:3762170b6d4d 452 Xn1a = *pIn++; //Channel a
mbed_official 5:3762170b6d4d 453 Xn1b = *pIn++; //Channel b
mbed_official 5:3762170b6d4d 454
mbed_official 5:3762170b6d4d 455 /* y[n] = b0 * x[n] + d1 */
mbed_official 5:3762170b6d4d 456 acc1a = (b0 * Xn1a) + d1a;
mbed_official 5:3762170b6d4d 457 acc1b = (b0 * Xn1b) + d1b;
mbed_official 5:3762170b6d4d 458
mbed_official 5:3762170b6d4d 459 /* Store the result in the accumulator in the destination buffer. */
mbed_official 5:3762170b6d4d 460 *pOut++ = acc1a;
mbed_official 5:3762170b6d4d 461 *pOut++ = acc1b;
mbed_official 5:3762170b6d4d 462
mbed_official 5:3762170b6d4d 463 /* Every time after the output is computed state should be updated. */
mbed_official 5:3762170b6d4d 464 /* d1 = b1 * x[n] + a1 * y[n] + d2 */
mbed_official 5:3762170b6d4d 465 d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a;
mbed_official 5:3762170b6d4d 466 d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b;
mbed_official 5:3762170b6d4d 467
mbed_official 5:3762170b6d4d 468 /* d2 = b2 * x[n] + a2 * y[n] */
mbed_official 5:3762170b6d4d 469 d2a = (b2 * Xn1a) + (a2 * acc1a);
mbed_official 5:3762170b6d4d 470 d2b = (b2 * Xn1b) + (a2 * acc1b);
mbed_official 5:3762170b6d4d 471
mbed_official 5:3762170b6d4d 472 /* decrement the loop counter */
mbed_official 5:3762170b6d4d 473 sample--;
mbed_official 5:3762170b6d4d 474 }
mbed_official 5:3762170b6d4d 475
mbed_official 5:3762170b6d4d 476 /* Store the updated state variables back into the state array */
mbed_official 5:3762170b6d4d 477 *pState++ = d1a;
mbed_official 5:3762170b6d4d 478 *pState++ = d2a;
mbed_official 5:3762170b6d4d 479 *pState++ = d1b;
mbed_official 5:3762170b6d4d 480 *pState++ = d2b;
mbed_official 5:3762170b6d4d 481
mbed_official 5:3762170b6d4d 482 /* The current stage input is given as the output to the next stage */
mbed_official 5:3762170b6d4d 483 pIn = pDst;
mbed_official 5:3762170b6d4d 484
mbed_official 5:3762170b6d4d 485 /*Reset the output working pointer */
mbed_official 5:3762170b6d4d 486 pOut = pDst;
mbed_official 5:3762170b6d4d 487
mbed_official 5:3762170b6d4d 488 /* decrement the loop counter */
mbed_official 5:3762170b6d4d 489 stage--;
mbed_official 5:3762170b6d4d 490
mbed_official 5:3762170b6d4d 491 } while(stage > 0u);
mbed_official 5:3762170b6d4d 492
mbed_official 5:3762170b6d4d 493 #else
mbed_official 5:3762170b6d4d 494
mbed_official 5:3762170b6d4d 495 float32_t Xn2a, Xn3a, Xn4a; /* Input State variables */
mbed_official 5:3762170b6d4d 496 float32_t Xn2b, Xn3b, Xn4b; /* Input State variables */
mbed_official 5:3762170b6d4d 497 float32_t acc2a, acc3a, acc4a; /* accumulator */
mbed_official 5:3762170b6d4d 498 float32_t acc2b, acc3b, acc4b; /* accumulator */
mbed_official 5:3762170b6d4d 499 float32_t p0a, p1a, p2a, p3a, p4a, A1a;
mbed_official 5:3762170b6d4d 500 float32_t p0b, p1b, p2b, p3b, p4b, A1b;
mbed_official 5:3762170b6d4d 501
mbed_official 5:3762170b6d4d 502 /* Run the below code for Cortex-M4 and Cortex-M3 */
mbed_official 5:3762170b6d4d 503 do
mbed_official 5:3762170b6d4d 504 {
mbed_official 5:3762170b6d4d 505 /* Reading the coefficients */
mbed_official 5:3762170b6d4d 506 b0 = *pCoeffs++;
mbed_official 5:3762170b6d4d 507 b1 = *pCoeffs++;
mbed_official 5:3762170b6d4d 508 b2 = *pCoeffs++;
mbed_official 5:3762170b6d4d 509 a1 = *pCoeffs++;
mbed_official 5:3762170b6d4d 510 a2 = *pCoeffs++;
mbed_official 5:3762170b6d4d 511
mbed_official 5:3762170b6d4d 512 /*Reading the state values */
mbed_official 5:3762170b6d4d 513 d1a = pState[0];
mbed_official 5:3762170b6d4d 514 d2a = pState[1];
mbed_official 5:3762170b6d4d 515 d1b = pState[2];
mbed_official 5:3762170b6d4d 516 d2b = pState[3];
mbed_official 5:3762170b6d4d 517
mbed_official 5:3762170b6d4d 518 /* Apply loop unrolling and compute 4 output values simultaneously. */
mbed_official 5:3762170b6d4d 519 sample = blockSize >> 2u;
mbed_official 5:3762170b6d4d 520
mbed_official 5:3762170b6d4d 521 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
mbed_official 5:3762170b6d4d 522 ** a second loop below computes the remaining 1 to 3 samples. */
mbed_official 5:3762170b6d4d 523 while(sample > 0u) {
mbed_official 5:3762170b6d4d 524
mbed_official 5:3762170b6d4d 525 /* y[n] = b0 * x[n] + d1 */
mbed_official 5:3762170b6d4d 526 /* d1 = b1 * x[n] + a1 * y[n] + d2 */
mbed_official 5:3762170b6d4d 527 /* d2 = b2 * x[n] + a2 * y[n] */
mbed_official 5:3762170b6d4d 528
mbed_official 5:3762170b6d4d 529 /* Read the four inputs */
mbed_official 5:3762170b6d4d 530 Xn1a = pIn[0];
mbed_official 5:3762170b6d4d 531 Xn1b = pIn[1];
mbed_official 5:3762170b6d4d 532 Xn2a = pIn[2];
mbed_official 5:3762170b6d4d 533 Xn2b = pIn[3];
mbed_official 5:3762170b6d4d 534 Xn3a = pIn[4];
mbed_official 5:3762170b6d4d 535 Xn3b = pIn[5];
mbed_official 5:3762170b6d4d 536 Xn4a = pIn[6];
mbed_official 5:3762170b6d4d 537 Xn4b = pIn[7];
mbed_official 5:3762170b6d4d 538 pIn += 8;
mbed_official 5:3762170b6d4d 539
mbed_official 5:3762170b6d4d 540 p0a = b0 * Xn1a;
mbed_official 5:3762170b6d4d 541 p0b = b0 * Xn1b;
mbed_official 5:3762170b6d4d 542 p1a = b1 * Xn1a;
mbed_official 5:3762170b6d4d 543 p1b = b1 * Xn1b;
mbed_official 5:3762170b6d4d 544 acc1a = p0a + d1a;
mbed_official 5:3762170b6d4d 545 acc1b = p0b + d1b;
mbed_official 5:3762170b6d4d 546 p0a = b0 * Xn2a;
mbed_official 5:3762170b6d4d 547 p0b = b0 * Xn2b;
mbed_official 5:3762170b6d4d 548 p3a = a1 * acc1a;
mbed_official 5:3762170b6d4d 549 p3b = a1 * acc1b;
mbed_official 5:3762170b6d4d 550 p2a = b2 * Xn1a;
mbed_official 5:3762170b6d4d 551 p2b = b2 * Xn1b;
mbed_official 5:3762170b6d4d 552 A1a = p1a + p3a;
mbed_official 5:3762170b6d4d 553 A1b = p1b + p3b;
mbed_official 5:3762170b6d4d 554 p4a = a2 * acc1a;
mbed_official 5:3762170b6d4d 555 p4b = a2 * acc1b;
mbed_official 5:3762170b6d4d 556 d1a = A1a + d2a;
mbed_official 5:3762170b6d4d 557 d1b = A1b + d2b;
mbed_official 5:3762170b6d4d 558 d2a = p2a + p4a;
mbed_official 5:3762170b6d4d 559 d2b = p2b + p4b;
mbed_official 5:3762170b6d4d 560
mbed_official 5:3762170b6d4d 561 p1a = b1 * Xn2a;
mbed_official 5:3762170b6d4d 562 p1b = b1 * Xn2b;
mbed_official 5:3762170b6d4d 563 acc2a = p0a + d1a;
mbed_official 5:3762170b6d4d 564 acc2b = p0b + d1b;
mbed_official 5:3762170b6d4d 565 p0a = b0 * Xn3a;
mbed_official 5:3762170b6d4d 566 p0b = b0 * Xn3b;
mbed_official 5:3762170b6d4d 567 p3a = a1 * acc2a;
mbed_official 5:3762170b6d4d 568 p3b = a1 * acc2b;
mbed_official 5:3762170b6d4d 569 p2a = b2 * Xn2a;
mbed_official 5:3762170b6d4d 570 p2b = b2 * Xn2b;
mbed_official 5:3762170b6d4d 571 A1a = p1a + p3a;
mbed_official 5:3762170b6d4d 572 A1b = p1b + p3b;
mbed_official 5:3762170b6d4d 573 p4a = a2 * acc2a;
mbed_official 5:3762170b6d4d 574 p4b = a2 * acc2b;
mbed_official 5:3762170b6d4d 575 d1a = A1a + d2a;
mbed_official 5:3762170b6d4d 576 d1b = A1b + d2b;
mbed_official 5:3762170b6d4d 577 d2a = p2a + p4a;
mbed_official 5:3762170b6d4d 578 d2b = p2b + p4b;
mbed_official 5:3762170b6d4d 579
mbed_official 5:3762170b6d4d 580 p1a = b1 * Xn3a;
mbed_official 5:3762170b6d4d 581 p1b = b1 * Xn3b;
mbed_official 5:3762170b6d4d 582 acc3a = p0a + d1a;
mbed_official 5:3762170b6d4d 583 acc3b = p0b + d1b;
mbed_official 5:3762170b6d4d 584 p0a = b0 * Xn4a;
mbed_official 5:3762170b6d4d 585 p0b = b0 * Xn4b;
mbed_official 5:3762170b6d4d 586 p3a = a1 * acc3a;
mbed_official 5:3762170b6d4d 587 p3b = a1 * acc3b;
mbed_official 5:3762170b6d4d 588 p2a = b2 * Xn3a;
mbed_official 5:3762170b6d4d 589 p2b = b2 * Xn3b;
mbed_official 5:3762170b6d4d 590 A1a = p1a + p3a;
mbed_official 5:3762170b6d4d 591 A1b = p1b + p3b;
mbed_official 5:3762170b6d4d 592 p4a = a2 * acc3a;
mbed_official 5:3762170b6d4d 593 p4b = a2 * acc3b;
mbed_official 5:3762170b6d4d 594 d1a = A1a + d2a;
mbed_official 5:3762170b6d4d 595 d1b = A1b + d2b;
mbed_official 5:3762170b6d4d 596 d2a = p2a + p4a;
mbed_official 5:3762170b6d4d 597 d2b = p2b + p4b;
mbed_official 5:3762170b6d4d 598
mbed_official 5:3762170b6d4d 599 acc4a = p0a + d1a;
mbed_official 5:3762170b6d4d 600 acc4b = p0b + d1b;
mbed_official 5:3762170b6d4d 601 p1a = b1 * Xn4a;
mbed_official 5:3762170b6d4d 602 p1b = b1 * Xn4b;
mbed_official 5:3762170b6d4d 603 p3a = a1 * acc4a;
mbed_official 5:3762170b6d4d 604 p3b = a1 * acc4b;
mbed_official 5:3762170b6d4d 605 p2a = b2 * Xn4a;
mbed_official 5:3762170b6d4d 606 p2b = b2 * Xn4b;
mbed_official 5:3762170b6d4d 607 A1a = p1a + p3a;
mbed_official 5:3762170b6d4d 608 A1b = p1b + p3b;
mbed_official 5:3762170b6d4d 609 p4a = a2 * acc4a;
mbed_official 5:3762170b6d4d 610 p4b = a2 * acc4b;
mbed_official 5:3762170b6d4d 611 d1a = A1a + d2a;
mbed_official 5:3762170b6d4d 612 d1b = A1b + d2b;
mbed_official 5:3762170b6d4d 613 d2a = p2a + p4a;
mbed_official 5:3762170b6d4d 614 d2b = p2b + p4b;
mbed_official 5:3762170b6d4d 615
mbed_official 5:3762170b6d4d 616 pOut[0] = acc1a;
mbed_official 5:3762170b6d4d 617 pOut[1] = acc1b;
mbed_official 5:3762170b6d4d 618 pOut[2] = acc2a;
mbed_official 5:3762170b6d4d 619 pOut[3] = acc2b;
mbed_official 5:3762170b6d4d 620 pOut[4] = acc3a;
mbed_official 5:3762170b6d4d 621 pOut[5] = acc3b;
mbed_official 5:3762170b6d4d 622 pOut[6] = acc4a;
mbed_official 5:3762170b6d4d 623 pOut[7] = acc4b;
mbed_official 5:3762170b6d4d 624 pOut += 8;
mbed_official 5:3762170b6d4d 625
mbed_official 5:3762170b6d4d 626 sample--;
mbed_official 5:3762170b6d4d 627 }
mbed_official 5:3762170b6d4d 628
mbed_official 5:3762170b6d4d 629 sample = blockSize & 0x3u;
mbed_official 5:3762170b6d4d 630 while(sample > 0u) {
mbed_official 5:3762170b6d4d 631 Xn1a = *pIn++;
mbed_official 5:3762170b6d4d 632 Xn1b = *pIn++;
mbed_official 5:3762170b6d4d 633
mbed_official 5:3762170b6d4d 634 p0a = b0 * Xn1a;
mbed_official 5:3762170b6d4d 635 p0b = b0 * Xn1b;
mbed_official 5:3762170b6d4d 636 p1a = b1 * Xn1a;
mbed_official 5:3762170b6d4d 637 p1b = b1 * Xn1b;
mbed_official 5:3762170b6d4d 638 acc1a = p0a + d1a;
mbed_official 5:3762170b6d4d 639 acc1b = p0b + d1b;
mbed_official 5:3762170b6d4d 640 p3a = a1 * acc1a;
mbed_official 5:3762170b6d4d 641 p3b = a1 * acc1b;
mbed_official 5:3762170b6d4d 642 p2a = b2 * Xn1a;
mbed_official 5:3762170b6d4d 643 p2b = b2 * Xn1b;
mbed_official 5:3762170b6d4d 644 A1a = p1a + p3a;
mbed_official 5:3762170b6d4d 645 A1b = p1b + p3b;
mbed_official 5:3762170b6d4d 646 p4a = a2 * acc1a;
mbed_official 5:3762170b6d4d 647 p4b = a2 * acc1b;
mbed_official 5:3762170b6d4d 648 d1a = A1a + d2a;
mbed_official 5:3762170b6d4d 649 d1b = A1b + d2b;
mbed_official 5:3762170b6d4d 650 d2a = p2a + p4a;
mbed_official 5:3762170b6d4d 651 d2b = p2b + p4b;
mbed_official 5:3762170b6d4d 652
mbed_official 5:3762170b6d4d 653 *pOut++ = acc1a;
mbed_official 5:3762170b6d4d 654 *pOut++ = acc1b;
mbed_official 5:3762170b6d4d 655
mbed_official 5:3762170b6d4d 656 sample--;
mbed_official 5:3762170b6d4d 657 }
mbed_official 5:3762170b6d4d 658
mbed_official 5:3762170b6d4d 659 /* Store the updated state variables back into the state array */
mbed_official 5:3762170b6d4d 660 *pState++ = d1a;
mbed_official 5:3762170b6d4d 661 *pState++ = d2a;
mbed_official 5:3762170b6d4d 662 *pState++ = d1b;
mbed_official 5:3762170b6d4d 663 *pState++ = d2b;
mbed_official 5:3762170b6d4d 664
mbed_official 5:3762170b6d4d 665 /* The current stage input is given as the output to the next stage */
mbed_official 5:3762170b6d4d 666 pIn = pDst;
mbed_official 5:3762170b6d4d 667
mbed_official 5:3762170b6d4d 668 /*Reset the output working pointer */
mbed_official 5:3762170b6d4d 669 pOut = pDst;
mbed_official 5:3762170b6d4d 670
mbed_official 5:3762170b6d4d 671 /* decrement the loop counter */
mbed_official 5:3762170b6d4d 672 stage--;
mbed_official 5:3762170b6d4d 673
mbed_official 5:3762170b6d4d 674 } while(stage > 0u);
mbed_official 5:3762170b6d4d 675
mbed_official 5:3762170b6d4d 676 #endif
mbed_official 5:3762170b6d4d 677
mbed_official 5:3762170b6d4d 678 }
mbed_official 5:3762170b6d4d 679 LOW_OPTIMIZATION_EXIT
mbed_official 5:3762170b6d4d 680
mbed_official 5:3762170b6d4d 681 /**
mbed_official 5:3762170b6d4d 682 * @} end of BiquadCascadeDF2T group
mbed_official 5:3762170b6d4d 683 */