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 "FO_2I_D16F32Css_LShx_TRC_WRA_01_Private.h"
20 #include "LVM_Macros.h"
21
22 /**************************************************************************
23 ASSUMPTIONS:
24 COEFS-
25 pBiquadState->coefs[0] is A1,
26 pBiquadState->coefs[1] is A0,
27 pBiquadState->coefs[2] is -B1, these are in Q15 format
28 pBiquadState->Shift is Shift value
29 DELAYS-
30 pBiquadState->pDelays[0] is x(n-1)L in Q15 format
31 pBiquadState->pDelays[1] is y(n-1)L in Q30 format
32 pBiquadState->pDelays[2] is x(n-1)R in Q15 format
33 pBiquadState->pDelays[3] is y(n-1)R in Q30 format
34 ***************************************************************************/
35 #ifdef BUILD_FLOAT
FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrSamples)36 void FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t *pInstance,
37 LVM_FLOAT *pDataIn,
38 LVM_FLOAT *pDataOut,
39 LVM_INT16 NrSamples)
40 {
41 LVM_FLOAT ynL,ynR;
42 LVM_FLOAT Temp;
43 LVM_FLOAT NegSatValue;
44 LVM_INT16 ii;
45
46 PFilter_Float_State pBiquadState = (PFilter_Float_State) pInstance;
47
48 NegSatValue = -1.0f;
49
50 for (ii = NrSamples; ii != 0; ii--)
51 {
52
53 /**************************************************************************
54 PROCESSING OF THE LEFT CHANNEL
55 ***************************************************************************/
56
57 // ynL =A1 * x(n-1)L
58 ynL = (LVM_FLOAT)pBiquadState->coefs[0] * pBiquadState->pDelays[0];
59 // ynR =A1 * x(n-1)R
60 ynR = (LVM_FLOAT)pBiquadState->coefs[0] * pBiquadState->pDelays[2];
61
62
63 // ynL+=A0 * x(n)L
64 ynL += (LVM_FLOAT)pBiquadState->coefs[1] * (*pDataIn);
65 // ynR+=A0 * x(n)L
66 ynR += (LVM_FLOAT)pBiquadState->coefs[1] * (*(pDataIn+1));
67
68
69 // ynL += (-B1 * y(n-1)L )
70 Temp = pBiquadState->pDelays[1] * pBiquadState->coefs[2];
71 ynL += Temp;
72 // ynR += (-B1 * y(n-1)R ) )
73 Temp = pBiquadState->pDelays[3] * pBiquadState->coefs[2];
74 ynR += Temp;
75
76
77 /**************************************************************************
78 UPDATING THE DELAYS
79 ***************************************************************************/
80 pBiquadState->pDelays[1] = ynL; // Update y(n-1)L
81 pBiquadState->pDelays[0] = (*pDataIn++); // Update x(n-1)L
82
83 pBiquadState->pDelays[3] = ynR; // Update y(n-1)R
84 pBiquadState->pDelays[2] = (*pDataIn++); // Update x(n-1)R
85
86 /**************************************************************************
87 WRITING THE OUTPUT
88 ***************************************************************************/
89
90 /*Saturate results*/
91 if(ynL > 1.0f)
92 {
93 ynL = 1.0f;
94 }
95 else
96 {
97 if(ynL < NegSatValue)
98 {
99 ynL = NegSatValue;
100 }
101 }
102
103 if(ynR > 1.0f)
104 {
105 ynR = 1.0f;
106 }
107 else
108 {
109 if(ynR < NegSatValue)
110 {
111 ynR = NegSatValue;
112 }
113 }
114
115 *pDataOut++ = (LVM_FLOAT)ynL;
116 *pDataOut++ = (LVM_FLOAT)ynR;
117 }
118
119 }
120 #ifdef SUPPORT_MC
121 /**************************************************************************
122 ASSUMPTIONS:
123 COEFS-
124 pBiquadState->coefs[0] is A1,
125 pBiquadState->coefs[1] is A0,
126 pBiquadState->coefs[2] is -B1,
127 DELAYS-
128 pBiquadState->pDelays[2*ch + 0] is x(n-1) of the 'ch' - channel
129 pBiquadState->pDelays[2*ch + 1] is y(n-1) of the 'ch' - channel
130 The index 'ch' runs from 0 to (NrChannels - 1)
131
132 PARAMETERS:
133 pInstance Pointer Instance
134 pDataIn Input/Source
135 pDataOut Output/Destination
136 NrFrames Number of frames
137 NrChannels Number of channels
138
139 RETURNS:
140 void
141 ***************************************************************************/
FO_Mc_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t * pInstance,LVM_FLOAT * pDataIn,LVM_FLOAT * pDataOut,LVM_INT16 NrFrames,LVM_INT16 NrChannels)142 void FO_Mc_D16F32C15_LShx_TRC_WRA_01(Biquad_FLOAT_Instance_t *pInstance,
143 LVM_FLOAT *pDataIn,
144 LVM_FLOAT *pDataOut,
145 LVM_INT16 NrFrames,
146 LVM_INT16 NrChannels)
147 {
148 LVM_FLOAT yn;
149 LVM_FLOAT Temp;
150 LVM_INT16 ii;
151 LVM_INT16 ch;
152 PFilter_Float_State pBiquadState = (PFilter_Float_State) pInstance;
153
154 LVM_FLOAT *pDelays = pBiquadState->pDelays;
155 LVM_FLOAT *pCoefs = &pBiquadState->coefs[0];
156 LVM_FLOAT A0 = pCoefs[1];
157 LVM_FLOAT A1 = pCoefs[0];
158 LVM_FLOAT B1 = pCoefs[2];
159
160
161
162
163 for (ii = NrFrames; ii != 0; ii--)
164 {
165
166 /**************************************************************************
167 PROCESSING OF THE CHANNELS
168 ***************************************************************************/
169 for (ch = 0; ch < NrChannels; ch++)
170 {
171 // yn =A1 * x(n-1)
172 yn = (LVM_FLOAT)A1 * pDelays[0];
173
174 // yn+=A0 * x(n)
175 yn += (LVM_FLOAT)A0 * (*pDataIn);
176
177 // yn += (-B1 * y(n-1))
178 Temp = B1 * pDelays[1];
179 yn += Temp;
180
181
182 /**************************************************************************
183 UPDATING THE DELAYS
184 ***************************************************************************/
185 pDelays[1] = yn; // Update y(n-1)
186 pDelays[0] = (*pDataIn++); // Update x(n-1)
187
188 /**************************************************************************
189 WRITING THE OUTPUT
190 ***************************************************************************/
191
192 /*Saturate results*/
193 if (yn > 1.0f)
194 {
195 yn = 1.0f;
196 } else if (yn < -1.0f) {
197 yn = -1.0f;
198 }
199
200 *pDataOut++ = (LVM_FLOAT)yn;
201 pDelays += 2;
202 }
203 pDelays -= NrChannels * 2;
204 }
205 }
206 #endif
207 #else
FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_Instance_t * pInstance,LVM_INT16 * pDataIn,LVM_INT16 * pDataOut,LVM_INT16 NrSamples)208 void FO_2I_D16F32C15_LShx_TRC_WRA_01(Biquad_Instance_t *pInstance,
209 LVM_INT16 *pDataIn,
210 LVM_INT16 *pDataOut,
211 LVM_INT16 NrSamples)
212 {
213 LVM_INT32 ynL,ynR;
214 LVM_INT32 Temp;
215 LVM_INT32 NegSatValue;
216 LVM_INT16 ii;
217 LVM_INT16 Shift;
218 PFilter_State pBiquadState = (PFilter_State) pInstance;
219
220 NegSatValue = LVM_MAXINT_16 +1;
221 NegSatValue = -NegSatValue;
222
223 Shift = pBiquadState->Shift;
224
225
226 for (ii = NrSamples; ii != 0; ii--)
227 {
228
229 /**************************************************************************
230 PROCESSING OF THE LEFT CHANNEL
231 ***************************************************************************/
232
233 // ynL =A1 (Q15) * x(n-1)L (Q15) in Q30
234 ynL=(LVM_INT32)pBiquadState->coefs[0]* pBiquadState->pDelays[0];
235 // ynR =A1 (Q15) * x(n-1)R (Q15) in Q30
236 ynR=(LVM_INT32)pBiquadState->coefs[0]* pBiquadState->pDelays[2];
237
238
239 // ynL+=A0 (Q15) * x(n)L (Q15) in Q30
240 ynL+=(LVM_INT32)pBiquadState->coefs[1]* (*pDataIn);
241 // ynR+=A0 (Q15) * x(n)L (Q15) in Q30
242 ynR+=(LVM_INT32)pBiquadState->coefs[1]* (*(pDataIn+1));
243
244
245 // ynL += (-B1 (Q15) * y(n-1)L (Q30) ) in Q30
246 MUL32x16INTO32(pBiquadState->pDelays[1],pBiquadState->coefs[2],Temp,15);
247 ynL +=Temp;
248 // ynR += (-B1 (Q15) * y(n-1)R (Q30) ) in Q30
249 MUL32x16INTO32(pBiquadState->pDelays[3],pBiquadState->coefs[2],Temp,15);
250 ynR +=Temp;
251
252
253 /**************************************************************************
254 UPDATING THE DELAYS
255 ***************************************************************************/
256 pBiquadState->pDelays[1]=ynL; // Update y(n-1)L in Q30
257 pBiquadState->pDelays[0]=(*pDataIn++); // Update x(n-1)L in Q15
258
259 pBiquadState->pDelays[3]=ynR; // Update y(n-1)R in Q30
260 pBiquadState->pDelays[2]=(*pDataIn++); // Update x(n-1)R in Q15
261
262 /**************************************************************************
263 WRITING THE OUTPUT
264 ***************************************************************************/
265 /*Apply shift: Instead of left shift on 16-bit result, right shift of (15-shift) is applied
266 for better SNR*/
267 ynL = ynL>>(15-Shift);
268 ynR = ynR>>(15-Shift);
269
270 /*Saturate results*/
271 if(ynL > LVM_MAXINT_16)
272 {
273 ynL = LVM_MAXINT_16;
274 }
275 else
276 {
277 if(ynL < NegSatValue)
278 {
279 ynL = NegSatValue;
280 }
281 }
282
283 if(ynR > LVM_MAXINT_16)
284 {
285 ynR = LVM_MAXINT_16;
286 }
287 else
288 {
289 if(ynR < NegSatValue)
290 {
291 ynR = NegSatValue;
292 }
293 }
294
295 *pDataOut++=(LVM_INT16)ynL;
296 *pDataOut++=(LVM_INT16)ynR;
297 }
298
299 }
300 #endif
301