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