CMSIS DSP Software Library: arm_biquad_cascade_df1_fast_q15.c Source File

- Main Page
- Modules
- Data Structures
- Files
- Examples

- File List
- Globals

# arm_biquad_cascade_df1_fast_q15.c

Go to the documentation of this file.

    00001 /* ----------------------------------------------------------------------
    00002 * Copyright (C) 2010 ARM Limited. All rights reserved.
    00003 *
    00004 * $Date:        15. July 2011
    00005 * $Revision:    V1.0.10
    00006 *
    00007 * Project:      CMSIS DSP Library
    00008 * Title:        arm_biquad_cascade_df1_fast_q15.c
    00009 *
    00010 * Description:  Fast processing function for the
    00011 *               Q15 Biquad cascade filter.
    00012 *
    00013 * Target Processor: Cortex-M4/Cortex-M3
    00014 *
    00015 * Version 1.0.10 2011/7/15
    00016 *    Big Endian support added and Merged M0 and M3/M4 Source code.
    00017 *
    00018 * Version 1.0.3 2010/11/29
    00019 *    Re-organized the CMSIS folders and updated documentation.
    00020 *
    00021 * Version 1.0.2 2010/11/11
    00022 *    Documentation updated.
    00023 *
    00024 * Version 1.0.1 2010/10/05
    00025 *    Production release and review comments incorporated.
    00026 *
    00027 * Version 1.0.0 2010/09/20
    00028 *    Production release and review comments incorporated.
    00029 *
    00030 * Version 0.0.9  2010/08/16
    00031 *    Initial version
    00032 *
    00033 *
    00034 * -------------------------------------------------------------------- */
    00035
    00036 #include "arm_math.h"
    00037
    00069 void arm_biquad_cascade_df1_fast_q15(
    00070   const arm_biquad_casd_df1_inst_q15 * S,
    00071   q15_t * pSrc,
    00072   q15_t * pDst,
    00073   uint32_t blockSize)
    00074 {
    00075   q15_t *pIn = pSrc;                             /*  Source pointer                               */
    00076   q15_t *pOut = pDst;                            /*  Destination pointer                          */
    00077   q31_t in;                                      /*  Temporary variable to hold input value       */
    00078   q31_t out;                                     /*  Temporary variable to hold output value      */
    00079   q31_t b0;                                      /*  Temporary variable to hold bo value          */
    00080   q31_t b1, a1;                                  /*  Filter coefficients                          */
    00081   q31_t state_in, state_out;                     /*  Filter state variables                       */
    00082   q31_t acc0;                                    /*  Accumulator                                  */
    00083   int32_t shift = (int32_t) (15 - S->postShift); /*  Post shift                                   */
    00084   q15_t *pState = S->pState;                     /*  State pointer                                */
    00085   q15_t *pCoeffs = S->pCoeffs;                   /*  Coefficient pointer                          */
    00086   q31_t *pState_q31;                             /*  32-bit state pointer for SIMD implementation */
    00087   uint32_t sample, stage = S->numStages;         /*  Stage loop counter                           */
    00088
    00089
    00090
    00091   do
    00092   {
    00093     /* Initialize state pointer of type q31 */
    00094     pState_q31 = (q31_t *) (pState);
    00095
    00096     /* Read the b0 and 0 coefficients using SIMD  */
    00097     b0 = *__SIMD32(pCoeffs)++;
    00098
    00099     /* Read the b1 and b2 coefficients using SIMD */
    00100     b1 = *__SIMD32(pCoeffs)++;
    00101
    00102     /* Read the a1 and a2 coefficients using SIMD */
    00103     a1 = *__SIMD32(pCoeffs)++;
    00104
    00105     /* Read the input state values from the state buffer:  x[n-1], x[n-2] */
    00106     state_in = (q31_t) (*pState_q31++);
    00107
    00108     /* Read the output state values from the state buffer:  y[n-1], y[n-2] */
    00109     state_out = (q31_t) (*pState_q31);
    00110
    00111     /* Apply loop unrolling and compute 2 output values simultaneously. */
    00112     /*      The variables acc0 ... acc3 hold output values that are being computed:
    00113      *
    00114      *    acc0 =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
    00115      *    acc0 =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
    00116      */
    00117     sample = blockSize >> 1u;
    00118
    00119     /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.
    00120      ** a second loop below computes the remaining 1 sample. */
    00121     while(sample > 0u)
    00122     {
    00123
    00124       /* Read the input */
    00125       in = *__SIMD32(pIn)++;
    00126
    00127       /* out =  b0 * x[n] + 0 * 0 */
    00128       out = __SMUAD(b0, in);
    00129       /* acc0 =  b1 * x[n-1] + acc0 +=  b2 * x[n-2] + out */
    00130       acc0 = __SMLAD(b1, state_in, out);
    00131       /* acc0 +=  a1 * y[n-1] + acc0 +=  a2 * y[n-2] */
    00132       acc0 = __SMLAD(a1, state_out, acc0);
    00133
    00134       /* The result is converted from 3.29 to 1.31 and then saturation is applied */
    00135       out = __SSAT((acc0 >> shift), 16);
    00136
    00137       /* Every time after the output is computed state should be updated. */
    00138       /* The states should be updated as:  */
    00139       /* Xn2 = Xn1    */
    00140       /* Xn1 = Xn     */
    00141       /* Yn2 = Yn1    */
    00142       /* Yn1 = acc0   */
    00143       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
    00144       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
    00145
    00146 #ifndef  ARM_MATH_BIG_ENDIAN
    00147
    00148       state_in = __PKHBT(in, state_in, 16);
    00149       state_out = __PKHBT(out, state_out, 16);
    00150
    00151 #else
    00152
    00153       state_in = __PKHBT(state_in >> 16, (in >> 16), 16);
    00154       state_out = __PKHBT(state_out >> 16, (out), 16);
    00155
    00156 #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
    00157
    00158       /* out =  b0 * x[n] + 0 * 0 */
    00159       out = __SMUADX(b0, in);
    00160       /* acc0 =  b1 * x[n-1] + acc0 +=  b2 * x[n-2] + out */
    00161       acc0 = __SMLAD(b1, state_in, out);
    00162       /* acc0 +=  a1 * y[n-1] + acc0 +=  a2 * y[n-2] */
    00163       acc0 = __SMLAD(a1, state_out, acc0);
    00164
    00165       /* The result is converted from 3.29 to 1.31 and then saturation is applied */
    00166       out = __SSAT((acc0 >> shift), 16);
    00167
    00168
    00169       /* Store the output in the destination buffer. */
    00170
    00171 #ifndef  ARM_MATH_BIG_ENDIAN
    00172
    00173       *__SIMD32(pOut)++ = __PKHBT(state_out, out, 16);
    00174
    00175 #else
    00176
    00177       *__SIMD32(pOut)++ = __PKHBT(out, state_out >> 16, 16);
    00178
    00179 #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
    00180
    00181       /* Every time after the output is computed state should be updated. */
    00182       /* The states should be updated as:  */
    00183       /* Xn2 = Xn1    */
    00184       /* Xn1 = Xn     */
    00185       /* Yn2 = Yn1    */
    00186       /* Yn1 = acc0   */
    00187       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
    00188       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
    00189
    00190 #ifndef  ARM_MATH_BIG_ENDIAN
    00191
    00192       state_in = __PKHBT(in >> 16, state_in, 16);
    00193       state_out = __PKHBT(out, state_out, 16);
    00194
    00195 #else
    00196
    00197       state_in = __PKHBT(state_in >> 16, in, 16);
    00198       state_out = __PKHBT(state_out >> 16, out, 16);
    00199
    00200 #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
    00201
    00202
    00203       /* Decrement the loop counter */
    00204       sample--;
    00205
    00206     }
    00207
    00208     /* If the blockSize is not a multiple of 2, compute any remaining output samples here.
    00209      ** No loop unrolling is used. */
    00210
    00211     if((blockSize & 0x1u) != 0u)
    00212     {
    00213       /* Read the input */
    00214       in = *pIn++;
    00215
    00216       /* out =  b0 * x[n] + 0 * 0 */
    00217
    00218 #ifndef  ARM_MATH_BIG_ENDIAN
    00219
    00220       out = __SMUAD(b0, in);
    00221
    00222 #else
    00223
    00224       out = __SMUADX(b0, in);
    00225
    00226 #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */
    00227
    00228       /* acc0 =  b1 * x[n-1] + acc0 +=  b2 * x[n-2] + out */
    00229       acc0 = __SMLAD(b1, state_in, out);
    00230       /* acc0 +=  a1 * y[n-1] + acc0 +=  a2 * y[n-2] */
    00231       acc0 = __SMLAD(a1, state_out, acc0);
    00232
    00233       /* The result is converted from 3.29 to 1.31 and then saturation is applied */
    00234       out = __SSAT((acc0 >> shift), 16);
    00235
    00236       /* Store the output in the destination buffer. */
    00237       *pOut++ = (q15_t) out;
    00238
    00239       /* Every time after the output is computed state should be updated. */
    00240       /* The states should be updated as:  */
    00241       /* Xn2 = Xn1    */
    00242       /* Xn1 = Xn     */
    00243       /* Yn2 = Yn1    */
    00244       /* Yn1 = acc0   */
    00245       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */
    00246       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */
    00247
    00248 #ifndef  ARM_MATH_BIG_ENDIAN
    00249
    00250       state_in = __PKHBT(in, state_in, 16);
    00251       state_out = __PKHBT(out, state_out, 16);
    00252
    00253 #else
    00254
    00255       state_in = __PKHBT(state_in >> 16, in, 16);
    00256       state_out = __PKHBT(state_out >> 16, out, 16);
    00257
    00258 #endif /*   #ifndef  ARM_MATH_BIG_ENDIAN    */
    00259
    00260     }
    00261
    00262     /*  The first stage goes from the input buffer to the output buffer.  */
    00263     /*  Subsequent (numStages - 1) occur in-place in the output buffer  */
    00264     pIn = pDst;
    00265
    00266     /* Reset the output pointer */
    00267     pOut = pDst;
    00268
    00269     /*  Store the updated state variables back into the state array */
    00270     *__SIMD32(pState)++ = state_in;
    00271     *__SIMD32(pState)++ = state_out;
    00272
    00273
    00274     /* Decrement the loop counter */
    00275     stage--;
    00276
    00277   } while(stage > 0u);
    00278 }
    00279
    00280

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines