pigweed / third_party / github / STMicroelectronics / cmsis_core / be0e91fb4ba19a2ee427ba8b6b75b43eb02b2810 / . / DSP / Source / FilteringFunctions / arm_fir_lattice_f32.c

/* ---------------------------------------------------------------------- | |

* Project: CMSIS DSP Library | |

* Title: arm_fir_lattice_f32.c | |

* Description: Processing function for floating-point FIR Lattice filter | |

* | |

* $Date: 18. March 2019 | |

* $Revision: V1.6.0 | |

* | |

* Target Processor: Cortex-M cores | |

* -------------------------------------------------------------------- */ | |

/* | |

* Copyright (C) 2010-2019 ARM Limited or its affiliates. All rights reserved. | |

* | |

* SPDX-License-Identifier: Apache-2.0 | |

* | |

* Licensed under the Apache License, Version 2.0 (the License); you may | |

* not use this file except in compliance with the License. | |

* You may obtain a copy of the License at | |

* | |

* www.apache.org/licenses/LICENSE-2.0 | |

* | |

* Unless required by applicable law or agreed to in writing, software | |

* distributed under the License is distributed on an AS IS BASIS, WITHOUT | |

* WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | |

* See the License for the specific language governing permissions and | |

* limitations under the License. | |

*/ | |

#include "arm_math.h" | |

/** | |

@ingroup groupFilters | |

*/ | |

/** | |

@defgroup FIR_Lattice Finite Impulse Response (FIR) Lattice Filters | |

This set of functions implements Finite Impulse Response (FIR) lattice filters | |

for Q15, Q31 and floating-point data types. Lattice filters are used in a | |

variety of adaptive filter applications. The filter structure is feedforward and | |

the net impulse response is finite length. | |

The functions operate on blocks | |

of input and output data and each call to the function processes | |

<code>blockSize</code> samples through the filter. <code>pSrc</code> and | |

<code>pDst</code> point to input and output arrays containing <code>blockSize</code> values. | |

@par Algorithm | |

\image html FIRLattice.gif "Finite Impulse Response Lattice filter" | |

The following difference equation is implemented: | |

@par | |

<pre> | |

f0[n] = g0[n] = x[n] | |

fm[n] = fm-1[n] + km * gm-1[n-1] for m = 1, 2, ...M | |

gm[n] = km * fm-1[n] + gm-1[n-1] for m = 1, 2, ...M | |

y[n] = fM[n] | |

</pre> | |

@par | |

<code>pCoeffs</code> points to tha array of reflection coefficients of size <code>numStages</code>. | |

Reflection Coefficients are stored in the following order. | |

@par | |

<pre> | |

{k1, k2, ..., kM} | |

</pre> | |

where M is number of stages | |

@par | |

<code>pState</code> points to a state array of size <code>numStages</code>. | |

The state variables (g values) hold previous inputs and are stored in the following order. | |

<pre> | |

{g0[n], g1[n], g2[n] ...gM-1[n]} | |

</pre> | |

The state variables are updated after each block of data is processed; the coefficients are untouched. | |

@par Instance Structure | |

The coefficients and state variables for a filter are stored together in an instance data structure. | |

A separate instance structure must be defined for each filter. | |

Coefficient arrays may be shared among several instances while state variable arrays cannot be shared. | |

There are separate instance structure declarations for each of the 3 supported data types. | |

@par Initialization Functions | |

There is also an associated initialization function for each data type. | |

The initialization function performs the following operations: | |

- Sets the values of the internal structure fields. | |

- Zeros out the values in the state buffer. | |

To do this manually without calling the init function, assign the follow subfields of the instance structure: | |

numStages, pCoeffs, pState. Also set all of the values in pState to zero. | |

@par | |

Use of the initialization function is optional. | |

However, if the initialization function is used, then the instance structure cannot be placed into a const data section. | |

To place an instance structure into a const data section, the instance structure must be manually initialized. | |

Set the values in the state buffer to zeros and then manually initialize the instance structure as follows: | |

<pre> | |

arm_fir_lattice_instance_f32 S = {numStages, pState, pCoeffs}; | |

arm_fir_lattice_instance_q31 S = {numStages, pState, pCoeffs}; | |

arm_fir_lattice_instance_q15 S = {numStages, pState, pCoeffs}; | |

</pre> | |

@par | |

where <code>numStages</code> is the number of stages in the filter; | |

<code>pState</code> is the address of the state buffer; | |

<code>pCoeffs</code> is the address of the coefficient buffer. | |

@par Fixed-Point Behavior | |

Care must be taken when using the fixed-point versions of the FIR Lattice filter functions. | |

In particular, the overflow and saturation behavior of the accumulator used in each function must be considered. | |

Refer to the function specific documentation below for usage guidelines. | |

*/ | |

