tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_biquad_cascade_df1_fast_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:            arm_biquad_cascade_df1_fast_q31.c
   9 *
  10 * Description:  Processing function for the
  11 *                               Q31 Fast Biquad cascade DirectFormI(DF1) filter.
  12 *
  13 * Target Processor: Cortex-M4/Cortex-M3
  14 *
  15 * Redistribution and use in source and binary forms, with or without
  16 * modification, are permitted provided that the following conditions
  17 * are met:
  18 *   - Redistributions of source code must retain the above copyright
  19 *     notice, this list of conditions and the following disclaimer.
  20 *   - Redistributions in binary form must reproduce the above copyright
  21 *     notice, this list of conditions and the following disclaimer in
  22 *     the documentation and/or other materials provided with the
  23 *     distribution.
  24 *   - Neither the name of ARM LIMITED nor the names of its contributors
  25 *     may be used to endorse or promote products derived from this
  26 *     software without specific prior written permission.
  27 *
  28 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  29 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  30 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  31 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  32 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  33 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  34 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  35 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  36 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  37 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39 * POSSIBILITY OF SUCH DAMAGE.
  40 * -------------------------------------------------------------------- */
  41
  42 #include "arm_math.h"
  43
  44 /**
  45  * @ingroup groupFilters
  46  */
  47
  48 /**
  49  * @addtogroup BiquadCascadeDF1
  50  * @{
  51  */
  52
  53 /**
  54  * @details
  55  *
  56  * @param[in]  *S        points to an instance of the Q31 Biquad cascade structure.
  57  * @param[in]  *pSrc     points to the block of input data.
  58  * @param[out] *pDst     points to the block of output data.
  59  * @param[in]  blockSize number of samples to process per call.
  60  * @return         none.
  61  *
  62  * <b>Scaling and Overflow Behavior:</b>
  63  * \par
  64  * This function is optimized for speed at the expense of fixed-point precision and overflow protection.
  65  * The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
  66  * These intermediate results are added to a 2.30 accumulator.
  67  * Finally, the accumulator is saturated and converted to a 1.31 result.
  68  * The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
  69  * 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). Use the intialization function
  70  * arm_biquad_cascade_df1_init_q31() to initialize filter structure.
  71  *
  72  * \par
  73  * Refer to the function <code>arm_biquad_cascade_df1_q31()</code> for a slower implementation of this function which uses 64-bit accumulation to provide higher precision.  Both the slow and the fast versions use the same instance structure.
  74  * Use the function <code>arm_biquad_cascade_df1_init_q31()</code> to initialize the filter structure.
  75  */
  76
  77 void arm_biquad_cascade_df1_fast_q31(
  78   const arm_biquad_casd_df1_inst_q31 * S,
  79   q31_t * pSrc,
  80   q31_t * pDst,
  81   uint32_t blockSize)
  82 {
  83   q31_t acc = 0;                                 /*  accumulator                   */
  84   q31_t Xn1, Xn2, Yn1, Yn2;                      /*  Filter state variables        */
  85   q31_t b0, b1, b2, a1, a2;                      /*  Filter coefficients           */
  86   q31_t *pIn = pSrc;                             /*  input pointer initialization  */
  87   q31_t *pOut = pDst;                            /*  output pointer initialization */
  88   q31_t *pState = S->pState;                     /*  pState pointer initialization */
  89   q31_t *pCoeffs = S->pCoeffs;                   /*  coeff pointer initialization  */
  90   q31_t Xn;                                      /*  temporary input               */
  91   int32_t shift = (int32_t) S->postShift + 1;    /*  Shift to be applied to the output */
  92   uint32_t sample, stage = S->numStages;         /*  loop counters                     */
  93
  94
  95   do
  96   {
  97     /* Reading the coefficients */
  98     b0 = *pCoeffs++;
  99     b1 = *pCoeffs++;
 100     b2 = *pCoeffs++;
 101     a1 = *pCoeffs++;
 102     a2 = *pCoeffs++;
 103
 104     /* Reading the state values */
 105     Xn1 = pState[0];
 106     Xn2 = pState[1];
 107     Yn1 = pState[2];
 108     Yn2 = pState[3];
 109
 110     /* Apply loop unrolling and compute 4 output values simultaneously. */
 111     /*      The variables acc ... acc3 hold output values that are being computed:
 112      *
 113      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
 114      */
 115
 116     sample = blockSize >> 2u;
 117
 118     /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
 119      ** a second loop below computes the remaining 1 to 3 samples. */
 120     while(sample > 0u)
 121     {
 122       /* Read the input */
 123       Xn = *pIn;
 124
 125       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
 126       /* acc =  b0 * x[n] */
 127       //acc = (q31_t) (((q63_t) b1 * Xn1) >> 32);
 128       mult_32x32_keep32_R(acc, b1, Xn1);
 129       /* acc +=  b1 * x[n-1] */
 130       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b0 * (Xn))) >> 32);
 131       multAcc_32x32_keep32_R(acc, b0, Xn);
 132       /* acc +=  b[2] * x[n-2] */
 133       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
 134       multAcc_32x32_keep32_R(acc, b2, Xn2);
 135       /* acc +=  a1 * y[n-1] */
 136       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
 137       multAcc_32x32_keep32_R(acc, a1, Yn1);
 138       /* acc +=  a2 * y[n-2] */
 139       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
 140       multAcc_32x32_keep32_R(acc, a2, Yn2);
 141
 142       /* The result is converted to 1.31 , Yn2 variable is reused */
 143       Yn2 = acc << shift;
 144
 145       /* Read the second input */
 146       Xn2 = *(pIn + 1u);
 147
 148       /* Store the output in the destination buffer. */
 149       *pOut = Yn2;
 150
 151       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
 152       /* acc =  b0 * x[n] */
 153       //acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);
 154       mult_32x32_keep32_R(acc, b0, Xn2);
 155       /* acc +=  b1 * x[n-1] */
 156       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);
 157       multAcc_32x32_keep32_R(acc, b1, Xn);
 158       /* acc +=  b[2] * x[n-2] */
 159       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);
 160       multAcc_32x32_keep32_R(acc, b2, Xn1);
 161       /* acc +=  a1 * y[n-1] */
 162       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
 163       multAcc_32x32_keep32_R(acc, a1, Yn2);
 164       /* acc +=  a2 * y[n-2] */
 165       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
 166       multAcc_32x32_keep32_R(acc, a2, Yn1);
 167
 168       /* The result is converted to 1.31, Yn1 variable is reused  */
 169       Yn1 = acc << shift;
 170
 171       /* Read the third input  */
 172       Xn1 = *(pIn + 2u);
 173
 174       /* Store the output in the destination buffer. */
 175       *(pOut + 1u) = Yn1;
 176
 177       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
 178       /* acc =  b0 * x[n] */
 179       //acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);
 180       mult_32x32_keep32_R(acc, b0, Xn1);
 181       /* acc +=  b1 * x[n-1] */
 182       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);
 183       multAcc_32x32_keep32_R(acc, b1, Xn2);
 184       /* acc +=  b[2] * x[n-2] */
 185       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);
 186       multAcc_32x32_keep32_R(acc, b2, Xn);
 187       /* acc +=  a1 * y[n-1] */
 188       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
 189       multAcc_32x32_keep32_R(acc, a1, Yn1);
 190       /* acc +=  a2 * y[n-2] */
 191       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
 192       multAcc_32x32_keep32_R(acc, a2, Yn2);
 193
 194       /* The result is converted to 1.31, Yn2 variable is reused  */
 195       Yn2 = acc << shift;
 196
 197       /* Read the forth input */
 198       Xn = *(pIn + 3u);
 199
 200       /* Store the output in the destination buffer. */
 201       *(pOut + 2u) = Yn2;
 202       pIn += 4u;
 203
 204       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
 205       /* acc =  b0 * x[n] */
 206       //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
 207       mult_32x32_keep32_R(acc, b0, Xn);
 208       /* acc +=  b1 * x[n-1] */
 209       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
 210       multAcc_32x32_keep32_R(acc, b1, Xn1);
 211       /* acc +=  b[2] * x[n-2] */
 212       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
 213       multAcc_32x32_keep32_R(acc, b2, Xn2);
 214       /* acc +=  a1 * y[n-1] */
 215       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
 216       multAcc_32x32_keep32_R(acc, a1, Yn2);
 217       /* acc +=  a2 * y[n-2] */
 218       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
 219       multAcc_32x32_keep32_R(acc, a2, Yn1);
 220
 221       /* Every time after the output is computed state should be updated. */
 222       /* The states should be updated as:  */
 223       /* Xn2 = Xn1    */
 224       Xn2 = Xn1;
 225
 226       /* The result is converted to 1.31, Yn1 variable is reused  */
 227       Yn1 = acc << shift;
 228
 229       /* Xn1 = Xn     */
 230       Xn1 = Xn;
 231
 232       /* Store the output in the destination buffer. */
 233       *(pOut + 3u) = Yn1;
 234       pOut += 4u;
 235
 236       /* decrement the loop counter */
 237       sample--;
 238     }
 239
 240     /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 241      ** No loop unrolling is used. */
 242     sample = (blockSize & 0x3u);
 243
 244    while(sample > 0u)
 245    {
 246       /* Read the input */
 247       Xn = *pIn++;
 248
 249       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
 250       /* acc =  b0 * x[n] */
 251       //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
 252       mult_32x32_keep32_R(acc, b0, Xn);
 253       /* acc +=  b1 * x[n-1] */
 254       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
 255       multAcc_32x32_keep32_R(acc, b1, Xn1);
 256       /* acc +=  b[2] * x[n-2] */
 257       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
 258       multAcc_32x32_keep32_R(acc, b2, Xn2);
 259       /* acc +=  a1 * y[n-1] */
 260       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
 261       multAcc_32x32_keep32_R(acc, a1, Yn1);
 262       /* acc +=  a2 * y[n-2] */
 263       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
 264       multAcc_32x32_keep32_R(acc, a2, Yn2);
 265
 266       /* The result is converted to 1.31  */
 267       acc = acc << shift;
 268
 269       /* Every time after the output is computed state should be updated. */
 270       /* The states should be updated as:  */
 271       /* Xn2 = Xn1    */
 272       /* Xn1 = Xn     */
 273       /* Yn2 = Yn1    */
 274       /* Yn1 = acc    */
 275       Xn2 = Xn1;
 276       Xn1 = Xn;
 277       Yn2 = Yn1;
 278       Yn1 = acc;
 279
 280       /* Store the output in the destination buffer. */
 281       *pOut++ = acc;
 282
 283       /* decrement the loop counter */
 284       sample--;
 285    }
 286
 287     /*  The first stage goes from the input buffer to the output buffer. */
 288     /*  Subsequent stages occur in-place in the output buffer */
 289     pIn = pDst;
 290
 291     /* Reset to destination pointer */
 292     pOut = pDst;
 293
 294     /*  Store the updated state variables back into the pState array */
 295     *pState++ = Xn1;
 296     *pState++ = Xn2;
 297     *pState++ = Yn1;
 298     *pState++ = Yn2;
 299
 300   } while(--stage);
 301 }
 302
 303 /**
 304   * @} end of BiquadCascadeDF1 group
 305   */