[go: nahoru, domu]

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
80LVEQNB_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
209LVEQNB_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