Neil Tan / dsp

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of dsp by Simon Ford

src/Cortex-M4-M3/FilteringFunctions/arm_biquad_cascade_df1_fast_q15.c@3:ad02f4ea1fbe, 2017-06-29 (annotated)

Committer:
nprobably
Date:
Thu Jun 29 06:56:55 2017 +0000
Revision:
3:ad02f4ea1fbe
Parent:
0:1014af42efd9 
A hack to make it compile with mbed 5

Who changed what in which revision?

UserRevisionLine numberNew contents of line
simon 0:1014af42efd9 1 /* ----------------------------------------------------------------------
simon 0:1014af42efd9 2 * Copyright (C) 2010 ARM Limited. All rights reserved.
simon 0:1014af42efd9 3 *
simon 0:1014af42efd9 4 * $Date: 29. November 2010
simon 0:1014af42efd9 5 * $Revision: V1.0.3
simon 0:1014af42efd9 6 *
simon 0:1014af42efd9 7 * Project: CMSIS DSP Library
simon 0:1014af42efd9 8 * Title: arm_biquad_cascade_df1_fast_q15.c
simon 0:1014af42efd9 9 *
simon 0:1014af42efd9 10 * Description: Fast processing function for the
simon 0:1014af42efd9 11 * Q15 Biquad cascade filter.
simon 0:1014af42efd9 12 *
simon 0:1014af42efd9 13 * Target Processor: Cortex-M4/Cortex-M3
simon 0:1014af42efd9 14 *
simon 0:1014af42efd9 15 * Version 1.0.3 2010/11/29
simon 0:1014af42efd9 16 * Re-organized the CMSIS folders and updated documentation.
simon 0:1014af42efd9 17 *
simon 0:1014af42efd9 18 * Version 1.0.2 2010/11/11
simon 0:1014af42efd9 19 * Documentation updated.
simon 0:1014af42efd9 20 *
simon 0:1014af42efd9 21 * Version 1.0.1 2010/10/05
simon 0:1014af42efd9 22 * Production release and review comments incorporated.
simon 0:1014af42efd9 23 *
simon 0:1014af42efd9 24 * Version 1.0.0 2010/09/20
simon 0:1014af42efd9 25 * Production release and review comments incorporated.
simon 0:1014af42efd9 26 *
simon 0:1014af42efd9 27 * Version 0.0.9 2010/08/16
simon 0:1014af42efd9 28 * Initial version
simon 0:1014af42efd9 29 *
simon 0:1014af42efd9 30 *
simon 0:1014af42efd9 31 * -------------------------------------------------------------------- */
simon 0:1014af42efd9 32
simon 0:1014af42efd9 33 #include "arm_math.h"
simon 0:1014af42efd9 34
simon 0:1014af42efd9 35 /**
simon 0:1014af42efd9 36 * @ingroup groupFilters
simon 0:1014af42efd9 37 */
simon 0:1014af42efd9 38
simon 0:1014af42efd9 39 /**
simon 0:1014af42efd9 40 * @addtogroup BiquadCascadeDF1
simon 0:1014af42efd9 41 * @{
simon 0:1014af42efd9 42 */
simon 0:1014af42efd9 43
simon 0:1014af42efd9 44 /**
simon 0:1014af42efd9 45 * @details
simon 0:1014af42efd9 46 * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
simon 0:1014af42efd9 47 * @param[in] *pSrc points to the block of input data.
simon 0:1014af42efd9 48 * @param[out] *pDst points to the block of output data.
simon 0:1014af42efd9 49 * @param[in] blockSize number of samples to process per call.
simon 0:1014af42efd9 50 * @return none.
simon 0:1014af42efd9 51 *
simon 0:1014af42efd9 52 * <b>Scaling and Overflow Behavior:</b>
simon 0:1014af42efd9 53 * \par
simon 0:1014af42efd9 54 * This fast version uses a 32-bit accumulator with 2.30 format.
simon 0:1014af42efd9 55 * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.
simon 0:1014af42efd9 56 * Thus, if the accumulator result overflows it wraps around and distorts the result.
simon 0:1014af42efd9 57 * In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25).
simon 0:1014af42efd9 58 * The 2.30 accumulator is then shifted by <code>postShift</code> bits and the result truncated to 1.15 format by discarding the low 16 bits.
simon 0:1014af42efd9 59 *
simon 0:1014af42efd9 60 * \par
simon 0:1014af42efd9 61 * Refer to the function <code>arm_biquad_cascade_df1_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion. Both the slow and the fast versions use the same instance structure.
simon 0:1014af42efd9 62 * Use the function <code>arm_biquad_cascade_df1_init_q15()</code> to initialize the filter structure.
simon 0:1014af42efd9 63 *
simon 0:1014af42efd9 64 */
simon 0:1014af42efd9 65
simon 0:1014af42efd9 66 void arm_biquad_cascade_df1_fast_q15(
simon 0:1014af42efd9 67 const arm_biquad_casd_df1_inst_q15 * S,
simon 0:1014af42efd9 68 q15_t * pSrc,
simon 0:1014af42efd9 69 q15_t * pDst,
simon 0:1014af42efd9 70 uint32_t blockSize)
simon 0:1014af42efd9 71 {
simon 0:1014af42efd9 72 q15_t *pIn = pSrc; /* Source pointer */
simon 0:1014af42efd9 73 q15_t *pOut = pDst; /* Destination pointer */
simon 0:1014af42efd9 74 q31_t in; /* Temporary variable to hold input value */
simon 0:1014af42efd9 75 q31_t out; /* Temporary variable to hold output value */
simon 0:1014af42efd9 76 q15_t b0;
simon 0:1014af42efd9 77 q31_t b1, a1; /* Filter coefficients */
simon 0:1014af42efd9 78 q31_t state_in, state_out; /* Filter state variables */
simon 0:1014af42efd9 79 q31_t acc0; /* Accumulator */
simon 0:1014af42efd9 80 int32_t shift = (int32_t) (15 - S->postShift); /* Post shift */
simon 0:1014af42efd9 81 q15_t *pState = S->pState; /* State pointer */
simon 0:1014af42efd9 82 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
simon 0:1014af42efd9 83 q31_t *pState_q31; /* 32-bit state pointer for SIMD implementation */
simon 0:1014af42efd9 84 uint32_t sample, stage = S->numStages; /* Stage loop counter */
simon 0:1014af42efd9 85
simon 0:1014af42efd9 86
simon 0:1014af42efd9 87
simon 0:1014af42efd9 88 do
simon 0:1014af42efd9 89 {
simon 0:1014af42efd9 90 /* Initialize state pointer of type q31 */
simon 0:1014af42efd9 91 pState_q31 = (q31_t *) (pState);
simon 0:1014af42efd9 92
simon 0:1014af42efd9 93 /* Read the b0 and 0 coefficients using SIMD */
simon 0:1014af42efd9 94 b0 = *__SIMD32(pCoeffs)++;
simon 0:1014af42efd9 95
simon 0:1014af42efd9 96 /* Read the b1 and b2 coefficients using SIMD */
simon 0:1014af42efd9 97 b1 = *__SIMD32(pCoeffs)++;
simon 0:1014af42efd9 98
simon 0:1014af42efd9 99 /* Read the a1 and a2 coefficients using SIMD */
simon 0:1014af42efd9 100 a1 = *__SIMD32(pCoeffs)++;
simon 0:1014af42efd9 101
simon 0:1014af42efd9 102 /* Read the input state values from the state buffer: x[n-1], x[n-2] */
simon 0:1014af42efd9 103 state_in = (q31_t) (*pState_q31++);
simon 0:1014af42efd9 104
simon 0:1014af42efd9 105 /* Read the output state values from the state buffer: y[n-1], y[n-2] */
simon 0:1014af42efd9 106 state_out = (q31_t) (*pState_q31);
simon 0:1014af42efd9 107
simon 0:1014af42efd9 108 /* Apply loop unrolling and compute 2 output values simultaneously. */
simon 0:1014af42efd9 109 /* The variables acc0 ... acc3 hold output values that are being computed:
simon 0:1014af42efd9 110 *
simon 0:1014af42efd9 111 * acc0 = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
simon 0:1014af42efd9 112 * acc0 = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
simon 0:1014af42efd9 113 */
simon 0:1014af42efd9 114 sample = blockSize >> 1u;
simon 0:1014af42efd9 115
simon 0:1014af42efd9 116 /* First part of the processing with loop unrolling. Compute 2 outputs at a time.
simon 0:1014af42efd9 117 ** a second loop below computes the remaining 1 sample. */
simon 0:1014af42efd9 118 while(sample > 0u)
simon 0:1014af42efd9 119 {
simon 0:1014af42efd9 120
simon 0:1014af42efd9 121 /* Read the input */
simon 0:1014af42efd9 122 in = *__SIMD32(pIn)++;
simon 0:1014af42efd9 123
simon 0:1014af42efd9 124 /* out = b0 * x[n] + 0 * 0 */
simon 0:1014af42efd9 125 out = (q31_t) b0 * ((q15_t) in);
simon 0:1014af42efd9 126 /* acc0 = b1 * x[n-1] + acc0 += b2 * x[n-2] + out */
simon 0:1014af42efd9 127 acc0 = __SMLAD(b1, state_in, out);
simon 0:1014af42efd9 128 /* acc0 += a1 * y[n-1] + acc0 += a2 * y[n-2] */
simon 0:1014af42efd9 129 acc0 = __SMLAD(a1, state_out, acc0);
simon 0:1014af42efd9 130
simon 0:1014af42efd9 131 /* The result is converted from 3.29 to 1.31 and then saturation is applied */
simon 0:1014af42efd9 132 out = __SSAT((acc0 >> shift), 16);
simon 0:1014af42efd9 133
simon 0:1014af42efd9 134 /* Every time after the output is computed state should be updated. */
simon 0:1014af42efd9 135 /* The states should be updated as: */
simon 0:1014af42efd9 136 /* Xn2 = Xn1 */
simon 0:1014af42efd9 137 /* Xn1 = Xn */
simon 0:1014af42efd9 138 /* Yn2 = Yn1 */
simon 0:1014af42efd9 139 /* Yn1 = acc0 */
simon 0:1014af42efd9 140 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
simon 0:1014af42efd9 141 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
simon 0:1014af42efd9 142 state_in = __PKHBT(in, state_in, 16);
simon 0:1014af42efd9 143 state_out = __PKHBT(out, state_out, 16);
simon 0:1014af42efd9 144
simon 0:1014af42efd9 145 /* out = b0 * x[n] + 0 * 0 */
simon 0:1014af42efd9 146 out = (q31_t) b0 *((q15_t)(in >> 16));
simon 0:1014af42efd9 147 /* acc0 = b1 * x[n-1] + acc0 += b2 * x[n-2] + out */
simon 0:1014af42efd9 148 acc0 = __SMLAD(b1, state_in, out);
simon 0:1014af42efd9 149 /* acc0 += a1 * y[n-1] + acc0 += a2 * y[n-2] */
simon 0:1014af42efd9 150 acc0 = __SMLAD(a1, state_out, acc0);
simon 0:1014af42efd9 151
simon 0:1014af42efd9 152 /* The result is converted from 3.29 to 1.31 and then saturation is applied */
simon 0:1014af42efd9 153 out = __SSAT((acc0 >> shift), 16);
simon 0:1014af42efd9 154
simon 0:1014af42efd9 155 /* Store the output in the destination buffer. */
simon 0:1014af42efd9 156 *__SIMD32(pOut)++ = __PKHBT(state_out, out, 16);
simon 0:1014af42efd9 157
simon 0:1014af42efd9 158 /* Every time after the output is computed state should be updated. */
simon 0:1014af42efd9 159 /* The states should be updated as: */
simon 0:1014af42efd9 160 /* Xn2 = Xn1 */
simon 0:1014af42efd9 161 /* Xn1 = Xn */
simon 0:1014af42efd9 162 /* Yn2 = Yn1 */
simon 0:1014af42efd9 163 /* Yn1 = acc0 */
simon 0:1014af42efd9 164 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
simon 0:1014af42efd9 165 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
simon 0:1014af42efd9 166 state_in = __PKHBT(in >> 16, state_in, 16);
simon 0:1014af42efd9 167 state_out = __PKHBT(out, state_out, 16);
simon 0:1014af42efd9 168
simon 0:1014af42efd9 169 /* Decrement the loop counter */
simon 0:1014af42efd9 170 sample--;
simon 0:1014af42efd9 171
simon 0:1014af42efd9 172 }
simon 0:1014af42efd9 173
simon 0:1014af42efd9 174 /* If the blockSize is not a multiple of 2, compute any remaining output samples here.
simon 0:1014af42efd9 175 ** No loop unrolling is used. */
simon 0:1014af42efd9 176
simon 0:1014af42efd9 177 if((blockSize & 0x1u) != 0u)
simon 0:1014af42efd9 178 {
simon 0:1014af42efd9 179 /* Read the input */
simon 0:1014af42efd9 180 in = *pIn++;
simon 0:1014af42efd9 181
simon 0:1014af42efd9 182 /* out = b0 * x[n] + 0 * 0 */
simon 0:1014af42efd9 183 out = (q31_t) in *b0;
simon 0:1014af42efd9 184 /* acc0 = b1 * x[n-1] + acc0 += b2 * x[n-2] + out */
simon 0:1014af42efd9 185 acc0 = __SMLAD(b1, state_in, out);
simon 0:1014af42efd9 186 /* acc0 += a1 * y[n-1] + acc0 += a2 * y[n-2] */
simon 0:1014af42efd9 187 acc0 = __SMLAD(a1, state_out, acc0);
simon 0:1014af42efd9 188
simon 0:1014af42efd9 189 /* The result is converted from 3.29 to 1.31 and then saturation is applied */
simon 0:1014af42efd9 190 out = __SSAT((acc0 >> shift), 16);
simon 0:1014af42efd9 191
simon 0:1014af42efd9 192 /* Store the output in the destination buffer. */
simon 0:1014af42efd9 193 *pOut++ = (q15_t) out;
simon 0:1014af42efd9 194
simon 0:1014af42efd9 195 /* Every time after the output is computed state should be updated. */
simon 0:1014af42efd9 196 /* The states should be updated as: */
simon 0:1014af42efd9 197 /* Xn2 = Xn1 */
simon 0:1014af42efd9 198 /* Xn1 = Xn */
simon 0:1014af42efd9 199 /* Yn2 = Yn1 */
simon 0:1014af42efd9 200 /* Yn1 = acc0 */
simon 0:1014af42efd9 201 /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
simon 0:1014af42efd9 202 /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
simon 0:1014af42efd9 203 state_in = __PKHBT(in, state_in, 16);
simon 0:1014af42efd9 204 state_out = __PKHBT(out, state_out, 16);
simon 0:1014af42efd9 205
simon 0:1014af42efd9 206 }
simon 0:1014af42efd9 207
simon 0:1014af42efd9 208 /* The first stage goes from the input buffer to the output buffer. */
simon 0:1014af42efd9 209 /* Subsequent (numStages - 1) occur in-place in the output buffer */
simon 0:1014af42efd9 210 pIn = pDst;
simon 0:1014af42efd9 211
simon 0:1014af42efd9 212 /* Reset the output pointer */
simon 0:1014af42efd9 213 pOut = pDst;
simon 0:1014af42efd9 214
simon 0:1014af42efd9 215 /* Store the updated state variables back into the state array */
simon 0:1014af42efd9 216 *__SIMD32(pState)++ = __PKHBT(state_in, (state_in >> 16), 16);
simon 0:1014af42efd9 217 *__SIMD32(pState)++ = __PKHBT(state_out, (state_out >> 16), 16);
simon 0:1014af42efd9 218
simon 0:1014af42efd9 219 /* Decrement the loop counter */
simon 0:1014af42efd9 220 stage--;
simon 0:1014af42efd9 221
simon 0:1014af42efd9 222 } while(stage > 0u);
simon 0:1014af42efd9 223 }
simon 0:1014af42efd9 224
simon 0:1014af42efd9 225
simon 0:1014af42efd9 226 /**
simon 0:1014af42efd9 227 * @} end of BiquadCascadeDF1 group
simon 0:1014af42efd9 228 */