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