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 "LVCS.h" 25#include "LVCS_Private.h" 26#include "LVCS_StereoEnhancer.h" 27#include "VectorArithmetic.h" 28#include "LVCS_Tables.h" 29 30/************************************************************************************/ 31/* */ 32/* FUNCTION: LVCS_StereoEnhanceInit */ 33/* */ 34/* DESCRIPTION: */ 35/* Initialises the stereo enhancement module based on the sample rate. */ 36/* */ 37/* The function selects the coefficients for the filters and clears the data */ 38/* history. It is also used for re-initialisation when one of the system control */ 39/* parameters changes but will only change the coefficients and clear the history */ 40/* if the sample rate or speaker type has changed. */ 41/* */ 42/* PARAMETERS: */ 43/* hInstance Instance Handle */ 44/* pParams Initialisation parameters */ 45/* */ 46/* RETURNS: */ 47/* LVCS_Success Always succeeds */ 48/* */ 49/* NOTES: */ 50/* */ 51/************************************************************************************/ 52#ifdef BUILD_FLOAT 53LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance, 54 LVCS_Params_t *pParams) 55{ 56 57 LVM_UINT16 Offset; 58 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 59 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; 60 LVCS_Data_t *pData; 61 LVCS_Coefficient_t *pCoefficient; 62 FO_FLOAT_Coefs_t CoeffsMid; 63 BQ_FLOAT_Coefs_t CoeffsSide; 64 const BiquadA012B12CoefsSP_t *pSESideCoefs; 65 66 67 pData = (LVCS_Data_t *) \ 68 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 69 70 pCoefficient = (LVCS_Coefficient_t *) \ 71 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 72 73 /* 74 * If the sample rate or speaker type has changed update the filters 75 */ 76 if ((pInstance->Params.SampleRate != pParams->SampleRate) || 77 (pInstance->Params.SpeakerType != pParams->SpeakerType)) 78 { 79 /* 80 * Set the filter coefficients based on the sample rate 81 */ 82 /* Mid filter */ 83 Offset = (LVM_UINT16)pParams->SampleRate; 84 85 /* Convert incoming coefficients to the required format/ordering */ 86 CoeffsMid.A0 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A0; 87 CoeffsMid.A1 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A1; 88 CoeffsMid.B1 = (LVM_FLOAT)-LVCS_SEMidCoefTable[Offset].B1; 89 90 /* Clear the taps */ 91 LoadConst_Float(0, /* Value */ 92 (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\ 93 no dereferencing in function*/ 94 /* Number of words */ 95 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid) / sizeof(LVM_FLOAT))); 96 97 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid, 98 &pData->SEBiquadTapsMid, 99 &CoeffsMid); 100 101 /* Callbacks */ 102 if(LVCS_SEMidCoefTable[Offset].Scale == 15) 103 { 104 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01; 105 } 106 107 Offset = (LVM_UINT16)(pParams->SampleRate); 108 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0]; 109 110 /* Side filter */ 111 /* Convert incoming coefficients to the required format/ordering */ 112 CoeffsSide.A0 = (LVM_FLOAT) pSESideCoefs[Offset].A0; 113 CoeffsSide.A1 = (LVM_FLOAT) pSESideCoefs[Offset].A1; 114 CoeffsSide.A2 = (LVM_FLOAT) pSESideCoefs[Offset].A2; 115 CoeffsSide.B1 = (LVM_FLOAT)-pSESideCoefs[Offset].B1; 116 CoeffsSide.B2 = (LVM_FLOAT)-pSESideCoefs[Offset].B2; 117 118 /* Clear the taps */ 119 LoadConst_Float(0, /* Value */ 120 (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\ 121 no dereferencing in function*/ 122 /* Number of words */ 123 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide) / sizeof(LVM_FLOAT))); 124 /* Callbacks */ 125 switch(pSESideCoefs[Offset].Scale) 126 { 127 case 14: 128 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 129 &pData->SEBiquadTapsSide, 130 &CoeffsSide); 131 132 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01; 133 break; 134 case 15: 135 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 136 &pData->SEBiquadTapsSide, 137 &CoeffsSide); 138 139 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01; 140 break; 141 } 142 143 } 144 145 146 return(LVCS_SUCCESS); 147} 148#else 149LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance, 150 LVCS_Params_t *pParams) 151{ 152 153 LVM_UINT16 Offset; 154 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 155 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; 156 LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 157 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 158 FO_C16_Coefs_t CoeffsMid; 159 BQ_C16_Coefs_t CoeffsSide; 160 const BiquadA012B12CoefsSP_t *pSESideCoefs; 161 162 /* 163 * If the sample rate or speaker type has changed update the filters 164 */ 165 if ((pInstance->Params.SampleRate != pParams->SampleRate) || 166 (pInstance->Params.SpeakerType != pParams->SpeakerType)) 167 { 168 /* 169 * Set the filter coefficients based on the sample rate 170 */ 171 /* Mid filter */ 172 Offset = (LVM_UINT16)pParams->SampleRate; 173 174 /* Convert incoming coefficients to the required format/ordering */ 175 CoeffsMid.A0 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A0; 176 CoeffsMid.A1 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A1; 177 CoeffsMid.B1 = (LVM_INT16)-LVCS_SEMidCoefTable[Offset].B1; 178 179 /* Clear the taps */ 180 LoadConst_16(0, /* Value */ 181 (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\ 182 no dereferencing in function*/ 183 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid)/sizeof(LVM_UINT16))); /* Number of words */ 184 185 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid, 186 &pData->SEBiquadTapsMid, 187 &CoeffsMid); 188 189 /* Callbacks */ 190 if(LVCS_SEMidCoefTable[Offset].Scale==15) 191 { 192 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01; 193 } 194 195 Offset = (LVM_UINT16)(pParams->SampleRate); 196 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0]; 197 198 /* Side filter */ 199 /* Convert incoming coefficients to the required format/ordering */ 200 CoeffsSide.A0 = (LVM_INT16) pSESideCoefs[Offset].A0; 201 CoeffsSide.A1 = (LVM_INT16) pSESideCoefs[Offset].A1; 202 CoeffsSide.A2 = (LVM_INT16) pSESideCoefs[Offset].A2; 203 CoeffsSide.B1 = (LVM_INT16)-pSESideCoefs[Offset].B1; 204 CoeffsSide.B2 = (LVM_INT16)-pSESideCoefs[Offset].B2; 205 206 /* Clear the taps */ 207 LoadConst_16(0, /* Value */ 208 (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\ 209 no dereferencing in function*/ 210 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide)/sizeof(LVM_UINT16))); /* Number of words */ 211 212 213 /* Callbacks */ 214 switch(pSESideCoefs[Offset].Scale) 215 { 216 case 14: 217 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 218 &pData->SEBiquadTapsSide, 219 &CoeffsSide); 220 221 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01; 222 break; 223 case 15: 224 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 225 &pData->SEBiquadTapsSide, 226 &CoeffsSide); 227 228 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01; 229 break; 230 } 231 232 } 233 234 235 return(LVCS_SUCCESS); 236} 237#endif 238/************************************************************************************/ 239/* */ 240/* FUNCTION: LVCS_StereoEnhance */ 241/* */ 242/* DESCRIPTION: */ 243/* Enhance the stereo image in the input samples based on the following block */ 244/* diagram: */ 245/* */ 246/* ________ */ 247/* ________ | | ________ */ 248/* | | Middle | Treble | | | */ 249/* | |---------->| Boost |-------->| | */ 250/* | Stereo | |________| | M & S | */ 251/* -->| to | ________ | to |--> */ 252/* | M & S | Side | | | Stereo | */ 253/* | |---------->| Side |-------->| | */ 254/* |________| | Boost | |________| */ 255/* |________| */ 256/* */ 257/* */ 258/* If the input signal is a mono signal there will be no side signal and hence */ 259/* the side filter will not be run. In mobile speaker mode the middle filter is */ 260/* not required and the Trebble boost filter is replaced by a simple gain block. */ 261/* */ 262/* */ 263/* PARAMETERS: */ 264/* hInstance Instance Handle */ 265/* pInData Pointer to the input data */ 266/* pOutData Pointer to the output data */ 267/* NumSamples Number of samples to process */ 268/* */ 269/* RETURNS: */ 270/* LVCS_Success Always succeeds */ 271/* */ 272/* NOTES: */ 273/* 1. The side filter is not used in Mobile Speaker mode */ 274/* */ 275/************************************************************************************/ 276#ifdef BUILD_FLOAT 277LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance, 278 const LVM_FLOAT *pInData, 279 LVM_FLOAT *pOutData, 280 LVM_UINT16 NumSamples) 281{ 282 283 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 284 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; 285 LVCS_Coefficient_t *pCoefficient; 286 LVM_FLOAT *pScratch; 287 288 pCoefficient = (LVCS_Coefficient_t *) \ 289 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 290 291 pScratch = (LVM_FLOAT *) \ 292 pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress; 293 /* 294 * Check if the Stereo Enhancer is enabled 295 */ 296 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0) 297 { 298 /* 299 * Convert from stereo to middle and side 300 */ 301 From2iToMS_Float(pInData, 302 pScratch, 303 pScratch + NumSamples, 304 (LVM_INT16)NumSamples); 305 306 /* 307 * Apply filter to the middle signal 308 */ 309 if (pInstance->OutputDevice == LVCS_HEADPHONE) 310 { 311 (pConfig->pBiquadCallBack_Mid)((Biquad_FLOAT_Instance_t*)\ 312 &pCoefficient->SEBiquadInstanceMid, 313 (LVM_FLOAT *)pScratch, 314 (LVM_FLOAT *)pScratch, 315 (LVM_INT16)NumSamples); 316 } 317 else 318 { 319 Mult3s_Float(pScratch, /* Source */ 320 (LVM_FLOAT)pConfig->MidGain, /* Gain */ 321 pScratch, /* Destination */ 322 (LVM_INT16)NumSamples); /* Number of samples */ 323 } 324 325 /* 326 * Apply the filter the side signal only in stereo mode for headphones 327 * and in all modes for mobile speakers 328 */ 329 if (pInstance->Params.SourceFormat == LVCS_STEREO) 330 { 331 (pConfig->pBiquadCallBack_Side)((Biquad_FLOAT_Instance_t*) \ 332 &pCoefficient->SEBiquadInstanceSide, 333 (LVM_FLOAT *)(pScratch + NumSamples), 334 (LVM_FLOAT *)(pScratch + NumSamples), 335 (LVM_INT16)NumSamples); 336 } 337 338 /* 339 * Convert from middle and side to stereo 340 */ 341 MSTo2i_Sat_Float(pScratch, 342 pScratch + NumSamples, 343 pOutData, 344 (LVM_INT16)NumSamples); 345 346 } 347 else 348 { 349 /* 350 * The stereo enhancer is disabled so just copy the data 351 */ 352 Copy_Float((LVM_FLOAT *)pInData, /* Source */ 353 (LVM_FLOAT *)pOutData, /* Destination */ 354 (LVM_INT16)(2 * NumSamples)); /* Left and right */ 355 } 356 357 return(LVCS_SUCCESS); 358} 359#else 360LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance, 361 const LVM_INT16 *pInData, 362 LVM_INT16 *pOutData, 363 LVM_UINT16 NumSamples) 364{ 365 366 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 367 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; 368 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 369 LVM_INT16 *pScratch = (LVM_INT16 *)pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress; 370 371 /* 372 * Check if the Stereo Enhancer is enabled 373 */ 374 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0) 375 { 376 /* 377 * Convert from stereo to middle and side 378 */ 379 From2iToMS_16x16(pInData, 380 pScratch, 381 pScratch+NumSamples, 382 (LVM_INT16)NumSamples); 383 384 /* 385 * Apply filter to the middle signal 386 */ 387 if (pInstance->OutputDevice == LVCS_HEADPHONE) 388 { 389 (pConfig->pBiquadCallBack_Mid)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceMid, 390 (LVM_INT16 *)pScratch, 391 (LVM_INT16 *)pScratch, 392 (LVM_INT16)NumSamples); 393 } 394 else 395 { 396 Mult3s_16x16(pScratch, /* Source */ 397 (LVM_INT16)pConfig->MidGain, /* Gain */ 398 pScratch, /* Destination */ 399 (LVM_INT16)NumSamples); /* Number of samples */ 400 } 401 402 /* 403 * Apply the filter the side signal only in stereo mode for headphones 404 * and in all modes for mobile speakers 405 */ 406 if (pInstance->Params.SourceFormat == LVCS_STEREO) 407 { 408 (pConfig->pBiquadCallBack_Side)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceSide, 409 (LVM_INT16 *)(pScratch + NumSamples), 410 (LVM_INT16 *)(pScratch + NumSamples), 411 (LVM_INT16)NumSamples); 412 } 413 414 /* 415 * Convert from middle and side to stereo 416 */ 417 MSTo2i_Sat_16x16(pScratch, 418 pScratch+NumSamples, 419 pOutData, 420 (LVM_INT16)NumSamples); 421 422 } 423 else 424 { 425 /* 426 * The stereo enhancer is disabled so just copy the data 427 */ 428 Copy_16((LVM_INT16 *)pInData, /* Source */ 429 (LVM_INT16 *)pOutData, /* Destination */ 430 (LVM_INT16)(2*NumSamples)); /* Left and right */ 431 432 } 433 434 return(LVCS_SUCCESS); 435} 436#endif 437