/* * Copyright (C) 2004-2010 NXP Software * Copyright (C) 2010 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ /****************************************************************************************/ /* */ /* Includes */ /* */ /****************************************************************************************/ #include "LVEQNB_Private.h" /****************************************************************************************/ /* */ /* Defines */ /* */ /****************************************************************************************/ #define PI 3.14159265358979 /****************************************************************************************/ /* */ /* FUNCTION: LVEQNB_DoublePrecCoefs */ /* */ /* DESCRIPTION: */ /* Calculate double precision coefficients for a peaking filter */ /* */ /* PARAMETERS: */ /* Fs Sampling frequency index */ /* pFilterDefinition Pointer to the filter definition */ /* pCoefficients Pointer to the coefficients */ /* */ /* RETURNS: */ /* LVEQNB_SUCCESS Always succeeds */ /* */ /* NOTES: */ /* 1. The equations used are as follows: */ /* */ /* G = 10^(GaindB/20) - 1 */ /* t0 = 2 * Pi * Fc / Fs */ /* D = 1 if GaindB >= 0 */ /* D = 1 / (1 + G) if GaindB < 0 */ /* */ /* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ /* b1 = (0.5 - b2) * (1 - coserr(t0)) */ /* a0 = (0.5 + b2) / 2 */ /* */ /* Where: */ /* GaindB is the gain in dBs, range -15dB to +15dB */ /* Fc is the centre frequency, DC to Fs/50 */ /* Fs is the sample frequency, 8000 to 48000 in descrete steps */ /* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */ /* */ /* 2. The double precision coefficients are only used when fc is less than fs/85, so */ /* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */ /* itself the difference from the value 1.0 is calculated, this can be done with */ /* lower precision maths. */ /* */ /* 3. The value of the B2 coefficient is only calculated as a single precision value, */ /* small errors in this value have a combined effect on the Q and Gain but not the */ /* the frequency of the filter. */ /* */ /****************************************************************************************/ LVEQNB_ReturnStatus_en LVEQNB_DoublePrecCoefs(LVM_UINT16 Fs, LVEQNB_BandDef_t *pFilterDefinition, PK_C32_Coefs_t *pCoefficients) { extern LVM_INT16 LVEQNB_GainTable[]; extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; extern LVM_INT16 LVEQNB_DTable[]; extern LVM_INT16 LVEQNB_DPCosCoef[]; /* * Get the filter definition */ LVM_INT16 Gain = pFilterDefinition->Gain; LVM_UINT16 Frequency = pFilterDefinition->Frequency; LVM_UINT16 QFactor = pFilterDefinition->QFactor; /* * Intermediate variables and temporary values */ LVM_INT32 T0; LVM_INT16 D; LVM_INT32 A0; LVM_INT32 B1; LVM_INT32 B2; LVM_INT32 Dt0; LVM_INT32 B2_Den; LVM_INT32 B2_Num; LVM_INT32 CosErr; LVM_INT16 coef; LVM_INT32 factor; LVM_INT16 t0; LVM_INT16 i; /* * Calculating the intermediate values */ T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ if (Gain >= 0) { D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ } else { D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ } /* * Calculate the B2 coefficient */ Dt0 = D * (T0 >> 10); B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); B2 = (B2_Num / (B2_Den >> 16)) << 15; /* * Calculate the cosine error by a polynomial expansion using the equation: * * CosErr += coef(n) * t0^n For n = 0 to 4 */ T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ t0 = (LVM_INT16)(T0 >> 16); factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ CosErr = 0; /* Initialise the error to zero */ for (i=1; i<5; i++) { coef = LVEQNB_DPCosCoef[i]; /* Get the nth coefficient */ CosErr += (factor * coef) >> 5; /* The nth partial sum */ factor = (factor * t0) >> 15; /* Calculate t0^n */ } CosErr = CosErr << (LVEQNB_DPCosCoef[0]); /* Correct the scaling */ /* * Calculate the B1 and A0 coefficients */ B1 = (0x40000000 - B2); /* B1 = (0.5 - b2/2) */ A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2/2) * coserr(t0) */ B1 -= A0; /* B1 = (0.5 - b2/2) * (1 - coserr(t0)) */ A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) */ /* * Write coeff into the data structure */ pCoefficients->A0 = A0; pCoefficients->B1 = B1; pCoefficients->B2 = B2; pCoefficients->G = LVEQNB_GainTable[Gain+15]; return(LVEQNB_SUCCESS); } /****************************************************************************************/ /* */ /* FUNCTION: LVEQNB_SinglePrecCoefs */ /* */ /* DESCRIPTION: */ /* Calculate single precision coefficients for a peaking filter */ /* */ /* PARAMETERS: */ /* Fs Sampling frequency index */ /* pFilterDefinition Pointer to the filter definition */ /* pCoefficients Pointer to the coefficients */ /* */ /* RETURNS: */ /* LVEQNB_SUCCESS Always succeeds */ /* */ /* NOTES: */ /* 1. The equations used are as follows: */ /* */ /* G = 10^(GaindB/20) - 1 */ /* t0 = 2 * Pi * Fc / Fs */ /* D = 1 if GaindB >= 0 */ /* D = 1 / (1 + G) if GaindB < 0 */ /* */ /* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ /* b1 = (0.5 - b2) * cos(t0) */ /* a0 = (0.5 + b2) / 2 */ /* */ /* Where: */ /* GaindB is the gain in dBs, range -15dB to +15dB */ /* Fc is the centre frequency, DC to Nyquist */ /* Fs is the sample frequency, 8000 to 48000 in descrete steps */ /* Q is the Q factor, 0.25 to 12 */ /* */ /****************************************************************************************/ LVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs, LVEQNB_BandDef_t *pFilterDefinition, PK_C16_Coefs_t *pCoefficients) { extern LVM_INT16 LVEQNB_GainTable[]; extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; extern LVM_INT16 LVEQNB_DTable[]; extern LVM_INT16 LVEQNB_CosCoef[]; /* * Get the filter definition */ LVM_INT16 Gain = pFilterDefinition->Gain; LVM_UINT16 Frequency = pFilterDefinition->Frequency; LVM_UINT16 QFactor = pFilterDefinition->QFactor; /* * Intermediate variables and temporary values */ LVM_INT32 T0; LVM_INT16 D; LVM_INT32 A0; LVM_INT32 B1; LVM_INT32 B2; LVM_INT32 Dt0; LVM_INT32 B2_Den; LVM_INT32 B2_Num; LVM_INT32 COS_T0; LVM_INT16 coef; LVM_INT32 factor; LVM_INT16 t0; LVM_INT16 i; /* * Calculating the intermediate values */ T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ if (Gain >= 0) { D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ } else { D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ } /* * Calculate the B2 coefficient */ Dt0 = D * (T0 >> 10); B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); B2 = (B2_Num / (B2_Den >> 16)) << 15; /* * Calculate the cosine by a polynomial expansion using the equation: * * Cos += coef(n) * t0^n For n = 0 to 6 */ T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ t0 = (LVM_INT16)(T0 >> 16); factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ COS_T0 = 0; /* Initialise the error to zero */ for (i=1; i<7; i++) { coef = LVEQNB_CosCoef[i]; /* Get the nth coefficient */ COS_T0 += (factor * coef) >> 5; /* The nth partial sum */ factor = (factor * t0) >> 15; /* Calculate t0^n */ } COS_T0 = COS_T0 << (LVEQNB_CosCoef[0]+6); /* Correct the scaling */ B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2/2) * cos(t0) */ A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2/2) */ /* * Write coeff into the data structure */ pCoefficients->A0 = (LVM_INT16)(A0>>16); pCoefficients->B1 = (LVM_INT16)(B1>>15); pCoefficients->B2 = (LVM_INT16)(B2>>16); pCoefficients->G = LVEQNB_GainTable[Gain+15]; return(LVEQNB_SUCCESS); }