/** | |

@addtogroup FIR_Lattice | |

@{ | |

*/ | |

/** | |

@brief Processing function for the floating-point FIR lattice filter. | |

@param[in] S points to an instance of the floating-point FIR lattice structure | |

@param[in] pSrc points to the block of input data | |

@param[out] pDst points to the block of output data | |

@param[in] blockSize number of samples to process | |

@return none | |

*/ | |

void arm_fir_lattice_f32( | |

const arm_fir_lattice_instance_f32 * S, | |

const float32_t * pSrc, | |

float32_t * pDst, | |

uint32_t blockSize) | |

{ | |

float32_t *pState = S->pState; /* State pointer */ | |

const float32_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */ | |

float32_t *px; /* Temporary state pointer */ | |

const float32_t *pk; /* Temporary coefficient pointer */ | |

uint32_t numStages = S->numStages; /* Number of stages in the filter */ | |

uint32_t blkCnt, stageCnt; /* Loop counters */ | |

float32_t fcurr0, fnext0, gnext0, gcurr0; /* Temporary variables */ | |

#if defined (ARM_MATH_LOOPUNROLL) | |

float32_t fcurr1, fnext1, gnext1; /* Temporary variables for second sample in loop unrolling */ | |

float32_t fcurr2, fnext2, gnext2; /* Temporary variables for third sample in loop unrolling */ | |

float32_t fcurr3, fnext3, gnext3; /* Temporary variables for fourth sample in loop unrolling */ | |

#endif | |

gcurr0 = 0.0f; | |

#if defined (ARM_MATH_LOOPUNROLL) | |

/* Loop unrolling: Compute 4 outputs at a time */ | |

blkCnt = blockSize >> 2U; | |

while (blkCnt > 0U) | |

{ | |

/* Read two samples from input buffer */ | |

/* f0(n) = x(n) */ | |

fcurr0 = *pSrc++; | |

fcurr1 = *pSrc++; | |

/* Initialize state pointer */ | |

px = pState; | |

/* Initialize coeff pointer */ | |

pk = pCoeffs; | |

/* Read g0(n-1) from state buffer */ | |

gcurr0 = *px; | |

/* Process first sample for first tap */ | |

/* f1(n) = f0(n) + K1 * g0(n-1) */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

/* g1(n) = f0(n) * K1 + g0(n-1) */ | |

gnext0 = (fcurr0 * (*pk)) + gcurr0; | |

/* Process second sample for first tap */ | |

fnext1 = (fcurr0 * (*pk)) + fcurr1; | |

gnext1 = (fcurr1 * (*pk)) + fcurr0; | |

/* Read next two samples from input buffer */ | |

/* f0(n+2) = x(n+2) */ | |

fcurr2 = *pSrc++; | |

fcurr3 = *pSrc++; | |

/* Process third sample for first tap */ | |

fnext2 = (fcurr1 * (*pk)) + fcurr2; | |

gnext2 = (fcurr2 * (*pk)) + fcurr1; | |

/* Process fourth sample for first tap */ | |

fnext3 = (fcurr2 * (*pk )) + fcurr3; | |

gnext3 = (fcurr3 * (*pk++)) + fcurr2; | |

/* Copy only last input sample into the state buffer | |

which will be used for next samples processing */ | |

*px++ = fcurr3; | |

/* Update of f values for next coefficient set processing */ | |

fcurr0 = fnext0; | |

fcurr1 = fnext1; | |

fcurr2 = fnext2; | |

fcurr3 = fnext3; | |

/* Loop unrolling. Process 4 taps at a time . */ | |

stageCnt = (numStages - 1U) >> 2U; | |

/* Loop over the number of taps. Unroll by a factor of 4. | |

Repeat until we've computed numStages-3 coefficients. */ | |

/* Process 2nd, 3rd, 4th and 5th taps ... here */ | |

while (stageCnt > 0U) | |

{ | |

/* Read g1(n-1), g3(n-1) .... from state */ | |

gcurr0 = *px; | |

/* save g1(n) in state buffer */ | |

*px++ = gnext3; | |

/* Process first sample for 2nd, 6th .. tap */ | |

/* Sample processing for K2, K6.... */ | |

/* f2(n) = f1(n) + K2 * g1(n-1) */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

/* Process second sample for 2nd, 6th .. tap */ | |

/* for sample 2 processing */ | |

fnext1 = (gnext0 * (*pk)) + fcurr1; | |

/* Process third sample for 2nd, 6th .. tap */ | |

fnext2 = (gnext1 * (*pk)) + fcurr2; | |

/* Process fourth sample for 2nd, 6th .. tap */ | |

fnext3 = (gnext2 * (*pk)) + fcurr3; | |

/* g2(n) = f1(n) * K2 + g1(n-1) */ | |

/* Calculation of state values for next stage */ | |

gnext3 = (fcurr3 * (*pk)) + gnext2; | |

gnext2 = (fcurr2 * (*pk)) + gnext1; | |

gnext1 = (fcurr1 * (*pk)) + gnext0; | |

gnext0 = (fcurr0 * (*pk++)) + gcurr0; | |

/* Read g2(n-1), g4(n-1) .... from state */ | |

gcurr0 = *px; | |

/* save g2(n) in state buffer */ | |

*px++ = gnext3; | |

/* Sample processing for K3, K7.... */ | |

/* Process first sample for 3rd, 7th .. tap */ | |

/* f3(n) = f2(n) + K3 * g2(n-1) */ | |

fcurr0 = (gcurr0 * (*pk)) + fnext0; | |

/* Process second sample for 3rd, 7th .. tap */ | |

fcurr1 = (gnext0 * (*pk)) + fnext1; | |

/* Process third sample for 3rd, 7th .. tap */ | |

fcurr2 = (gnext1 * (*pk)) + fnext2; | |

/* Process fourth sample for 3rd, 7th .. tap */ | |

fcurr3 = (gnext2 * (*pk)) + fnext3; | |

/* Calculation of state values for next stage */ | |

/* g3(n) = f2(n) * K3 + g2(n-1) */ | |

gnext3 = (fnext3 * (*pk)) + gnext2; | |

gnext2 = (fnext2 * (*pk)) + gnext1; | |

gnext1 = (fnext1 * (*pk)) + gnext0; | |

gnext0 = (fnext0 * (*pk++)) + gcurr0; | |

/* Read g1(n-1), g3(n-1) .... from state */ | |

gcurr0 = *px; | |

/* save g3(n) in state buffer */ | |

*px++ = gnext3; | |

/* Sample processing for K4, K8.... */ | |

/* Process first sample for 4th, 8th .. tap */ | |

/* f4(n) = f3(n) + K4 * g3(n-1) */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

/* Process second sample for 4th, 8th .. tap */ | |

/* for sample 2 processing */ | |

fnext1 = (gnext0 * (*pk)) + fcurr1; | |

/* Process third sample for 4th, 8th .. tap */ | |

fnext2 = (gnext1 * (*pk)) + fcurr2; | |

/* Process fourth sample for 4th, 8th .. tap */ | |

fnext3 = (gnext2 * (*pk)) + fcurr3; | |

/* g4(n) = f3(n) * K4 + g3(n-1) */ | |

/* Calculation of state values for next stage */ | |

gnext3 = (fcurr3 * (*pk)) + gnext2; | |

gnext2 = (fcurr2 * (*pk)) + gnext1; | |

gnext1 = (fcurr1 * (*pk)) + gnext0; | |

gnext0 = (fcurr0 * (*pk++)) + gcurr0; | |

/* Read g2(n-1), g4(n-1) .... from state */ | |

gcurr0 = *px; | |

/* save g4(n) in state buffer */ | |

*px++ = gnext3; | |

/* Sample processing for K5, K9.... */ | |

/* Process first sample for 5th, 9th .. tap */ | |

/* f5(n) = f4(n) + K5 * g4(n-1) */ | |

fcurr0 = (gcurr0 * (*pk)) + fnext0; | |

/* Process second sample for 5th, 9th .. tap */ | |

fcurr1 = (gnext0 * (*pk)) + fnext1; | |

/* Process third sample for 5th, 9th .. tap */ | |

fcurr2 = (gnext1 * (*pk)) + fnext2; | |

/* Process fourth sample for 5th, 9th .. tap */ | |

fcurr3 = (gnext2 * (*pk)) + fnext3; | |

/* Calculation of state values for next stage */ | |

/* g5(n) = f4(n) * K5 + g4(n-1) */ | |

gnext3 = (fnext3 * (*pk)) + gnext2; | |

gnext2 = (fnext2 * (*pk)) + gnext1; | |

gnext1 = (fnext1 * (*pk)) + gnext0; | |

gnext0 = (fnext0 * (*pk++)) + gcurr0; | |

stageCnt--; | |

} | |

/* If the (filter length -1) is not a multiple of 4, compute the remaining filter taps */ | |

stageCnt = (numStages - 1U) % 0x4U; | |

while (stageCnt > 0U) | |

{ | |

gcurr0 = *px; | |

/* save g value in state buffer */ | |

*px++ = gnext3; | |

/* Process four samples for last three taps here */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

fnext1 = (gnext0 * (*pk)) + fcurr1; | |

fnext2 = (gnext1 * (*pk)) + fcurr2; | |

fnext3 = (gnext2 * (*pk)) + fcurr3; | |

/* g1(n) = f0(n) * K1 + g0(n-1) */ | |

gnext3 = (fcurr3 * (*pk)) + gnext2; | |

gnext2 = (fcurr2 * (*pk)) + gnext1; | |

gnext1 = (fcurr1 * (*pk)) + gnext0; | |

gnext0 = (fcurr0 * (*pk++)) + gcurr0; | |

/* Update of f values for next coefficient set processing */ | |

fcurr0 = fnext0; | |

fcurr1 = fnext1; | |

fcurr2 = fnext2; | |

fcurr3 = fnext3; | |

stageCnt--; | |

} | |

/* The results in the 4 accumulators, store in the destination buffer. */ | |

/* y(n) = fN(n) */ | |

*pDst++ = fcurr0; | |

*pDst++ = fcurr1; | |

*pDst++ = fcurr2; | |

*pDst++ = fcurr3; | |

blkCnt--; | |

} | |

/* Loop unrolling: Compute remaining outputs */ | |

blkCnt = blockSize % 0x4U; | |

#else | |

/* Initialize blkCnt with number of samples */ | |

blkCnt = blockSize; | |

#endif /* #if defined (ARM_MATH_LOOPUNROLL) */ | |

while (blkCnt > 0U) | |

{ | |

/* f0(n) = x(n) */ | |

fcurr0 = *pSrc++; | |

/* Initialize state pointer */ | |

px = pState; | |

/* Initialize coeff pointer */ | |

pk = pCoeffs; | |

/* read g2(n) from state buffer */ | |

gcurr0 = *px; | |

/* for sample 1 processing */ | |

/* f1(n) = f0(n) + K1 * g0(n-1) */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

/* g1(n) = f0(n) * K1 + g0(n-1) */ | |

gnext0 = (fcurr0 * (*pk++)) + gcurr0; | |

/* save g1(n) in state buffer */ | |

*px++ = fcurr0; | |

/* f1(n) is saved in fcurr0 for next stage processing */ | |

fcurr0 = fnext0; | |

stageCnt = (numStages - 1U); | |

/* stage loop */ | |

while (stageCnt > 0U) | |

{ | |

/* read g2(n) from state buffer */ | |

gcurr0 = *px; | |

/* save g1(n) in state buffer */ | |

*px++ = gnext0; | |

/* Sample processing for K2, K3.... */ | |

/* f2(n) = f1(n) + K2 * g1(n-1) */ | |

fnext0 = (gcurr0 * (*pk)) + fcurr0; | |

/* g2(n) = f1(n) * K2 + g1(n-1) */ | |

gnext0 = (fcurr0 * (*pk++)) + gcurr0; | |

/* f1(n) is saved in fcurr0 for next stage processing */ | |

fcurr0 = fnext0; | |

stageCnt--; | |

} | |

/* y(n) = fN(n) */ | |

*pDst++ = fcurr0; | |

blkCnt--; | |

} | |

} | |

/** | |

@} end of FIR_Lattice group | |

*/ |