arm_fir_q7.c

00001 /* ----------------------------------------------------------------------
00002  * Project:      CMSIS DSP Library
00003  * Title:        arm_fir_q7.c
00004  * Description:  Q7 FIR 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
00037  * @{
00038  */
00039 
00040 /**
00041  * @param[in]   *S points to an instance of the Q7 FIR filter structure.
00042  * @param[in]   *pSrc points to the block of input data.
00043  * @param[out]  *pDst points to the block of output data.
00044  * @param[in]   blockSize number of samples to process per call.
00045  * @return  none.
00046  *
00047  * <b>Scaling and Overflow Behavior:</b>
00048  * \par
00049  * The function is implemented using a 32-bit internal accumulator.
00050  * Both coefficients and state variables are represented in 1.7 format and multiplications yield a 2.14 result.
00051  * The 2.14 intermediate results are accumulated in a 32-bit accumulator in 18.14 format.
00052  * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
00053  * The accumulator is converted to 18.7 format by discarding the low 7 bits.
00054  * Finally, the result is truncated to 1.7 format.
00055  */
00056 
00057 void arm_fir_q7(
00058   const arm_fir_instance_q7 * S,
00059   q7_t * pSrc,
00060   q7_t * pDst,
00061   uint32_t blockSize)
00062 {
00063 
00064 #if defined (ARM_MATH_DSP)
00065 
00066   /* Run the below code for Cortex-M4 and Cortex-M3 */
00067 
00068   q7_t *pState = S->pState;                      /* State pointer */
00069   q7_t *pCoeffs = S->pCoeffs;                    /* Coefficient pointer */
00070   q7_t *pStateCurnt;                             /* Points to the current sample of the state */
00071   q7_t x0, x1, x2, x3;                           /* Temporary variables to hold state */
00072   q7_t c0;                                       /* Temporary variable to hold coefficient value */
00073   q7_t *px;                                      /* Temporary pointer for state */
00074   q7_t *pb;                                      /* Temporary pointer for coefficient buffer */
00075   q31_t acc0, acc1, acc2, acc3;                  /* Accumulators */
00076   uint32_t numTaps = S->numTaps;                 /* Number of filter coefficients in the filter */
00077   uint32_t i, tapCnt, blkCnt;                    /* Loop counters */
00078 
00079   /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
00080   /* pStateCurnt points to the location where the new input data should be written */
00081   pStateCurnt = &(S->pState[(numTaps - 1U)]);
00082 
00083   /* Apply loop unrolling and compute 4 output values simultaneously.
00084    * The variables acc0 ... acc3 hold output values that are being computed:
00085    *
00086    *    acc0 =  b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0]
00087    *    acc1 =  b[numTaps-1] * x[n-numTaps] +   b[numTaps-2] * x[n-numTaps-1] + b[numTaps-3] * x[n-numTaps-2] +...+ b[0] * x[1]
00088    *    acc2 =  b[numTaps-1] * x[n-numTaps+1] + b[numTaps-2] * x[n-numTaps] +   b[numTaps-3] * x[n-numTaps-1] +...+ b[0] * x[2]
00089    *    acc3 =  b[numTaps-1] * x[n-numTaps+2] + b[numTaps-2] * x[n-numTaps+1] + b[numTaps-3] * x[n-numTaps]   +...+ b[0] * x[3]
00090    */
00091   blkCnt = blockSize >> 2;
00092 
00093   /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
00094    ** a second loop below computes the remaining 1 to 3 samples. */
00095   while (blkCnt > 0U)
00096   {
00097     /* Copy four new input samples into the state buffer */
00098     *pStateCurnt++ = *pSrc++;
00099     *pStateCurnt++ = *pSrc++;
00100     *pStateCurnt++ = *pSrc++;
00101     *pStateCurnt++ = *pSrc++;
00102 
00103     /* Set all accumulators to zero */
00104     acc0 = 0;
00105     acc1 = 0;
00106     acc2 = 0;
00107     acc3 = 0;
00108 
00109     /* Initialize state pointer */
00110     px = pState;
00111 
00112     /* Initialize coefficient pointer */
00113     pb = pCoeffs;
00114 
00115     /* Read the first three samples from the state buffer:
00116      *  x[n-numTaps], x[n-numTaps-1], x[n-numTaps-2] */
00117     x0 = *(px++);
00118     x1 = *(px++);
00119     x2 = *(px++);
00120 
00121     /* Loop unrolling.  Process 4 taps at a time. */
00122     tapCnt = numTaps >> 2;
00123     i = tapCnt;
00124 
00125     while (i > 0U)
00126     {
00127       /* Read the b[numTaps] coefficient */
00128       c0 = *pb;
00129 
00130       /* Read x[n-numTaps-3] sample */
00131       x3 = *px;
00132 
00133       /* acc0 +=  b[numTaps] * x[n-numTaps] */
00134       acc0 += ((q15_t) x0 * c0);
00135 
00136       /* acc1 +=  b[numTaps] * x[n-numTaps-1] */
00137       acc1 += ((q15_t) x1 * c0);
00138 
00139       /* acc2 +=  b[numTaps] * x[n-numTaps-2] */
00140       acc2 += ((q15_t) x2 * c0);
00141 
00142       /* acc3 +=  b[numTaps] * x[n-numTaps-3] */
00143       acc3 += ((q15_t) x3 * c0);
00144 
00145       /* Read the b[numTaps-1] coefficient */
00146       c0 = *(pb + 1U);
00147 
00148       /* Read x[n-numTaps-4] sample */
00149       x0 = *(px + 1U);
00150 
00151       /* Perform the multiply-accumulates */
00152       acc0 += ((q15_t) x1 * c0);
00153       acc1 += ((q15_t) x2 * c0);
00154       acc2 += ((q15_t) x3 * c0);
00155       acc3 += ((q15_t) x0 * c0);
00156 
00157       /* Read the b[numTaps-2] coefficient */
00158       c0 = *(pb + 2U);
00159 
00160       /* Read x[n-numTaps-5] sample */
00161       x1 = *(px + 2U);
00162 
00163       /* Perform the multiply-accumulates */
00164       acc0 += ((q15_t) x2 * c0);
00165       acc1 += ((q15_t) x3 * c0);
00166       acc2 += ((q15_t) x0 * c0);
00167       acc3 += ((q15_t) x1 * c0);
00168 
00169       /* Read the b[numTaps-3] coefficients */
00170       c0 = *(pb + 3U);
00171 
00172       /* Read x[n-numTaps-6] sample */
00173       x2 = *(px + 3U);
00174 
00175       /* Perform the multiply-accumulates */
00176       acc0 += ((q15_t) x3 * c0);
00177       acc1 += ((q15_t) x0 * c0);
00178       acc2 += ((q15_t) x1 * c0);
00179       acc3 += ((q15_t) x2 * c0);
00180 
00181       /* update coefficient pointer */
00182       pb += 4U;
00183       px += 4U;
00184 
00185       /* Decrement the loop counter */
00186       i--;
00187     }
00188 
00189     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
00190 
00191     i = numTaps - (tapCnt * 4U);
00192     while (i > 0U)
00193     {
00194       /* Read coefficients */
00195       c0 = *(pb++);
00196 
00197       /* Fetch 1 state variable */
00198       x3 = *(px++);
00199 
00200       /* Perform the multiply-accumulates */
00201       acc0 += ((q15_t) x0 * c0);
00202       acc1 += ((q15_t) x1 * c0);
00203       acc2 += ((q15_t) x2 * c0);
00204       acc3 += ((q15_t) x3 * c0);
00205 
00206       /* Reuse the present sample states for next sample */
00207       x0 = x1;
00208       x1 = x2;
00209       x2 = x3;
00210 
00211       /* Decrement the loop counter */
00212       i--;
00213     }
00214 
00215     /* Advance the state pointer by 4 to process the next group of 4 samples */
00216     pState = pState + 4;
00217 
00218     /* The results in the 4 accumulators are in 2.62 format.  Convert to 1.31
00219      ** Then store the 4 outputs in the destination buffer. */
00220     acc0 = __SSAT((acc0 >> 7U), 8);
00221     *pDst++ = acc0;
00222     acc1 = __SSAT((acc1 >> 7U), 8);
00223     *pDst++ = acc1;
00224     acc2 = __SSAT((acc2 >> 7U), 8);
00225     *pDst++ = acc2;
00226     acc3 = __SSAT((acc3 >> 7U), 8);
00227     *pDst++ = acc3;
00228 
00229     /* Decrement the samples loop counter */
00230     blkCnt--;
00231   }
00232 
00233 
00234   /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
00235    ** No loop unrolling is used. */
00236   blkCnt = blockSize % 4U;
00237 
00238   while (blkCnt > 0U)
00239   {
00240     /* Copy one sample at a time into state buffer */
00241     *pStateCurnt++ = *pSrc++;
00242 
00243     /* Set the accumulator to zero */
00244     acc0 = 0;
00245 
00246     /* Initialize state pointer */
00247     px = pState;
00248 
00249     /* Initialize Coefficient pointer */
00250     pb = (pCoeffs);
00251 
00252     i = numTaps;
00253 
00254     /* Perform the multiply-accumulates */
00255     do
00256     {
00257       acc0 += (q15_t) * (px++) * (*(pb++));
00258       i--;
00259     } while (i > 0U);
00260 
00261     /* The result is in 2.14 format.  Convert to 1.7
00262      ** Then store the output in the destination buffer. */
00263     *pDst++ = __SSAT((acc0 >> 7U), 8);
00264 
00265     /* Advance state pointer by 1 for the next sample */
00266     pState = pState + 1;
00267 
00268     /* Decrement the samples loop counter */
00269     blkCnt--;
00270   }
00271 
00272   /* Processing is complete.
00273    ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
00274    ** This prepares the state buffer for the next function call. */
00275 
00276   /* Points to the start of the state buffer */
00277   pStateCurnt = S->pState;
00278 
00279   tapCnt = (numTaps - 1U) >> 2U;
00280 
00281   /* copy data */
00282   while (tapCnt > 0U)
00283   {
00284     *pStateCurnt++ = *pState++;
00285     *pStateCurnt++ = *pState++;
00286     *pStateCurnt++ = *pState++;
00287     *pStateCurnt++ = *pState++;
00288 
00289     /* Decrement the loop counter */
00290     tapCnt--;
00291   }
00292 
00293   /* Calculate remaining number of copies */
00294   tapCnt = (numTaps - 1U) % 0x4U;
00295 
00296   /* Copy the remaining q31_t data */
00297   while (tapCnt > 0U)
00298   {
00299     *pStateCurnt++ = *pState++;
00300 
00301     /* Decrement the loop counter */
00302     tapCnt--;
00303   }
00304 
00305 #else
00306 
00307 /* Run the below code for Cortex-M0 */
00308 
00309   uint32_t numTaps = S->numTaps;                 /* Number of taps in the filter */
00310   uint32_t i, blkCnt;                            /* Loop counters */
00311   q7_t *pState = S->pState;                      /* State pointer */
00312   q7_t *pCoeffs = S->pCoeffs;                    /* Coefficient pointer */
00313   q7_t *px, *pb;                                 /* Temporary pointers to state and coeff */
00314   q31_t acc = 0;                                 /* Accumlator */
00315   q7_t *pStateCurnt;                             /* Points to the current sample of the state */
00316 
00317 
00318   /* S->pState points to state array which contains previous frame (numTaps - 1) samples */
00319   /* pStateCurnt points to the location where the new input data should be written */
00320   pStateCurnt = S->pState + (numTaps - 1U);
00321 
00322   /* Initialize blkCnt with blockSize */
00323   blkCnt = blockSize;
00324 
00325   /* Perform filtering upto BlockSize - BlockSize%4  */
00326   while (blkCnt > 0U)
00327   {
00328     /* Copy one sample at a time into state buffer */
00329     *pStateCurnt++ = *pSrc++;
00330 
00331     /* Set accumulator to zero */
00332     acc = 0;
00333 
00334     /* Initialize state pointer of type q7 */
00335     px = pState;
00336 
00337     /* Initialize coeff pointer of type q7 */
00338     pb = pCoeffs;
00339 
00340 
00341     i = numTaps;
00342 
00343     while (i > 0U)
00344     {
00345       /* acc =  b[numTaps-1] * x[n-numTaps-1] + b[numTaps-2] * x[n-numTaps-2] + b[numTaps-3] * x[n-numTaps-3] +...+ b[0] * x[0] */
00346       acc += (q15_t) * px++ * *pb++;
00347       i--;
00348     }
00349 
00350     /* Store the 1.7 format filter output in destination buffer */
00351     *pDst++ = (q7_t) __SSAT((acc >> 7), 8);
00352 
00353     /* Advance the state pointer by 1 to process the next sample */
00354     pState = pState + 1;
00355 
00356     /* Decrement the loop counter */
00357     blkCnt--;
00358   }
00359 
00360   /* Processing is complete.
00361    ** Now copy the last numTaps - 1 samples to the satrt of the state buffer.
00362    ** This prepares the state buffer for the next function call. */
00363 
00364 
00365   /* Points to the start of the state buffer */
00366   pStateCurnt = S->pState;
00367 
00368 
00369   /* Copy numTaps number of values */
00370   i = (numTaps - 1U);
00371 
00372   /* Copy q7_t data */
00373   while (i > 0U)
00374   {
00375     *pStateCurnt++ = *pState++;
00376     i--;
00377   }
00378 
00379 #endif /*   #if defined (ARM_MATH_DSP) */
00380 
00381 }
00382 
00383 /**
00384  * @} end of FIR group
00385  */