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 "PK_2I_D32F32CssGss_TRC_WRA_01_Private.h"
20 #include "LVM_Macros.h"
21
22 /**************************************************************************
23 ASSUMPTIONS:
24 COEFS-
25 pBiquadState->coefs[0] is A0,
26 pBiquadState->coefs[1] is -B2,
27 pBiquadState->coefs[2] is -B1, these are in Q14 format
28 pBiquadState->coefs[3] is Gain, in Q11 format
29
30 DELAYS-
31 pBiquadState->pDelays[0] is x(n-1)L in Q0 format
32 pBiquadState->pDelays[1] is x(n-1)R in Q0 format
33 pBiquadState->pDelays[2] is x(n-2)L in Q0 format
34 pBiquadState->pDelays[3] is x(n-2)R in Q0 format
35 pBiquadState->pDelays[4] is y(n-1)L in Q0 format
36 pBiquadState->pDelays[5] is y(n-1)R in Q0 format
37 pBiquadState->pDelays[6] is y(n-2)L in Q0 format
38 pBiquadState->pDelays[7] is y(n-2)R in Q0 format
39 ***************************************************************************/
PK_2I_D32F32C14G11_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrSamples)40 void PK_2I_D32F32C14G11_TRC_WRA_01 ( Biquad_FLOAT_Instance_t *pInstance,
41 LVM_FLOAT *pDataIn,
42 LVM_FLOAT *pDataOut,
43 LVM_INT16 NrSamples)
44 {
45 LVM_FLOAT ynL,ynR,ynLO,ynRO,templ;
46 LVM_INT16 ii;
47 PFilter_State_Float pBiquadState = (PFilter_State_Float) pInstance;
48
49 for (ii = NrSamples; ii != 0; ii--)
50 {
51
52 /**************************************************************************
53 PROCESSING OF THE LEFT CHANNEL
54 ***************************************************************************/
55 /* ynL= (A0 * (x(n)L - x(n-2)L ) )*/
56 templ = (*pDataIn) - pBiquadState->pDelays[2];
57 ynL = templ * pBiquadState->coefs[0];
58
59 /* ynL+= ((-B2 * y(n-2)L )) */
60 templ = pBiquadState->pDelays[6] * pBiquadState->coefs[1];
61 ynL += templ;
62
63 /* ynL+= ((-B1 * y(n-1)L ) ) */
64 templ = pBiquadState->pDelays[4] * pBiquadState->coefs[2];
65 ynL += templ;
66
67 /* ynLO= ((Gain * ynL )) */
68 ynLO = ynL * pBiquadState->coefs[3];
69
70 /* ynLO=( ynLO + x(n)L )*/
71 ynLO += (*pDataIn);
72
73 /**************************************************************************
74 PROCESSING OF THE RIGHT CHANNEL
75 ***************************************************************************/
76 /* ynR= (A0 * (x(n)R - x(n-2)R ) ) */
77 templ = (*(pDataIn + 1)) - pBiquadState->pDelays[3];
78 ynR = templ * pBiquadState->coefs[0];
79
80 /* ynR+= ((-B2 * y(n-2)R ) ) */
81 templ = pBiquadState->pDelays[7] * pBiquadState->coefs[1];
82 ynR += templ;
83
84 /* ynR+= ((-B1 * y(n-1)R ) ) */
85 templ = pBiquadState->pDelays[5] * pBiquadState->coefs[2];
86 ynR += templ;
87
88 /* ynRO= ((Gain * ynR )) */
89 ynRO = ynR * pBiquadState->coefs[3];
90
91 /* ynRO=( ynRO + x(n)R )*/
92 ynRO += (*(pDataIn+1));
93
94 /**************************************************************************
95 UPDATING THE DELAYS
96 ***************************************************************************/
97 pBiquadState->pDelays[7] = pBiquadState->pDelays[5]; /* y(n-2)R=y(n-1)R*/
98 pBiquadState->pDelays[6] = pBiquadState->pDelays[4]; /* y(n-2)L=y(n-1)L*/
99 pBiquadState->pDelays[3] = pBiquadState->pDelays[1]; /* x(n-2)R=x(n-1)R*/
100 pBiquadState->pDelays[2] = pBiquadState->pDelays[0]; /* x(n-2)L=x(n-1)L*/
101 pBiquadState->pDelays[5] = ynR; /* Update y(n-1)R */
102 pBiquadState->pDelays[4] = ynL; /* Update y(n-1)L */
103 pBiquadState->pDelays[0] = (*pDataIn); /* Update x(n-1)L */
104 pDataIn++;
105 pBiquadState->pDelays[1] = (*pDataIn); /* Update x(n-1)R */
106 pDataIn++;
107
108 /**************************************************************************
109 WRITING THE OUTPUT
110 ***************************************************************************/
111 *pDataOut = ynLO; /* Write Left output*/
112 pDataOut++;
113 *pDataOut = ynRO; /* Write Right ouput*/
114 pDataOut++;
115
116 }
117
118 }
119
120 #ifdef SUPPORT_MC
121 /**************************************************************************
122 DELAYS-
123 pBiquadState->pDelays[0] to
124 pBiquadState->pDelays[NrChannels - 1] is x(n-1) for all NrChannels
125
126 pBiquadState->pDelays[NrChannels] to
127 pBiquadState->pDelays[2*NrChannels - 1] is x(n-2) for all NrChannels
128
129 pBiquadState->pDelays[2*NrChannels] to
130 pBiquadState->pDelays[3*NrChannels - 1] is y(n-1) for all NrChannels
131
132 pBiquadState->pDelays[3*NrChannels] to
133 pBiquadState->pDelays[4*NrChannels - 1] is y(n-2) for all NrChannels
134 ***************************************************************************/
135
PK_Mc_D32F32C14G11_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrFrames,LVM_INT16 NrChannels)136 void PK_Mc_D32F32C14G11_TRC_WRA_01 (Biquad_FLOAT_Instance_t *pInstance,
137 LVM_FLOAT *pDataIn,
138 LVM_FLOAT *pDataOut,
139 LVM_INT16 NrFrames,
140 LVM_INT16 NrChannels)
141 {
142 LVM_FLOAT yn, ynO, temp;
143 LVM_INT16 ii, jj;
144 PFilter_State_Float pBiquadState = (PFilter_State_Float) pInstance;
145
146 for (ii = NrFrames; ii != 0; ii--)
147 {
148
149 for (jj = 0; jj < NrChannels; jj++)
150 {
151 /**************************************************************************
152 PROCESSING OF THE jj CHANNEL
153 ***************************************************************************/
154 /* yn= (A0 * (x(n) - x(n-2)))*/
155 temp = (*pDataIn) - pBiquadState->pDelays[NrChannels + jj];
156 yn = temp * pBiquadState->coefs[0];
157
158 /* yn+= ((-B2 * y(n-2))) */
159 temp = pBiquadState->pDelays[NrChannels*3 + jj] * pBiquadState->coefs[1];
160 yn += temp;
161
162 /* yn+= ((-B1 * y(n-1))) */
163 temp = pBiquadState->pDelays[NrChannels*2 + jj] * pBiquadState->coefs[2];
164 yn += temp;
165
166 /* ynO= ((Gain * yn)) */
167 ynO = yn * pBiquadState->coefs[3];
168
169 /* ynO=(ynO + x(n))*/
170 ynO += (*pDataIn);
171
172 /**************************************************************************
173 UPDATING THE DELAYS
174 ***************************************************************************/
175 pBiquadState->pDelays[NrChannels * 3 + jj] =
176 pBiquadState->pDelays[NrChannels * 2 + jj]; /* y(n-2)=y(n-1)*/
177 pBiquadState->pDelays[NrChannels * 1 + jj] =
178 pBiquadState->pDelays[jj]; /* x(n-2)=x(n-1)*/
179 pBiquadState->pDelays[NrChannels * 2 + jj] = yn; /* Update y(n-1) */
180 pBiquadState->pDelays[jj] = (*pDataIn); /* Update x(n-1)*/
181 pDataIn++;
182
183 /**************************************************************************
184 WRITING THE OUTPUT
185 ***************************************************************************/
186 *pDataOut = ynO; /* Write output*/
187 pDataOut++;
188 }
189 }
190
191 }
192 #endif
193