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 /****************************************************************************************/
19 /* */
20 /* Includes */
21 /* */
22 /****************************************************************************************/
23
24 #include "LVEQNB_Private.h"
25 #ifdef BUILD_FLOAT
26 #include <math.h>
27 #endif
28
29 /****************************************************************************************/
30 /* */
31 /* Defines */
32 /* */
33 /****************************************************************************************/
34
35 #define PI 3.14159265358979
36
37 /****************************************************************************************/
38 /* */
39 /* FUNCTION: LVEQNB_DoublePrecCoefs */
40 /* */
41 /* DESCRIPTION: */
42 /* Calculate double precision coefficients for a peaking filter */
43 /* */
44 /* PARAMETERS: */
45 /* Fs Sampling frequency index */
46 /* pFilterDefinition Pointer to the filter definition */
47 /* pCoefficients Pointer to the coefficients */
48 /* */
49 /* RETURNS: */
50 /* LVEQNB_SUCCESS Always succeeds */
51 /* */
52 /* NOTES: */
53 /* 1. The equations used are as follows: */
54 /* */
55 /* G = 10^(GaindB/20) - 1 */
56 /* t0 = 2 * Pi * Fc / Fs */
57 /* D = 1 if GaindB >= 0 */
58 /* D = 1 / (1 + G) if GaindB < 0 */
59 /* */
60 /* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */
61 /* b1 = (0.5 - b2) * (1 - coserr(t0)) */
62 /* a0 = (0.5 + b2) / 2 */
63 /* */
64 /* Where: */
65 /* GaindB is the gain in dBs, range -15dB to +15dB */
66 /* Fc is the centre frequency, DC to Fs/50 */
67 /* Fs is the sample frequency, 8000 to 48000 in descrete steps */
68 /* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */
69 /* */
70 /* 2. The double precision coefficients are only used when fc is less than fs/85, so */
71 /* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */
72 /* itself the difference from the value 1.0 is calculated, this can be done with */
73 /* lower precision maths. */
74 /* */
75 /* 3. The value of the B2 coefficient is only calculated as a single precision value, */
76 /* small errors in this value have a combined effect on the Q and Gain but not the */
77 /* the frequency of the filter. */
78 /* */
79 /****************************************************************************************/
80
81
82 #ifndef BUILD_FLOAT
LVEQNB_DoublePrecCoefs(LVM_UINT16 Fs,LVEQNB_BandDef_t * pFilterDefinition,PK_C32_Coefs_t * pCoefficients)83 LVEQNB_ReturnStatus_en LVEQNB_DoublePrecCoefs(LVM_UINT16 Fs,
84 LVEQNB_BandDef_t *pFilterDefinition,
85 PK_C32_Coefs_t *pCoefficients)
86 {
87
88 extern LVM_INT16 LVEQNB_GainTable[];
89 extern LVM_INT16 LVEQNB_TwoPiOnFsTable[];
90 extern LVM_INT16 LVEQNB_DTable[];
91 extern LVM_INT16 LVEQNB_DPCosCoef[];
92
93 /*
94 * Get the filter definition
95 */
96 LVM_INT16 Gain = pFilterDefinition->Gain;
97 LVM_UINT16 Frequency = pFilterDefinition->Frequency;
98 LVM_UINT16 QFactor = pFilterDefinition->QFactor;
99
100 /*
101 * Intermediate variables and temporary values
102 */
103 LVM_INT32 T0;
104 LVM_INT16 D;
105 LVM_INT32 A0;
106 LVM_INT32 B1;
107 LVM_INT32 B2;
108 LVM_INT32 Dt0;
109 LVM_INT32 B2_Den;
110 LVM_INT32 B2_Num;
111 LVM_INT32 CosErr;
112 LVM_INT16 coef;
113 LVM_INT32 factor;
114 LVM_INT16 t0;
115 LVM_INT16 i;
116
117 /*
118 * Calculating the intermediate values
119 */
120 T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */
121 if (Gain >= 0)
122 {
123 D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */
124 }
125 else
126 {
127 D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */
128 }
129
130 /*
131 * Calculate the B2 coefficient
132 */
133 Dt0 = D * (T0 >> 10);
134 B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2);
135 B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18);
136 B2 = (B2_Num / (B2_Den >> 16)) << 15;
137
138 /*
139 * Calculate the cosine error by a polynomial expansion using the equation:
140 *
141 * CosErr += coef(n) * t0^n For n = 0 to 4
142 */
143 T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */
144 t0 = (LVM_INT16)(T0 >> 16);
145 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */
146 CosErr = 0; /* Initialise the error to zero */
147 for (i=1; i<5; i++)
148 {
149 coef = LVEQNB_DPCosCoef[i]; /* Get the nth coefficient */
150 CosErr += (factor * coef) >> 5; /* The nth partial sum */
151 factor = (factor * t0) >> 15; /* Calculate t0^n */
152 }
153 CosErr = CosErr << (LVEQNB_DPCosCoef[0]); /* Correct the scaling */
154
155 /*
156 * Calculate the B1 and A0 coefficients
157 */
158 B1 = (0x40000000 - B2); /* B1 = (0.5 - b2/2) */
159 A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2/2) * coserr(t0) */
160 B1 -= A0; /* B1 = (0.5 - b2/2) * (1 - coserr(t0)) */
161 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) */
162
163 /*
164 * Write coeff into the data structure
165 */
166 pCoefficients->A0 = A0;
167 pCoefficients->B1 = B1;
168 pCoefficients->B2 = B2;
169 pCoefficients->G = LVEQNB_GainTable[Gain+15];
170
171 return(LVEQNB_SUCCESS);
172
173 }
174 #endif
175
176 /****************************************************************************************/
177 /* */
178 /* FUNCTION: LVEQNB_SinglePrecCoefs */
179 /* */
180 /* DESCRIPTION: */
181 /* Calculate single precision coefficients for a peaking filter */
182 /* */
183 /* PARAMETERS: */
184 /* Fs Sampling frequency index */
185 /* pFilterDefinition Pointer to the filter definition */
186 /* pCoefficients Pointer to the coefficients */
187 /* */
188 /* RETURNS: */
189 /* LVEQNB_SUCCESS Always succeeds */
190 /* */
191 /* NOTES: */
192 /* 1. The equations used are as follows: */
193 /* */
194 /* G = 10^(GaindB/20) - 1 */
195 /* t0 = 2 * Pi * Fc / Fs */
196 /* D = 1 if GaindB >= 0 */
197 /* D = 1 / (1 + G) if GaindB < 0 */
198 /* */
199 /* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */
200 /* b1 = (0.5 - b2) * cos(t0) */
201 /* a0 = (0.5 + b2) / 2 */
202 /* */
203 /* Where: */
204 /* GaindB is the gain in dBs, range -15dB to +15dB */
205 /* Fc is the centre frequency, DC to Nyquist */
206 /* Fs is the sample frequency, 8000 to 48000 in descrete steps */
207 /* Q is the Q factor, 0.25 to 12 */
208 /* */
209 /****************************************************************************************/
210
211 #ifdef BUILD_FLOAT
LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs,LVEQNB_BandDef_t * pFilterDefinition,PK_FLOAT_Coefs_t * pCoefficients)212 LVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs,
213 LVEQNB_BandDef_t *pFilterDefinition,
214 PK_FLOAT_Coefs_t *pCoefficients)
215 {
216
217 extern LVM_FLOAT LVEQNB_GainTable[];
218 extern LVM_FLOAT LVEQNB_TwoPiOnFsTable[];
219 extern LVM_FLOAT LVEQNB_DTable[];
220
221
222 /*
223 * Get the filter definition
224 */
225 LVM_INT16 Gain = pFilterDefinition->Gain;
226 LVM_UINT16 Frequency = pFilterDefinition->Frequency;
227 /* As mentioned in effectbundle.h */
228 LVM_FLOAT QFactor = (LVM_FLOAT)pFilterDefinition->QFactor / 100.0f;
229
230
231 /*
232 * Intermediate variables and temporary values
233 */
234 LVM_FLOAT T0;
235 LVM_FLOAT D;
236 LVM_FLOAT A0;
237 LVM_FLOAT B1;
238 LVM_FLOAT B2;
239
240 /*
241 * Calculating the intermediate values
242 */
243 T0 = Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */
244 if (Gain >= 0)
245 {
246 D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */
247 }
248 else
249 {
250 D = LVEQNB_DTable[Gain + 15]; /* D = 1 / (1 + G) if GaindB < 0 */
251 }
252
253 /*
254 * Calculate the B2,B1,A0 coefficients
255 */
256 B2 = -0.5 * (2 * QFactor - D * T0) / (2 * QFactor + D * T0);
257 B1 = (0.5 - B2) * cos(T0);
258 A0 = (0.5 + B2) / 2.0;
259
260 /*
261 * Write coeff into the data structure
262 */
263 /* all the coefficients are multiplied with 2 to make them align with fixed point values*/
264 pCoefficients->A0 = 2 * A0;
265 pCoefficients->B1 = 2 * B1;
266 pCoefficients->B2 = 2 * B2;
267 pCoefficients->G = LVEQNB_GainTable[Gain + 15];
268
269 return(LVEQNB_SUCCESS);
270 }
271 #else
LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs,LVEQNB_BandDef_t * pFilterDefinition,PK_C16_Coefs_t * pCoefficients)272 LVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs,
273 LVEQNB_BandDef_t *pFilterDefinition,
274 PK_C16_Coefs_t *pCoefficients)
275 {
276
277 extern LVM_INT16 LVEQNB_GainTable[];
278 extern LVM_INT16 LVEQNB_TwoPiOnFsTable[];
279 extern LVM_INT16 LVEQNB_DTable[];
280 extern LVM_INT16 LVEQNB_CosCoef[];
281
282
283 /*
284 * Get the filter definition
285 */
286 LVM_INT16 Gain = pFilterDefinition->Gain;
287 LVM_UINT16 Frequency = pFilterDefinition->Frequency;
288 LVM_UINT16 QFactor = pFilterDefinition->QFactor;
289
290
291 /*
292 * Intermediate variables and temporary values
293 */
294 LVM_INT32 T0;
295 LVM_INT16 D;
296 LVM_INT32 A0;
297 LVM_INT32 B1;
298 LVM_INT32 B2;
299 LVM_INT32 Dt0;
300 LVM_INT32 B2_Den;
301 LVM_INT32 B2_Num;
302 LVM_INT32 COS_T0;
303 LVM_INT16 coef;
304 LVM_INT32 factor;
305 LVM_INT16 t0;
306 LVM_INT16 i;
307
308 /*
309 * Calculating the intermediate values
310 */
311 T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */
312 if (Gain >= 0)
313 {
314 D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */
315 }
316 else
317 {
318 D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */
319 }
320
321 /*
322 * Calculate the B2 coefficient
323 */
324 Dt0 = D * (T0 >> 10);
325 B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2);
326 B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18);
327 B2 = (B2_Num / (B2_Den >> 16)) << 15;
328
329 /*
330 * Calculate the cosine by a polynomial expansion using the equation:
331 *
332 * Cos += coef(n) * t0^n For n = 0 to 6
333 */
334 T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */
335 t0 = (LVM_INT16)(T0 >> 16);
336 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */
337 COS_T0 = 0; /* Initialise the error to zero */
338 for (i=1; i<7; i++)
339 {
340 coef = LVEQNB_CosCoef[i]; /* Get the nth coefficient */
341 COS_T0 += (factor * coef) >> 5; /* The nth partial sum */
342 factor = (factor * t0) >> 15; /* Calculate t0^n */
343 }
344 COS_T0 = COS_T0 << (LVEQNB_CosCoef[0]+6); /* Correct the scaling */
345
346
347 B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2/2) * cos(t0) */
348 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2/2) */
349
350 /*
351 * Write coeff into the data structure
352 */
353 pCoefficients->A0 = (LVM_INT16)(A0>>16);
354 pCoefficients->B1 = (LVM_INT16)(B1>>15);
355 pCoefficients->B2 = (LVM_INT16)(B2>>16);
356 pCoefficients->G = LVEQNB_GainTable[Gain+15];
357
358
359 return(LVEQNB_SUCCESS);
360
361 }
362 #endif