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