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.h" 25 #include "LVEQNB_Private.h" 26 #include "VectorArithmetic.h" 27 #include "BIQUAD.h" 28 29 30 /****************************************************************************************/ 31 /* */ 32 /* Defines */ 33 /* */ 34 /****************************************************************************************/ 35 36 #define LOW_FREQ 298 /* 32768/110 for low test frequency */ 37 #define HIGH_FREQ 386 /* 32768/85 for high test frequency */ 38 39 /****************************************************************************************/ 40 /* */ 41 /* FUNCTION: LVEQNB_GetParameters */ 42 /* */ 43 /* DESCRIPTION: */ 44 /* Request the N-Band equaliser parameters. The current parameter set is returned via */ 45 /* the parameter pointer. */ 46 /* */ 47 /* PARAMETERS: */ 48 /* hInstance Instance handle */ 49 /* pParams Pointer to an empty parameter structure */ 50 /* */ 51 /* RETURNS: */ 52 /* LVEQNB_SUCCESS Succeeds */ 53 /* LVEQNB_NULLADDRESS Instance or pParams is NULL pointer */ 54 /* */ 55 /* NOTES: */ 56 /* 1. This function may be interrupted by the LVEQNB_Process function */ 57 /* */ 58 /****************************************************************************************/ 59 60 LVEQNB_ReturnStatus_en LVEQNB_GetParameters(LVEQNB_Handle_t hInstance, 61 LVEQNB_Params_t *pParams) 62 { 63 64 LVEQNB_Instance_t *pInstance =(LVEQNB_Instance_t *)hInstance; 65 66 /* 67 * Check for error conditions 68 */ 69 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 70 { 71 return LVEQNB_NULLADDRESS; 72 } 73 74 *pParams = pInstance->Params; 75 76 return(LVEQNB_SUCCESS); 77 } 78 79 80 /************************************************************************************/ 81 /* */ 82 /* FUNCTION: LVEQNB_GetCapabilities */ 83 /* */ 84 /* DESCRIPTION: */ 85 /* Get the N-Band equaliser capabilities. The current capabilities are returned */ 86 /* via the pointer. */ 87 /* */ 88 /* PARAMETERS: */ 89 /* hInstance Instance handle */ 90 /* pCapabilities Pointer to an empty capability structure */ 91 /* */ 92 /* RETURNS: */ 93 /* LVEQNB_Success Succeeds */ 94 /* LVEQNB_NULLADDRESS hInstance or pCapabilities is NULL */ 95 /* */ 96 /* NOTES: */ 97 /* 1. This function may be interrupted by the LVEQNB_Process function */ 98 /* */ 99 /************************************************************************************/ 100 101 LVEQNB_ReturnStatus_en LVEQNB_GetCapabilities(LVEQNB_Handle_t hInstance, 102 LVEQNB_Capabilities_t *pCapabilities) 103 { 104 105 LVEQNB_Instance_t *pInstance =(LVEQNB_Instance_t *)hInstance; 106 107 if((hInstance == LVM_NULL) || (pCapabilities == LVM_NULL)) 108 { 109 return LVEQNB_NULLADDRESS; 110 } 111 112 *pCapabilities = pInstance->Capabilities; 113 114 return(LVEQNB_SUCCESS); 115 } 116 117 118 /************************************************************************************/ 119 /* */ 120 /* FUNCTION: LVEQNB_SetFilters */ 121 /* */ 122 /* DESCRIPTION: */ 123 /* Sets the filter type based on the definition. */ 124 /* */ 125 /* PARAMETERS: */ 126 /* pInstance Pointer to the instance */ 127 /* pParams Initialisation parameters */ 128 /* */ 129 /* RETURNS: */ 130 /* void Nothing */ 131 /* */ 132 /* NOTES: */ 133 /* 1. To select the biquad type the follow rules are applied: */ 134 /* Double precision if (fc <= fs/110) */ 135 /* Double precision if (fs/110 < fc < fs/85) & (Q>3) */ 136 /* Single precision otherwise */ 137 /* */ 138 /************************************************************************************/ 139 140 void LVEQNB_SetFilters(LVEQNB_Instance_t *pInstance, 141 LVEQNB_Params_t *pParams) 142 { 143 #ifdef HIGHER_FS 144 extern const LVM_UINT32 LVEQNB_SampleRateTab[]; /* Sample rate table */ 145 #else 146 extern const LVM_UINT16 LVEQNB_SampleRateTab[]; /* Sample rate table */ 147 #endif 148 149 LVM_UINT16 i; /* Filter band index */ 150 LVM_UINT32 fs = (LVM_UINT32)LVEQNB_SampleRateTab[(LVM_UINT16)pParams->SampleRate]; /* Sample rate */ 151 LVM_UINT32 fc; /* Filter centre frequency */ 152 LVM_INT16 QFactor; /* Filter Q factor */ 153 154 155 pInstance->NBands = pParams->NBands; 156 157 for (i=0; i<pParams->NBands; i++) 158 { 159 /* 160 * Get the filter settings 161 */ 162 fc = (LVM_UINT32)pParams->pBandDefinition[i].Frequency; /* Get the band centre frequency */ 163 QFactor = (LVM_INT16)pParams->pBandDefinition[i].QFactor; /* Get the band Q factor */ 164 165 #ifdef BUILD_FLOAT 166 pInstance->pBiquadType[i] = LVEQNB_SinglePrecision_Float; /* Default to single precision */ 167 #else 168 /* 169 * For each filter set the type of biquad required 170 */ 171 pInstance->pBiquadType[i] = LVEQNB_SinglePrecision; /* Default to single precision */ 172 #endif 173 #ifndef BUILD_FLOAT 174 if ((fc << 15) <= (LOW_FREQ * fs)) 175 { 176 /* 177 * fc <= fs/110 178 */ 179 pInstance->pBiquadType[i] = LVEQNB_DoublePrecision; 180 } 181 else if (((fc << 15) <= (HIGH_FREQ * fs)) && (QFactor > 300)) 182 { 183 /* 184 * (fs/110 < fc < fs/85) & (Q>3) 185 */ 186 pInstance->pBiquadType[i] = LVEQNB_DoublePrecision; 187 } 188 #endif 189 190 /* 191 * Check for out of range frequencies 192 */ 193 if (fc > (fs >> 1)) 194 { 195 pInstance->pBiquadType[i] = LVEQNB_OutOfRange; 196 } 197 198 199 /* 200 * Copy the filter definition to persistant memory 201 */ 202 pInstance->pBandDefinitions[i] = pParams->pBandDefinition[i]; 203 204 } 205 } 206 207 208 /************************************************************************************/ 209 /* */ 210 /* FUNCTION: LVEQNB_SetCoefficients */ 211 /* */ 212 /* DESCRIPTION: */ 213 /* Sets the filter coefficients. This uses the type to select single or double */ 214 /* precision coefficients. */ 215 /* */ 216 /* PARAMETERS: */ 217 /* pInstance Pointer to the instance */ 218 /* pParams Initialisation parameters */ 219 /* */ 220 /************************************************************************************/ 221 222 void LVEQNB_SetCoefficients(LVEQNB_Instance_t *pInstance) 223 { 224 225 LVM_UINT16 i; /* Filter band index */ 226 LVEQNB_BiquadType_en BiquadType; /* Filter biquad type */ 227 228 229 /* 230 * Set the coefficients for each band by the init function 231 */ 232 for (i=0; i<pInstance->Params.NBands; i++) 233 { 234 235 /* 236 * Check band type for correct initialisation method and recalculate the coefficients 237 */ 238 BiquadType = pInstance->pBiquadType[i]; 239 switch (BiquadType) 240 { 241 #ifdef BUILD_FLOAT 242 case LVEQNB_SinglePrecision_Float: 243 { 244 PK_FLOAT_Coefs_t Coefficients; 245 /* 246 * Calculate the single precision coefficients 247 */ 248 LVEQNB_SinglePrecCoefs((LVM_UINT16)pInstance->Params.SampleRate, 249 &pInstance->pBandDefinitions[i], 250 &Coefficients); 251 /* 252 * Set the coefficients 253 */ 254 PK_2I_D32F32CssGss_TRC_WRA_01_Init(&pInstance->pEQNB_FilterState_Float[i], 255 &pInstance->pEQNB_Taps_Float[i], 256 &Coefficients); 257 break; 258 } 259 #else 260 case LVEQNB_DoublePrecision: 261 { 262 PK_C32_Coefs_t Coefficients; 263 264 /* 265 * Calculate the double precision coefficients 266 */ 267 LVEQNB_DoublePrecCoefs((LVM_UINT16)pInstance->Params.SampleRate, 268 &pInstance->pBandDefinitions[i], 269 &Coefficients); 270 271 /* 272 * Set the coefficients 273 */ 274 PK_2I_D32F32CllGss_TRC_WRA_01_Init(&pInstance->pEQNB_FilterState[i], 275 &pInstance->pEQNB_Taps[i], 276 &Coefficients); 277 break; 278 } 279 280 case LVEQNB_SinglePrecision: 281 { 282 PK_C16_Coefs_t Coefficients; 283 284 /* 285 * Calculate the single precision coefficients 286 */ 287 LVEQNB_SinglePrecCoefs((LVM_UINT16)pInstance->Params.SampleRate, 288 &pInstance->pBandDefinitions[i], 289 &Coefficients); 290 291 /* 292 * Set the coefficients 293 */ 294 PK_2I_D32F32CssGss_TRC_WRA_01_Init(&pInstance->pEQNB_FilterState[i], 295 &pInstance->pEQNB_Taps[i], 296 &Coefficients); 297 break; 298 } 299 #endif 300 default: 301 break; 302 } 303 } 304 305 } 306 307 308 /************************************************************************************/ 309 /* */ 310 /* FUNCTION: LVEQNB_ClearFilterHistory */ 311 /* */ 312 /* DESCRIPTION: */ 313 /* Clears the filter data history */ 314 /* */ 315 /* PARAMETERS: */ 316 /* pInstance Pointer to the instance */ 317 /* */ 318 /************************************************************************************/ 319 #ifndef BUILD_FLOAT 320 void LVEQNB_ClearFilterHistory(LVEQNB_Instance_t *pInstance) 321 { 322 LVM_INT16 *pTapAddress; 323 LVM_INT16 NumTaps; 324 325 326 pTapAddress = (LVM_INT16 *)pInstance->pEQNB_Taps; 327 NumTaps = (LVM_INT16)((pInstance->Capabilities.MaxBands * sizeof(Biquad_2I_Order2_Taps_t))/sizeof(LVM_INT16)); 328 329 if (NumTaps != 0) 330 { 331 LoadConst_16(0, /* Clear the history, value 0 */ 332 pTapAddress, /* Destination */ 333 NumTaps); /* Number of words */ 334 } 335 } 336 #else 337 void LVEQNB_ClearFilterHistory(LVEQNB_Instance_t *pInstance) 338 { 339 LVM_FLOAT *pTapAddress; 340 LVM_INT16 NumTaps; 341 342 pTapAddress = (LVM_FLOAT *)pInstance->pEQNB_Taps_Float; 343 NumTaps = (LVM_INT16)((pInstance->Capabilities.MaxBands * \ 344 sizeof(Biquad_2I_Order2_FLOAT_Taps_t)) / sizeof(LVM_FLOAT)); 345 346 if (NumTaps != 0) 347 { 348 LoadConst_Float(0, /* Clear the history, value 0 */ 349 pTapAddress, /* Destination */ 350 NumTaps); /* Number of words */ 351 } 352 } 353 #endif 354 /****************************************************************************************/ 355 /* */ 356 /* FUNCTION: LVEQNB_Control */ 357 /* */ 358 /* DESCRIPTION: */ 359 /* Sets or changes the LifeVibes module parameters. */ 360 /* */ 361 /* PARAMETERS: */ 362 /* hInstance Instance handle */ 363 /* pParams Pointer to a parameter structure */ 364 /* */ 365 /* RETURNS: */ 366 /* LVEQNB_Success Always succeeds */ 367 /* LVEQNB_NULLADDRESS Instance or pParams is NULL pointer */ 368 /* LVEQNB_NULLADDRESS NULL address for the equaliser filter definitions and the */ 369 /* number of bands is non-zero */ 370 /* */ 371 /* NOTES: */ 372 /* 1. This function may be interrupted by the LVEQNB_Process function */ 373 /* */ 374 /****************************************************************************************/ 375 376 LVEQNB_ReturnStatus_en LVEQNB_Control(LVEQNB_Handle_t hInstance, 377 LVEQNB_Params_t *pParams) 378 { 379 380 LVEQNB_Instance_t *pInstance = (LVEQNB_Instance_t *)hInstance; 381 LVM_INT16 bChange = LVM_FALSE; 382 LVM_INT16 i = 0; 383 LVEQNB_Mode_en OperatingModeSave ; 384 385 /* 386 * Check for error conditions 387 */ 388 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 389 { 390 return LVEQNB_NULLADDRESS; 391 } 392 393 if((pParams->NBands !=0) && (pParams->pBandDefinition==LVM_NULL)) 394 { 395 return LVEQNB_NULLADDRESS; 396 } 397 398 OperatingModeSave = pInstance->Params.OperatingMode; 399 400 /* Set the alpha factor of the mixer */ 401 if (pParams->SampleRate != pInstance->Params.SampleRate) 402 { 403 LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[0],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2); 404 LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[1],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2); 405 } 406 407 408 if( (pInstance->Params.NBands != pParams->NBands ) || 409 (pInstance->Params.OperatingMode != pParams->OperatingMode ) || 410 (pInstance->Params.pBandDefinition != pParams->pBandDefinition ) || 411 (pInstance->Params.SampleRate != pParams->SampleRate ) || 412 (pInstance->Params.SourceFormat != pParams->SourceFormat )) 413 { 414 415 bChange = LVM_TRUE; 416 } 417 else 418 { 419 for(i = 0; i < pParams->NBands; i++) 420 { 421 422 if((pInstance->pBandDefinitions[i].Frequency != pParams->pBandDefinition[i].Frequency )|| 423 (pInstance->pBandDefinitions[i].Gain != pParams->pBandDefinition[i].Gain )|| 424 (pInstance->pBandDefinitions[i].QFactor != pParams->pBandDefinition[i].QFactor )) 425 { 426 427 bChange = LVM_TRUE; 428 } 429 } 430 } 431 432 433 if(bChange){ 434 435 /* 436 * If the sample rate has changed clear the history 437 */ 438 if (pInstance->Params.SampleRate != pParams->SampleRate) 439 { 440 LVEQNB_ClearFilterHistory(pInstance); /* Clear the history */ 441 } 442 443 /* 444 * Update the instance parameters 445 */ 446 pInstance->Params = *pParams; 447 448 449 /* 450 * Reset the filters except if the algo is switched off 451 */ 452 if(pParams->OperatingMode != LVEQNB_BYPASS){ 453 /* 454 * Reset the filters as all parameters could have changed 455 */ 456 LVEQNB_SetFilters(pInstance, /* Instance pointer */ 457 pParams); /* New parameters */ 458 459 /* 460 * Update the filters 461 */ 462 LVEQNB_SetCoefficients(pInstance); /* Instance pointer */ 463 } 464 465 if(pParams->OperatingMode != OperatingModeSave) 466 { 467 if(pParams->OperatingMode == LVEQNB_ON) 468 { 469 #ifdef BUILD_FLOAT 470 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0], 1.0f); 471 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1], 0.0f); 472 #else 473 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0],LVM_MAXINT_16); 474 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1],0); 475 #endif 476 pInstance->BypassMixer.MixerStream[0].CallbackSet = 1; 477 pInstance->BypassMixer.MixerStream[1].CallbackSet = 1; 478 } 479 else 480 { 481 /* Stay on the ON operating mode until the transition is done */ 482 pInstance->Params.OperatingMode = LVEQNB_ON; 483 #ifdef BUILD_FLOAT 484 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0], 0.0f); 485 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1], 1.0f); 486 #else 487 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[0],0); 488 LVC_Mixer_SetTarget(&pInstance->BypassMixer.MixerStream[1],LVM_MAXINT_16); 489 #endif 490 pInstance->BypassMixer.MixerStream[0].CallbackSet = 1; 491 pInstance->BypassMixer.MixerStream[1].CallbackSet = 1; 492 } 493 LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[0],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2); 494 LVC_Mixer_VarSlope_SetTimeConstant(&pInstance->BypassMixer.MixerStream[1],LVEQNB_BYPASS_MIXER_TC,(LVM_Fs_en)pParams->SampleRate,2); 495 pInstance->bInOperatingModeTransition = LVM_TRUE; 496 } 497 498 } 499 return(LVEQNB_SUCCESS); 500 } 501 502 503 /****************************************************************************************/ 504 /* */ 505 /* FUNCTION: LVEQNB_BypassMixerCallBack */ 506 /* */ 507 /* DESCRIPTION: */ 508 /* CallBack function of the mixer */ 509 /* transition */ 510 /* */ 511 /****************************************************************************************/ 512 LVM_INT32 LVEQNB_BypassMixerCallBack (void* hInstance, 513 void *pGeneralPurpose, 514 LVM_INT16 CallbackParam) 515 { 516 LVEQNB_Instance_t *pInstance =(LVEQNB_Instance_t *)hInstance; 517 LVM_Callback CallBack = pInstance->Capabilities.CallBack; 518 519 (void) pGeneralPurpose; 520 521 /* 522 * Send an ALGOFF event if the ON->OFF switch transition is finished 523 */ 524 #ifdef BUILD_FLOAT 525 if((LVC_Mixer_GetTarget(&pInstance->BypassMixer.MixerStream[0]) == 0) && 526 (CallbackParam == 0)){ 527 #else 528 if((LVC_Mixer_GetTarget(&pInstance->BypassMixer.MixerStream[0]) == 0x00000000) && 529 (CallbackParam == 0)){ 530 #endif 531 pInstance->Params.OperatingMode = LVEQNB_BYPASS; 532 if (CallBack != LVM_NULL){ 533 CallBack(pInstance->Capabilities.pBundleInstance, LVM_NULL, ALGORITHM_EQNB_ID|LVEQNB_EVENT_ALGOFF); 534 } 535 } 536 537 /* 538 * Exit transition state 539 */ 540 pInstance->bInOperatingModeTransition = LVM_FALSE; 541 542 return 1; 543 } 544