1/*
2 * Copyright (C) 2004-2010 NXP Software
3 * Copyright (C) 2010 The Android Open Source Project
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 *      http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18#include "BIQUAD.h"
19#include "BQ_2I_D32F32Cll_TRC_WRA_01_Private.h"
20#include "LVM_Macros.h"
21
22/**************************************************************************
23 ASSUMPTIONS:
24 COEFS-
25 pBiquadState->coefs[0] is A2, pBiquadState->coefs[1] is A1
26 pBiquadState->coefs[2] is A0, pBiquadState->coefs[3] is -B2
27 pBiquadState->coefs[4] is -B1, these are in Q30 format
28
29 DELAYS-
30 pBiquadState->pDelays[0] is x(n-1)L in Q0 format
31 pBiquadState->pDelays[1] is x(n-1)R in Q0 format
32 pBiquadState->pDelays[2] is x(n-2)L in Q0 format
33 pBiquadState->pDelays[3] is x(n-2)R in Q0 format
34 pBiquadState->pDelays[4] is y(n-1)L in Q0 format
35 pBiquadState->pDelays[5] is y(n-1)R in Q0 format
36 pBiquadState->pDelays[6] is y(n-2)L in Q0 format
37 pBiquadState->pDelays[7] is y(n-2)R in Q0 format
38***************************************************************************/
39
40void BQ_2I_D32F32C30_TRC_WRA_01 (           Biquad_Instance_t       *pInstance,
41                                            LVM_INT32                    *pDataIn,
42                                            LVM_INT32                    *pDataOut,
43                                            LVM_INT16                    NrSamples)
44
45
46    {
47        LVM_INT32 ynL,ynR,templ,tempd;
48        LVM_INT16 ii;
49        PFilter_State pBiquadState = (PFilter_State) pInstance;
50
51         for (ii = NrSamples; ii != 0; ii--)
52         {
53
54
55            /**************************************************************************
56                            PROCESSING OF THE LEFT CHANNEL
57            ***************************************************************************/
58            /* ynL= ( A2 (Q30) * x(n-2)L (Q0) ) >>30 in Q0*/
59            MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[2],ynL,30)
60
61            /* ynL+= ( A1 (Q30) * x(n-1)L (Q0) ) >> 30 in Q0*/
62            MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[0],templ,30)
63            ynL+=templ;
64
65            /* ynL+= ( A0 (Q30) * x(n)L (Q0) ) >> 30 in Q0*/
66            MUL32x32INTO32(pBiquadState->coefs[2],*pDataIn,templ,30)
67            ynL+=templ;
68
69             /* ynL+= (-B2 (Q30) * y(n-2)L (Q0) ) >> 30 in Q0*/
70            MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[6],templ,30)
71            ynL+=templ;
72
73            /* ynL+= (-B1 (Q30) * y(n-1)L (Q0) ) >> 30 in Q0 */
74            MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[4],templ,30)
75            ynL+=templ;
76
77            /**************************************************************************
78                            PROCESSING OF THE RIGHT CHANNEL
79            ***************************************************************************/
80            /* ynR= ( A2 (Q30) * x(n-2)R (Q0) ) >> 30 in Q0*/
81            MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[3],ynR,30)
82
83            /* ynR+= ( A1 (Q30) * x(n-1)R (Q0) ) >> 30  in Q0*/
84            MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[1],templ,30)
85            ynR+=templ;
86
87            /* ynR+= ( A0 (Q30) * x(n)R (Q0) ) >> 30 in Q0*/
88            tempd=*(pDataIn+1);
89            MUL32x32INTO32(pBiquadState->coefs[2],tempd,templ,30)
90            ynR+=templ;
91
92            /* ynR+= (-B2 (Q30) * y(n-2)R (Q0) ) >> 30 in Q0*/
93            MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[7],templ,30)
94            ynR+=templ;
95
96            /* ynR+= (-B1 (Q30) * y(n-1)R (Q0) ) >> 30 in Q0 */
97            MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[5],templ,30)
98            ynR+=templ;
99
100            /**************************************************************************
101                            UPDATING THE DELAYS
102            ***************************************************************************/
103            pBiquadState->pDelays[7]=pBiquadState->pDelays[5]; /* y(n-2)R=y(n-1)R*/
104            pBiquadState->pDelays[6]=pBiquadState->pDelays[4]; /* y(n-2)L=y(n-1)L*/
105            pBiquadState->pDelays[3]=pBiquadState->pDelays[1]; /* x(n-2)R=x(n-1)R*/
106            pBiquadState->pDelays[2]=pBiquadState->pDelays[0]; /* x(n-2)L=x(n-1)L*/
107            pBiquadState->pDelays[5]=(LVM_INT32)ynR; /* Update y(n-1)R in Q0*/
108            pBiquadState->pDelays[4]=(LVM_INT32)ynL; /* Update y(n-1)L in Q0*/
109            pBiquadState->pDelays[0]=(*pDataIn); /* Update x(n-1)L in Q0*/
110            pDataIn++;
111            pBiquadState->pDelays[1]=(*pDataIn); /* Update x(n-1)R in Q0*/
112            pDataIn++;
113
114            /**************************************************************************
115                            WRITING THE OUTPUT
116            ***************************************************************************/
117            *pDataOut=(LVM_INT32)ynL; /* Write Left output in Q0*/
118            pDataOut++;
119            *pDataOut=(LVM_INT32)ynR; /* Write Right ouput in Q0*/
120            pDataOut++;
121
122
123        }
124
125    }
126
127