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 #ifdef BUILD_FLOAT
BQ_2I_D32F32C30_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrSamples)40 void BQ_2I_D32F32C30_TRC_WRA_01 ( Biquad_FLOAT_Instance_t *pInstance,
41 LVM_FLOAT *pDataIn,
42 LVM_FLOAT *pDataOut,
43 LVM_INT16 NrSamples)
44
45
46 {
47 LVM_FLOAT ynL,ynR,templ,tempd;
48 LVM_INT16 ii;
49 PFilter_State_FLOAT pBiquadState = (PFilter_State_FLOAT) pInstance;
50
51 for (ii = NrSamples; ii != 0; ii--)
52 {
53
54
55 /**************************************************************************
56 PROCESSING OF THE LEFT CHANNEL
57 ***************************************************************************/
58 /* ynL= ( A2 * x(n-2)L ) */
59 ynL = pBiquadState->coefs[0] * pBiquadState->pDelays[2];
60
61 /* ynL+= ( A1 * x(n-1)L )*/
62 templ = pBiquadState->coefs[1] * pBiquadState->pDelays[0];
63 ynL += templ;
64
65 /* ynL+= ( A0 * x(n)L ) */
66 templ = pBiquadState->coefs[2] * (*pDataIn);
67 ynL += templ;
68
69 /* ynL+= (-B2 * y(n-2)L ) */
70 templ = pBiquadState->coefs[3] * pBiquadState->pDelays[6];
71 ynL += templ;
72
73 /* ynL+= (-B1 * y(n-1)L )*/
74 templ = pBiquadState->coefs[4] * pBiquadState->pDelays[4];
75 ynL += templ;
76
77 /**************************************************************************
78 PROCESSING OF THE RIGHT CHANNEL
79 ***************************************************************************/
80 /* ynR= ( A2 * x(n-2)R ) */
81 ynR = pBiquadState->coefs[0] * pBiquadState->pDelays[3];
82
83 /* ynR+= ( A1 * x(n-1)R ) */
84 templ = pBiquadState->coefs[1] * pBiquadState->pDelays[1];
85 ynR += templ;
86
87 /* ynR+= ( A0 * x(n)R ) */
88 tempd =* (pDataIn+1);
89 templ = pBiquadState->coefs[2] * tempd;
90 ynR += templ;
91
92 /* ynR+= (-B2 * y(n-2)R ) */
93 templ = pBiquadState->coefs[3] * pBiquadState->pDelays[7];
94 ynR += templ;
95
96 /* ynR+= (-B1 * y(n-1)R ) */
97 templ = pBiquadState->coefs[4] * pBiquadState->pDelays[5];
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_FLOAT)ynR; /* Update y(n-1)R */
108 pBiquadState->pDelays[4] = (LVM_FLOAT)ynL; /* Update y(n-1)L */
109 pBiquadState->pDelays[0] = (*pDataIn); /* Update x(n-1)L */
110 pDataIn++;
111 pBiquadState->pDelays[1] = (*pDataIn); /* Update x(n-1)R */
112 pDataIn++;
113
114 /**************************************************************************
115 WRITING THE OUTPUT
116 ***************************************************************************/
117 *pDataOut = (LVM_FLOAT)ynL; /* Write Left output */
118 pDataOut++;
119 *pDataOut = (LVM_FLOAT)ynR; /* Write Right ouput */
120 pDataOut++;
121
122
123 }
124
125 }
126
127 #ifdef SUPPORT_MC
128 /**************************************************************************
129 ASSUMPTIONS:
130 COEFS-
131 pBiquadState->coefs[0] is A2, pBiquadState->coefs[1] is A1
132 pBiquadState->coefs[2] is A0, pBiquadState->coefs[3] is -B2
133 pBiquadState->coefs[4] is -B1
134
135 DELAYS-
136 pBiquadState->pDelays[0] to
137 pBiquadState->pDelays[NrChannels - 1] is x(n-1) for all NrChannels
138
139 pBiquadState->pDelays[NrChannels] to
140 pBiquadState->pDelays[2*NrChannels - 1] is x(n-2) for all NrChannels
141
142 pBiquadState->pDelays[2*NrChannels] to
143 pBiquadState->pDelays[3*NrChannels - 1] is y(n-1) for all NrChannels
144
145 pBiquadState->pDelays[3*NrChannels] to
146 pBiquadState->pDelays[4*NrChannels - 1] is y(n-2) for all NrChannels
147 ***************************************************************************/
BQ_MC_D32F32C30_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrFrames,LVM_INT16 NrChannels)148 void BQ_MC_D32F32C30_TRC_WRA_01 ( Biquad_FLOAT_Instance_t *pInstance,
149 LVM_FLOAT *pDataIn,
150 LVM_FLOAT *pDataOut,
151 LVM_INT16 NrFrames,
152 LVM_INT16 NrChannels)
153
154
155 {
156 LVM_FLOAT yn, temp;
157 LVM_INT16 ii, jj;
158 PFilter_State_FLOAT pBiquadState = (PFilter_State_FLOAT) pInstance;
159
160 for (ii = NrFrames; ii != 0; ii--)
161 {
162 /**************************************************************************
163 PROCESSING CHANNEL-WISE
164 ***************************************************************************/
165 for (jj = 0; jj < NrChannels; jj++)
166 {
167 /* yn= (A2 * x(n-2)) */
168 yn = pBiquadState->coefs[0] * pBiquadState->pDelays[NrChannels + jj];
169
170 /* yn+= (A1 * x(n-1)) */
171 temp = pBiquadState->coefs[1] * pBiquadState->pDelays[jj];
172 yn += temp;
173
174 /* yn+= (A0 * x(n)) */
175 temp = pBiquadState->coefs[2] * (*pDataIn);
176 yn += temp;
177
178 /* yn+= (-B2 * y(n-2)) */
179 temp = pBiquadState->coefs[3] * pBiquadState->pDelays[NrChannels*3 + jj];
180 yn += temp;
181
182 /* yn+= (-B1 * y(n-1)) */
183 temp = pBiquadState->coefs[4] * pBiquadState->pDelays[NrChannels*2 + jj];
184 yn += temp;
185
186 /**************************************************************************
187 UPDATING THE DELAYS
188 ***************************************************************************/
189 pBiquadState->pDelays[NrChannels * 3 + jj] =
190 pBiquadState->pDelays[NrChannels * 2 + jj]; /* y(n-2)=y(n-1)*/
191 pBiquadState->pDelays[NrChannels * 1 + jj] =
192 pBiquadState->pDelays[jj]; /* x(n-2)=x(n-1)*/
193 pBiquadState->pDelays[NrChannels * 2 + jj] = (LVM_FLOAT)yn; /* Update y(n-1)*/
194 pBiquadState->pDelays[jj] = (*pDataIn); /* Update x(n-1)*/
195 pDataIn++;
196 /**************************************************************************
197 WRITING THE OUTPUT
198 ***************************************************************************/
199 *pDataOut = (LVM_FLOAT)yn; /* Write jj Channel output */
200 pDataOut++;
201 }
202 }
203
204 }
205 #endif /*SUPPORT_MC*/
206
207 #else
BQ_2I_D32F32C30_TRC_WRA_01(Biquad_Instance_t * pInstance,LVM_INT32 * pDataIn,LVM_INT32 * pDataOut,LVM_INT16 NrSamples)208 void BQ_2I_D32F32C30_TRC_WRA_01 ( Biquad_Instance_t *pInstance,
209 LVM_INT32 *pDataIn,
210 LVM_INT32 *pDataOut,
211 LVM_INT16 NrSamples)
212
213
214 {
215 LVM_INT32 ynL,ynR,templ,tempd;
216 LVM_INT16 ii;
217 PFilter_State pBiquadState = (PFilter_State) pInstance;
218
219 for (ii = NrSamples; ii != 0; ii--)
220 {
221
222
223 /**************************************************************************
224 PROCESSING OF THE LEFT CHANNEL
225 ***************************************************************************/
226 /* ynL= ( A2 (Q30) * x(n-2)L (Q0) ) >>30 in Q0*/
227 MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[2],ynL,30)
228
229 /* ynL+= ( A1 (Q30) * x(n-1)L (Q0) ) >> 30 in Q0*/
230 MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[0],templ,30)
231 ynL+=templ;
232
233 /* ynL+= ( A0 (Q30) * x(n)L (Q0) ) >> 30 in Q0*/
234 MUL32x32INTO32(pBiquadState->coefs[2],*pDataIn,templ,30)
235 ynL+=templ;
236
237 /* ynL+= (-B2 (Q30) * y(n-2)L (Q0) ) >> 30 in Q0*/
238 MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[6],templ,30)
239 ynL+=templ;
240
241 /* ynL+= (-B1 (Q30) * y(n-1)L (Q0) ) >> 30 in Q0 */
242 MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[4],templ,30)
243 ynL+=templ;
244
245 /**************************************************************************
246 PROCESSING OF THE RIGHT CHANNEL
247 ***************************************************************************/
248 /* ynR= ( A2 (Q30) * x(n-2)R (Q0) ) >> 30 in Q0*/
249 MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[3],ynR,30)
250
251 /* ynR+= ( A1 (Q30) * x(n-1)R (Q0) ) >> 30 in Q0*/
252 MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[1],templ,30)
253 ynR+=templ;
254
255 /* ynR+= ( A0 (Q30) * x(n)R (Q0) ) >> 30 in Q0*/
256 tempd=*(pDataIn+1);
257 MUL32x32INTO32(pBiquadState->coefs[2],tempd,templ,30)
258 ynR+=templ;
259
260 /* ynR+= (-B2 (Q30) * y(n-2)R (Q0) ) >> 30 in Q0*/
261 MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[7],templ,30)
262 ynR+=templ;
263
264 /* ynR+= (-B1 (Q30) * y(n-1)R (Q0) ) >> 30 in Q0 */
265 MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[5],templ,30)
266 ynR+=templ;
267
268 /**************************************************************************
269 UPDATING THE DELAYS
270 ***************************************************************************/
271 pBiquadState->pDelays[7]=pBiquadState->pDelays[5]; /* y(n-2)R=y(n-1)R*/
272 pBiquadState->pDelays[6]=pBiquadState->pDelays[4]; /* y(n-2)L=y(n-1)L*/
273 pBiquadState->pDelays[3]=pBiquadState->pDelays[1]; /* x(n-2)R=x(n-1)R*/
274 pBiquadState->pDelays[2]=pBiquadState->pDelays[0]; /* x(n-2)L=x(n-1)L*/
275 pBiquadState->pDelays[5]=(LVM_INT32)ynR; /* Update y(n-1)R in Q0*/
276 pBiquadState->pDelays[4]=(LVM_INT32)ynL; /* Update y(n-1)L in Q0*/
277 pBiquadState->pDelays[0]=(*pDataIn); /* Update x(n-1)L in Q0*/
278 pDataIn++;
279 pBiquadState->pDelays[1]=(*pDataIn); /* Update x(n-1)R in Q0*/
280 pDataIn++;
281
282 /**************************************************************************
283 WRITING THE OUTPUT
284 ***************************************************************************/
285 *pDataOut=(LVM_INT32)ynL; /* Write Left output in Q0*/
286 pDataOut++;
287 *pDataOut=(LVM_INT32)ynR; /* Write Right ouput in Q0*/
288 pDataOut++;
289
290
291 }
292
293 }
294 #endif /*BUILD_FLOAT*/
295