frameworks/base/media/libeffects/lvm/lib/Eq/src/LVEQNB_CalcCoef.c

   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
  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
 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 }