CMSIS DSP Software Library: arm_biquad_cascade_df1_q31.c Source File

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

- File List
- Globals

# arm_biquad_cascade_df1_q31.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_q31.c
    00009 *
    00010 * Description:  Processing function for the
    00011 *               Q31 Biquad cascade filter
    00012 *
    00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
    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.5  2010/04/26
    00031 *    incorporated review comments and updated with latest CMSIS layer
    00032 *
    00033 * Version 0.0.3  2010/03/10
    00034 *    Initial version
    00035 * -------------------------------------------------------------------- */
    00036
    00037 #include "arm_math.h"
    00038
    00069 void arm_biquad_cascade_df1_q31(
    00070   const arm_biquad_casd_df1_inst_q31 * S,
    00071   q31_t * pSrc,
    00072   q31_t * pDst,
    00073   uint32_t blockSize)
    00074 {
    00075   q31_t *pIn = pSrc;                             /*  input pointer initialization  */
    00076   q31_t *pOut = pDst;                            /*  output pointer initialization */
    00077   q31_t *pState = S->pState;                     /*  pState pointer initialization */
    00078   q31_t *pCoeffs = S->pCoeffs;                   /*  coeff pointer initialization  */
    00079   q63_t acc;                                     /*  accumulator                   */
    00080   q31_t Xn1, Xn2, Yn1, Yn2;                      /*  Filter state variables        */
    00081   q31_t b0, b1, b2, a1, a2;                      /*  Filter coefficients           */
    00082   q31_t Xn;                                      /*  temporary input               */
    00083   uint32_t shift = 32u - ((uint32_t) S->postShift + 1u);        /*  Shift to be applied to the output */
    00084   uint32_t sample, stage = S->numStages;         /*  loop counters                     */
    00085
    00086
    00087 #ifndef ARM_MATH_CM0
    00088
    00089   /* Run the below code for Cortex-M4 and Cortex-M3 */
    00090
    00091   do
    00092   {
    00093     /* Reading the coefficients */
    00094     b0 = *pCoeffs++;
    00095     b1 = *pCoeffs++;
    00096     b2 = *pCoeffs++;
    00097     a1 = *pCoeffs++;
    00098     a2 = *pCoeffs++;
    00099
    00100     /* Reading the state values */
    00101     Xn1 = pState[0];
    00102     Xn2 = pState[1];
    00103     Yn1 = pState[2];
    00104     Yn2 = pState[3];
    00105
    00106     /* Apply loop unrolling and compute 4 output values simultaneously. */
    00107     /*      The variable acc hold output values that are being computed:
    00108      *
    00109      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
    00110      */
    00111
    00112     sample = blockSize >> 2u;
    00113
    00114     /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
    00115      ** a second loop below computes the remaining 1 to 3 samples. */
    00116     while(sample > 0u)
    00117     {
    00118       /* Read the input */
    00119       Xn = *pIn++;
    00120
    00121       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00122
    00123       /* acc =  b0 * x[n] */
    00124       acc = (q63_t) b0 *Xn;
    00125       /* acc +=  b1 * x[n-1] */
    00126       acc += (q63_t) b1 *Xn1;
    00127       /* acc +=  b[2] * x[n-2] */
    00128       acc += (q63_t) b2 *Xn2;
    00129       /* acc +=  a1 * y[n-1] */
    00130       acc += (q63_t) a1 *Yn1;
    00131       /* acc +=  a2 * y[n-2] */
    00132       acc += (q63_t) a2 *Yn2;
    00133
    00134       /* The result is converted to 1.31 , Yn2 variable is reused */
    00135       Yn2 = (q31_t) (acc >> shift);
    00136
    00137       /* Store the output in the destination buffer. */
    00138       *pOut++ = Yn2;
    00139
    00140       /* Read the second input */
    00141       Xn2 = *pIn++;
    00142
    00143       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00144
    00145       /* acc =  b0 * x[n] */
    00146       acc = (q63_t) b0 *Xn2;
    00147       /* acc +=  b1 * x[n-1] */
    00148       acc += (q63_t) b1 *Xn;
    00149       /* acc +=  b[2] * x[n-2] */
    00150       acc += (q63_t) b2 *Xn1;
    00151       /* acc +=  a1 * y[n-1] */
    00152       acc += (q63_t) a1 *Yn2;
    00153       /* acc +=  a2 * y[n-2] */
    00154       acc += (q63_t) a2 *Yn1;
    00155
    00156
    00157       /* The result is converted to 1.31, Yn1 variable is reused  */
    00158       Yn1 = (q31_t) (acc >> shift);
    00159
    00160       /* Store the output in the destination buffer. */
    00161       *pOut++ = Yn1;
    00162
    00163       /* Read the third input  */
    00164       Xn1 = *pIn++;
    00165
    00166       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00167
    00168       /* acc =  b0 * x[n] */
    00169       acc = (q63_t) b0 *Xn1;
    00170       /* acc +=  b1 * x[n-1] */
    00171       acc += (q63_t) b1 *Xn2;
    00172       /* acc +=  b[2] * x[n-2] */
    00173       acc += (q63_t) b2 *Xn;
    00174       /* acc +=  a1 * y[n-1] */
    00175       acc += (q63_t) a1 *Yn1;
    00176       /* acc +=  a2 * y[n-2] */
    00177       acc += (q63_t) a2 *Yn2;
    00178
    00179       /* The result is converted to 1.31, Yn2 variable is reused  */
    00180       Yn2 = (q31_t) (acc >> shift);
    00181
    00182       /* Store the output in the destination buffer. */
    00183       *pOut++ = Yn2;
    00184
    00185       /* Read the forth input */
    00186       Xn = *pIn++;
    00187
    00188       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00189
    00190       /* acc =  b0 * x[n] */
    00191       acc = (q63_t) b0 *Xn;
    00192       /* acc +=  b1 * x[n-1] */
    00193       acc += (q63_t) b1 *Xn1;
    00194       /* acc +=  b[2] * x[n-2] */
    00195       acc += (q63_t) b2 *Xn2;
    00196       /* acc +=  a1 * y[n-1] */
    00197       acc += (q63_t) a1 *Yn2;
    00198       /* acc +=  a2 * y[n-2] */
    00199       acc += (q63_t) a2 *Yn1;
    00200
    00201       /* The result is converted to 1.31, Yn1 variable is reused  */
    00202       Yn1 = (q31_t) (acc >> shift);
    00203
    00204       /* Every time after the output is computed state should be updated. */
    00205       /* The states should be updated as:  */
    00206       /* Xn2 = Xn1    */
    00207       /* Xn1 = Xn     */
    00208       /* Yn2 = Yn1    */
    00209       /* Yn1 = acc    */
    00210       Xn2 = Xn1;
    00211       Xn1 = Xn;
    00212
    00213       /* Store the output in the destination buffer. */
    00214       *pOut++ = Yn1;
    00215
    00216       /* decrement the loop counter */
    00217       sample--;
    00218     }
    00219
    00220     /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
    00221      ** No loop unrolling is used. */
    00222     sample = (blockSize & 0x3u);
    00223
    00224     while(sample > 0u)
    00225     {
    00226       /* Read the input */
    00227       Xn = *pIn++;
    00228
    00229       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00230
    00231       /* acc =  b0 * x[n] */
    00232       acc = (q63_t) b0 *Xn;
    00233       /* acc +=  b1 * x[n-1] */
    00234       acc += (q63_t) b1 *Xn1;
    00235       /* acc +=  b[2] * x[n-2] */
    00236       acc += (q63_t) b2 *Xn2;
    00237       /* acc +=  a1 * y[n-1] */
    00238       acc += (q63_t) a1 *Yn1;
    00239       /* acc +=  a2 * y[n-2] */
    00240       acc += (q63_t) a2 *Yn2;
    00241
    00242       /* The result is converted to 1.31  */
    00243       acc = acc >> shift;
    00244
    00245       /* Every time after the output is computed state should be updated. */
    00246       /* The states should be updated as:  */
    00247       /* Xn2 = Xn1    */
    00248       /* Xn1 = Xn     */
    00249       /* Yn2 = Yn1    */
    00250       /* Yn1 = acc    */
    00251       Xn2 = Xn1;
    00252       Xn1 = Xn;
    00253       Yn2 = Yn1;
    00254       Yn1 = (q31_t) acc;
    00255
    00256       /* Store the output in the destination buffer. */
    00257       *pOut++ = (q31_t) acc;
    00258
    00259       /* decrement the loop counter */
    00260       sample--;
    00261     }
    00262
    00263     /*  The first stage goes from the input buffer to the output buffer. */
    00264     /*  Subsequent stages occur in-place in the output buffer */
    00265     pIn = pDst;
    00266
    00267     /* Reset to destination pointer */
    00268     pOut = pDst;
    00269
    00270     /*  Store the updated state variables back into the pState array */
    00271     *pState++ = Xn1;
    00272     *pState++ = Xn2;
    00273     *pState++ = Yn1;
    00274     *pState++ = Yn2;
    00275
    00276   } while(--stage);
    00277
    00278 #else
    00279
    00280   /* Run the below code for Cortex-M0 */
    00281
    00282   do
    00283   {
    00284     /* Reading the coefficients */
    00285     b0 = *pCoeffs++;
    00286     b1 = *pCoeffs++;
    00287     b2 = *pCoeffs++;
    00288     a1 = *pCoeffs++;
    00289     a2 = *pCoeffs++;
    00290
    00291     /* Reading the state values */
    00292     Xn1 = pState[0];
    00293     Xn2 = pState[1];
    00294     Yn1 = pState[2];
    00295     Yn2 = pState[3];
    00296
    00297     /*      The variables acc holds the output value that is computed:
    00298      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
    00299      */
    00300
    00301     sample = blockSize;
    00302
    00303     while(sample > 0u)
    00304     {
    00305       /* Read the input */
    00306       Xn = *pIn++;
    00307
    00308       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
    00309       /* acc =  b0 * x[n] */
    00310       acc = (q63_t) b0 *Xn;
    00311
    00312       /* acc +=  b1 * x[n-1] */
    00313       acc += (q63_t) b1 *Xn1;
    00314       /* acc +=  b[2] * x[n-2] */
    00315       acc += (q63_t) b2 *Xn2;
    00316       /* acc +=  a1 * y[n-1] */
    00317       acc += (q63_t) a1 *Yn1;
    00318       /* acc +=  a2 * y[n-2] */
    00319       acc += (q63_t) a2 *Yn2;
    00320
    00321       /* The result is converted to 1.31  */
    00322       acc = acc >> shift;
    00323
    00324       /* Every time after the output is computed state should be updated. */
    00325       /* The states should be updated as:  */
    00326       /* Xn2 = Xn1    */
    00327       /* Xn1 = Xn     */
    00328       /* Yn2 = Yn1    */
    00329       /* Yn1 = acc    */
    00330       Xn2 = Xn1;
    00331       Xn1 = Xn;
    00332       Yn2 = Yn1;
    00333       Yn1 = (q31_t) acc;
    00334
    00335       /* Store the output in the destination buffer. */
    00336       *pOut++ = (q31_t) acc;
    00337
    00338       /* decrement the loop counter */
    00339       sample--;
    00340     }
    00341
    00342     /*  The first stage goes from the input buffer to the output buffer. */
    00343     /*  Subsequent stages occur in-place in the output buffer */
    00344     pIn = pDst;
    00345
    00346     /* Reset to destination pointer */
    00347     pOut = pDst;
    00348
    00349     /*  Store the updated state variables back into the pState array */
    00350     *pState++ = Xn1;
    00351     *pState++ = Xn2;
    00352     *pState++ = Yn1;
    00353     *pState++ = Yn2;
    00354
    00355   } while(--stage);
    00356
    00357 #endif /*  #ifndef ARM_MATH_CM0 */
    00358 }
    00359

All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines