tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FilteringFunctions/arm_fir_lattice_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_fir_lattice_q31.c
   9 *
  10 * Description:  Q31 FIR lattice filter processing function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40
  41 #include "arm_math.h"
  42
  43 /**
  44  * @ingroup groupFilters
  45  */
  46
  47 /**
  48  * @addtogroup FIR_Lattice
  49  * @{
  50  */
  51
  52
  53 /**
  54  * @brief Processing function for the Q31 FIR lattice filter.
  55  * @param[in]  *S        points to an instance of the Q31 FIR lattice structure.
  56  * @param[in]  *pSrc     points to the block of input data.
  57  * @param[out] *pDst     points to the block of output data
  58  * @param[in]  blockSize number of samples to process.
  59  * @return none.
  60  *
  61  * @details
  62  * <b>Scaling and Overflow Behavior:</b>
  63  * In order to avoid overflows the input signal must be scaled down by 2*log2(numStages) bits.
  64  */
  65
  66 #ifndef ARM_MATH_CM0_FAMILY
  67
  68   /* Run the below code for Cortex-M4 and Cortex-M3 */
  69
  70 void arm_fir_lattice_q31(
  71   const arm_fir_lattice_instance_q31 * S,
  72   q31_t * pSrc,
  73   q31_t * pDst,
  74   uint32_t blockSize)
  75 {
  76   q31_t *pState;                                 /* State pointer */
  77   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
  78   q31_t *px;                                     /* temporary state pointer */
  79   q31_t *pk;                                     /* temporary coefficient pointer */
  80   q31_t fcurr1, fnext1, gcurr1 = 0, gnext1;      /* temporary variables for first sample in loop unrolling */
  81   q31_t fcurr2, fnext2, gnext2;                  /* temporary variables for second sample in loop unrolling */
  82   uint32_t numStages = S->numStages;             /* Length of the filter */
  83   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
  84   q31_t k;
  85
  86   pState = &S->pState[0];
  87
  88   blkCnt = blockSize >> 1u;
  89
  90   /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.
  91      a second loop below computes the remaining 1 sample. */
  92   while(blkCnt > 0u)
  93   {
  94     /* f0(n) = x(n) */
  95     fcurr1 = *pSrc++;
  96
  97     /* f0(n) = x(n) */
  98     fcurr2 = *pSrc++;
  99
 100     /* Initialize coeff pointer */
 101     pk = (pCoeffs);
 102
 103     /* Initialize state pointer */
 104     px = pState;
 105
 106     /* read g0(n - 1) from state buffer */
 107     gcurr1 = *px;
 108
 109     /* Read the reflection coefficient */
 110     k = *pk++;
 111
 112     /* for sample 1 processing */
 113     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 114     fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
 115
 116     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 117     gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
 118     fnext1 = fcurr1 + (fnext1 << 1u);
 119     gnext1 = gcurr1 + (gnext1 << 1u);
 120
 121     /* for sample 1 processing */
 122     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 123     fnext2 = (q31_t) (((q63_t) fcurr1 * k) >> 32);
 124
 125     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 126     gnext2 = (q31_t) (((q63_t) fcurr2 * (k)) >> 32);
 127     fnext2 = fcurr2 + (fnext2 << 1u);
 128     gnext2 = fcurr1 + (gnext2 << 1u);
 129
 130     /* save g1(n) in state buffer */
 131     *px++ = fcurr2;
 132
 133     /* f1(n) is saved in fcurr1
 134        for next stage processing */
 135     fcurr1 = fnext1;
 136     fcurr2 = fnext2;
 137
 138     stageCnt = (numStages - 1u);
 139
 140     /* stage loop */
 141     while(stageCnt > 0u)
 142     {
 143
 144       /* Read the reflection coefficient */
 145       k = *pk++;
 146
 147       /* read g2(n) from state buffer */
 148       gcurr1 = *px;
 149
 150       /* save g1(n) in state buffer */
 151       *px++ = gnext2;
 152
 153       /* Sample processing for K2, K3.... */
 154       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 155       fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
 156       fnext2 = (q31_t) (((q63_t) gnext1 * k) >> 32);
 157
 158       fnext1 = fcurr1 + (fnext1 << 1u);
 159       fnext2 = fcurr2 + (fnext2 << 1u);
 160
 161       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 162       gnext2 = (q31_t) (((q63_t) fcurr2 * (k)) >> 32);
 163       gnext2 = gnext1 + (gnext2 << 1u);
 164
 165       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 166       gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
 167       gnext1 = gcurr1 + (gnext1 << 1u);
 168
 169       /* f1(n) is saved in fcurr1
 170          for next stage processing */
 171       fcurr1 = fnext1;
 172       fcurr2 = fnext2;
 173
 174       stageCnt--;
 175
 176     }
 177
 178     /* y(n) = fN(n) */
 179     *pDst++ = fcurr1;
 180     *pDst++ = fcurr2;
 181
 182     blkCnt--;
 183
 184   }
 185
 186   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 187    ** No loop unrolling is used. */
 188   blkCnt = blockSize % 0x2u;
 189
 190   while(blkCnt > 0u)
 191   {
 192     /* f0(n) = x(n) */
 193     fcurr1 = *pSrc++;
 194
 195     /* Initialize coeff pointer */
 196     pk = (pCoeffs);
 197
 198     /* Initialize state pointer */
 199     px = pState;
 200
 201     /* read g0(n - 1) from state buffer */
 202     gcurr1 = *px;
 203
 204     /* Read the reflection coefficient */
 205     k = *pk++;
 206
 207     /* for sample 1 processing */
 208     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 209     fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
 210     fnext1 = fcurr1 + (fnext1 << 1u);
 211
 212     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 213     gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
 214     gnext1 = gcurr1 + (gnext1 << 1u);
 215
 216     /* save g1(n) in state buffer */
 217     *px++ = fcurr1;
 218
 219     /* f1(n) is saved in fcurr1
 220        for next stage processing */
 221     fcurr1 = fnext1;
 222
 223     stageCnt = (numStages - 1u);
 224
 225     /* stage loop */
 226     while(stageCnt > 0u)
 227     {
 228       /* Read the reflection coefficient */
 229       k = *pk++;
 230
 231       /* read g2(n) from state buffer */
 232       gcurr1 = *px;
 233
 234       /* save g1(n) in state buffer */
 235       *px++ = gnext1;
 236
 237       /* Sample processing for K2, K3.... */
 238       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 239       fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
 240       fnext1 = fcurr1 + (fnext1 << 1u);
 241
 242       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 243       gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
 244       gnext1 = gcurr1 + (gnext1 << 1u);
 245
 246       /* f1(n) is saved in fcurr1
 247          for next stage processing */
 248       fcurr1 = fnext1;
 249
 250       stageCnt--;
 251
 252     }
 253
 254
 255     /* y(n) = fN(n) */
 256     *pDst++ = fcurr1;
 257
 258     blkCnt--;
 259
 260   }
 261
 262
 263 }
 264
 265
 266 #else
 267
 268 /* Run the below code for Cortex-M0 */
 269
 270 void arm_fir_lattice_q31(
 271   const arm_fir_lattice_instance_q31 * S,
 272   q31_t * pSrc,
 273   q31_t * pDst,
 274   uint32_t blockSize)
 275 {
 276   q31_t *pState;                                 /* State pointer */
 277   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
 278   q31_t *px;                                     /* temporary state pointer */
 279   q31_t *pk;                                     /* temporary coefficient pointer */
 280   q31_t fcurr, fnext, gcurr, gnext;              /* temporary variables */
 281   uint32_t numStages = S->numStages;             /* Length of the filter */
 282   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
 283
 284   pState = &S->pState[0];
 285
 286   blkCnt = blockSize;
 287
 288   while(blkCnt > 0u)
 289   {
 290     /* f0(n) = x(n) */
 291     fcurr = *pSrc++;
 292
 293     /* Initialize coeff pointer */
 294     pk = (pCoeffs);
 295
 296     /* Initialize state pointer */
 297     px = pState;
 298
 299     /* read g0(n-1) from state buffer */
 300     gcurr = *px;
 301
 302     /* for sample 1 processing */
 303     /* f1(n) = f0(n) +  K1 * g0(n-1) */
 304     fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
 305     /* g1(n) = f0(n) * K1  +  g0(n-1) */
 306     gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
 307     /* save g1(n) in state buffer */
 308     *px++ = fcurr;
 309
 310     /* f1(n) is saved in fcurr1
 311        for next stage processing */
 312     fcurr = fnext;
 313
 314     stageCnt = (numStages - 1u);
 315
 316     /* stage loop */
 317     while(stageCnt > 0u)
 318     {
 319       /* read g2(n) from state buffer */
 320       gcurr = *px;
 321
 322       /* save g1(n) in state buffer */
 323       *px++ = gnext;
 324
 325       /* Sample processing for K2, K3.... */
 326       /* f2(n) = f1(n) +  K2 * g1(n-1) */
 327       fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
 328       /* g2(n) = f1(n) * K2  +  g1(n-1) */
 329       gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
 330
 331       /* f1(n) is saved in fcurr1
 332          for next stage processing */
 333       fcurr = fnext;
 334
 335       stageCnt--;
 336
 337     }
 338
 339     /* y(n) = fN(n) */
 340     *pDst++ = fcurr;
 341
 342     blkCnt--;
 343
 344   }
 345
 346 }
 347
 348 #endif /*   #ifndef ARM_MATH_CM0_FAMILY */
 349
 350
 351 /**
 352  * @} end of FIR_Lattice group
 353  */