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