arm_fir_lattice_q31.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_fir_lattice_q31.c
00004  * Description:  Q31 FIR lattice filter processing function
00005  *
00006  * $Date:        27. January 2017
00007  * $Revision:    V.1.5.1
00008  *
00009  * Target Processor: Cortex-M cores
00010  * -------------------------------------------------------------------- */
00011 /*
00012  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
00013  *
00014  * SPDX-License-Identifier: Apache-2.0
00015  *
00016  * Licensed under the Apache License, Version 2.0 (the License); you may
00017  * not use this file except in compliance with the License.
00018  * You may obtain a copy of the License at
00019  *
00020  * www.apache.org/licenses/LICENSE-2.0
00021  *
00022  * Unless required by applicable law or agreed to in writing, software
00023  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00024  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00025  * See the License for the specific language governing permissions and
00026  * limitations under the License.
00027  */
00028 
00029 #include "arm_math.h"
00030 
00031 /**
00032  * @ingroup groupFilters
00033  */
00034 
00035 /**
00036  * @addtogroup FIR_Lattice
00037  * @{
00038  */
00039 
00040 
00041 /**
00042  * @brief Processing function for the Q31 FIR lattice filter.
00043  * @param[in]  *S        points to an instance of the Q31 FIR lattice structure.
00044  * @param[in]  *pSrc     points to the block of input data.
00045  * @param[out] *pDst     points to the block of output data
00046  * @param[in]  blockSize number of samples to process.
00047  * @return none.
00048  *
00049  * @details
00050  * <b>Scaling and Overflow Behavior:</b>
00051  * In order to avoid overflows the input signal must be scaled down by 2*log2(numStages) bits.
00052  */
00053 
00054 #if defined (ARM_MATH_DSP)
00055 
00056   /* Run the below code for Cortex-M4 and Cortex-M3 */
00057 
00058 void arm_fir_lattice_q31(
00059   const arm_fir_lattice_instance_q31 * S,
00060   q31_t * pSrc,
00061   q31_t * pDst,
00062   uint32_t blockSize)
00063 {
00064   q31_t *pState;                                 /* State pointer */
00065   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
00066   q31_t *px;                                     /* temporary state pointer */
00067   q31_t *pk;                                     /* temporary coefficient pointer */
00068   q31_t fcurr1, fnext1, gcurr1 = 0, gnext1;      /* temporary variables for first sample in loop unrolling */
00069   q31_t fcurr2, fnext2, gnext2;                  /* temporary variables for second sample in loop unrolling */
00070   uint32_t numStages = S->numStages;             /* Length of the filter */
00071   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
00072   q31_t k;
00073 
00074   pState = &S->pState[0];
00075 
00076   blkCnt = blockSize >> 1U;
00077 
00078   /* First part of the processing with loop unrolling.  Compute 2 outputs at a time.
00079      a second loop below computes the remaining 1 sample. */
00080   while (blkCnt > 0U)
00081   {
00082     /* f0(n) = x(n) */
00083     fcurr1 = *pSrc++;
00084 
00085     /* f0(n) = x(n) */
00086     fcurr2 = *pSrc++;
00087 
00088     /* Initialize coeff pointer */
00089     pk = (pCoeffs);
00090 
00091     /* Initialize state pointer */
00092     px = pState;
00093 
00094     /* read g0(n - 1) from state buffer */
00095     gcurr1 = *px;
00096 
00097     /* Read the reflection coefficient */
00098     k = *pk++;
00099 
00100     /* for sample 1 processing */
00101     /* f1(n) = f0(n) +  K1 * g0(n-1) */
00102     fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
00103 
00104     /* g1(n) = f0(n) * K1  +  g0(n-1) */
00105     gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
00106     fnext1 = fcurr1 + (fnext1 << 1U);
00107     gnext1 = gcurr1 + (gnext1 << 1U);
00108 
00109     /* for sample 1 processing */
00110     /* f1(n) = f0(n) +  K1 * g0(n-1) */
00111     fnext2 = (q31_t) (((q63_t) fcurr1 * k) >> 32);
00112 
00113     /* g1(n) = f0(n) * K1  +  g0(n-1) */
00114     gnext2 = (q31_t) (((q63_t) fcurr2 * (k)) >> 32);
00115     fnext2 = fcurr2 + (fnext2 << 1U);
00116     gnext2 = fcurr1 + (gnext2 << 1U);
00117 
00118     /* save g1(n) in state buffer */
00119     *px++ = fcurr2;
00120 
00121     /* f1(n) is saved in fcurr1
00122        for next stage processing */
00123     fcurr1 = fnext1;
00124     fcurr2 = fnext2;
00125 
00126     stageCnt = (numStages - 1U);
00127 
00128     /* stage loop */
00129     while (stageCnt > 0U)
00130     {
00131 
00132       /* Read the reflection coefficient */
00133       k = *pk++;
00134 
00135       /* read g2(n) from state buffer */
00136       gcurr1 = *px;
00137 
00138       /* save g1(n) in state buffer */
00139       *px++ = gnext2;
00140 
00141       /* Sample processing for K2, K3.... */
00142       /* f2(n) = f1(n) +  K2 * g1(n-1) */
00143       fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
00144       fnext2 = (q31_t) (((q63_t) gnext1 * k) >> 32);
00145 
00146       fnext1 = fcurr1 + (fnext1 << 1U);
00147       fnext2 = fcurr2 + (fnext2 << 1U);
00148 
00149       /* g2(n) = f1(n) * K2  +  g1(n-1) */
00150       gnext2 = (q31_t) (((q63_t) fcurr2 * (k)) >> 32);
00151       gnext2 = gnext1 + (gnext2 << 1U);
00152 
00153       /* g2(n) = f1(n) * K2  +  g1(n-1) */
00154       gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
00155       gnext1 = gcurr1 + (gnext1 << 1U);
00156 
00157       /* f1(n) is saved in fcurr1
00158          for next stage processing */
00159       fcurr1 = fnext1;
00160       fcurr2 = fnext2;
00161 
00162       stageCnt--;
00163 
00164     }
00165 
00166     /* y(n) = fN(n) */
00167     *pDst++ = fcurr1;
00168     *pDst++ = fcurr2;
00169 
00170     blkCnt--;
00171 
00172   }
00173 
00174   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
00175    ** No loop unrolling is used. */
00176   blkCnt = blockSize % 0x2U;
00177 
00178   while (blkCnt > 0U)
00179   {
00180     /* f0(n) = x(n) */
00181     fcurr1 = *pSrc++;
00182 
00183     /* Initialize coeff pointer */
00184     pk = (pCoeffs);
00185 
00186     /* Initialize state pointer */
00187     px = pState;
00188 
00189     /* read g0(n - 1) from state buffer */
00190     gcurr1 = *px;
00191 
00192     /* Read the reflection coefficient */
00193     k = *pk++;
00194 
00195     /* for sample 1 processing */
00196     /* f1(n) = f0(n) +  K1 * g0(n-1) */
00197     fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
00198     fnext1 = fcurr1 + (fnext1 << 1U);
00199 
00200     /* g1(n) = f0(n) * K1  +  g0(n-1) */
00201     gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
00202     gnext1 = gcurr1 + (gnext1 << 1U);
00203 
00204     /* save g1(n) in state buffer */
00205     *px++ = fcurr1;
00206 
00207     /* f1(n) is saved in fcurr1
00208        for next stage processing */
00209     fcurr1 = fnext1;
00210 
00211     stageCnt = (numStages - 1U);
00212 
00213     /* stage loop */
00214     while (stageCnt > 0U)
00215     {
00216       /* Read the reflection coefficient */
00217       k = *pk++;
00218 
00219       /* read g2(n) from state buffer */
00220       gcurr1 = *px;
00221 
00222       /* save g1(n) in state buffer */
00223       *px++ = gnext1;
00224 
00225       /* Sample processing for K2, K3.... */
00226       /* f2(n) = f1(n) +  K2 * g1(n-1) */
00227       fnext1 = (q31_t) (((q63_t) gcurr1 * k) >> 32);
00228       fnext1 = fcurr1 + (fnext1 << 1U);
00229 
00230       /* g2(n) = f1(n) * K2  +  g1(n-1) */
00231       gnext1 = (q31_t) (((q63_t) fcurr1 * (k)) >> 32);
00232       gnext1 = gcurr1 + (gnext1 << 1U);
00233 
00234       /* f1(n) is saved in fcurr1
00235          for next stage processing */
00236       fcurr1 = fnext1;
00237 
00238       stageCnt--;
00239 
00240     }
00241 
00242 
00243     /* y(n) = fN(n) */
00244     *pDst++ = fcurr1;
00245 
00246     blkCnt--;
00247 
00248   }
00249 
00250 
00251 }
00252 
00253 
00254 #else
00255 
00256 /* Run the below code for Cortex-M0 */
00257 
00258 void arm_fir_lattice_q31(
00259   const arm_fir_lattice_instance_q31 * S,
00260   q31_t * pSrc,
00261   q31_t * pDst,
00262   uint32_t blockSize)
00263 {
00264   q31_t *pState;                                 /* State pointer */
00265   q31_t *pCoeffs = S->pCoeffs;                   /* Coefficient pointer */
00266   q31_t *px;                                     /* temporary state pointer */
00267   q31_t *pk;                                     /* temporary coefficient pointer */
00268   q31_t fcurr, fnext, gcurr, gnext;              /* temporary variables */
00269   uint32_t numStages = S->numStages;             /* Length of the filter */
00270   uint32_t blkCnt, stageCnt;                     /* temporary variables for counts */
00271 
00272   pState = &S->pState[0];
00273 
00274   blkCnt = blockSize;
00275 
00276   while (blkCnt > 0U)
00277   {
00278     /* f0(n) = x(n) */
00279     fcurr = *pSrc++;
00280 
00281     /* Initialize coeff pointer */
00282     pk = (pCoeffs);
00283 
00284     /* Initialize state pointer */
00285     px = pState;
00286 
00287     /* read g0(n-1) from state buffer */
00288     gcurr = *px;
00289 
00290     /* for sample 1 processing */
00291     /* f1(n) = f0(n) +  K1 * g0(n-1) */
00292     fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
00293     /* g1(n) = f0(n) * K1  +  g0(n-1) */
00294     gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
00295     /* save g1(n) in state buffer */
00296     *px++ = fcurr;
00297 
00298     /* f1(n) is saved in fcurr1
00299        for next stage processing */
00300     fcurr = fnext;
00301 
00302     stageCnt = (numStages - 1U);
00303 
00304     /* stage loop */
00305     while (stageCnt > 0U)
00306     {
00307       /* read g2(n) from state buffer */
00308       gcurr = *px;
00309 
00310       /* save g1(n) in state buffer */
00311       *px++ = gnext;
00312 
00313       /* Sample processing for K2, K3.... */
00314       /* f2(n) = f1(n) +  K2 * g1(n-1) */
00315       fnext = (q31_t) (((q63_t) gcurr * (*pk)) >> 31) + fcurr;
00316       /* g2(n) = f1(n) * K2  +  g1(n-1) */
00317       gnext = (q31_t) (((q63_t) fcurr * (*pk++)) >> 31) + gcurr;
00318 
00319       /* f1(n) is saved in fcurr1
00320          for next stage processing */
00321       fcurr = fnext;
00322 
00323       stageCnt--;
00324 
00325     }
00326 
00327     /* y(n) = fN(n) */
00328     *pDst++ = fcurr;
00329 
00330     blkCnt--;
00331 
00332   }
00333 
00334 }
00335 
00336 #endif /*   #if defined (ARM_MATH_DSP) */
00337 
00338 
00339 /**
00340  * @} end of FIR_Lattice group
00341  */