| /* |
| * 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 "LVCS.h" |
| #include "LVCS_Private.h" |
| #include "LVCS_StereoEnhancer.h" |
| #include "VectorArithmetic.h" |
| #include "LVCS_Tables.h" |
| |
| /************************************************************************************/ |
| /* */ |
| /* FUNCTION: LVCS_StereoEnhanceInit */ |
| /* */ |
| /* DESCRIPTION: */ |
| /* Initialises the stereo enhancement module based on the sample rate. */ |
| /* */ |
| /* The function selects the coefficients for the filters and clears the data */ |
| /* history. It is also used for re-initialisation when one of the system control */ |
| /* parameters changes but will only change the coefficients and clear the history */ |
| /* if the sample rate or speaker type has changed. */ |
| /* */ |
| /* PARAMETERS: */ |
| /* hInstance Instance Handle */ |
| /* pParams Initialisation parameters */ |
| /* */ |
| /* RETURNS: */ |
| /* LVCS_Success Always succeeds */ |
| /* */ |
| /* NOTES: */ |
| /* */ |
| /************************************************************************************/ |
| |
| LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance, |
| LVCS_Params_t *pParams) |
| { |
| |
| LVM_UINT16 Offset; |
| LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; |
| LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; |
| LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; |
| LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; |
| FO_C16_Coefs_t CoeffsMid; |
| BQ_C16_Coefs_t CoeffsSide; |
| const BiquadA012B12CoefsSP_t *pSESideCoefs; |
| |
| /* |
| * If the sample rate or speaker type has changed update the filters |
| */ |
| if ((pInstance->Params.SampleRate != pParams->SampleRate) || |
| (pInstance->Params.SpeakerType != pParams->SpeakerType)) |
| { |
| /* |
| * Set the filter coefficients based on the sample rate |
| */ |
| /* Mid filter */ |
| Offset = (LVM_UINT16)pParams->SampleRate; |
| |
| /* Convert incoming coefficients to the required format/ordering */ |
| CoeffsMid.A0 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A0; |
| CoeffsMid.A1 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A1; |
| CoeffsMid.B1 = (LVM_INT16)-LVCS_SEMidCoefTable[Offset].B1; |
| |
| /* Clear the taps */ |
| LoadConst_16(0, /* Value */ |
| (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\ |
| no dereferencing in function*/ |
| (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid)/sizeof(LVM_UINT16))); /* Number of words */ |
| |
| FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid, |
| &pData->SEBiquadTapsMid, |
| &CoeffsMid); |
| |
| /* Callbacks */ |
| if(LVCS_SEMidCoefTable[Offset].Scale==15) |
| { |
| pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01; |
| } |
| |
| Offset = (LVM_UINT16)(pParams->SampleRate); |
| pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0]; |
| |
| /* Side filter */ |
| /* Convert incoming coefficients to the required format/ordering */ |
| CoeffsSide.A0 = (LVM_INT16) pSESideCoefs[Offset].A0; |
| CoeffsSide.A1 = (LVM_INT16) pSESideCoefs[Offset].A1; |
| CoeffsSide.A2 = (LVM_INT16) pSESideCoefs[Offset].A2; |
| CoeffsSide.B1 = (LVM_INT16)-pSESideCoefs[Offset].B1; |
| CoeffsSide.B2 = (LVM_INT16)-pSESideCoefs[Offset].B2; |
| |
| /* Clear the taps */ |
| LoadConst_16(0, /* Value */ |
| (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\ |
| no dereferencing in function*/ |
| (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide)/sizeof(LVM_UINT16))); /* Number of words */ |
| |
| |
| /* Callbacks */ |
| switch(pSESideCoefs[Offset].Scale) |
| { |
| case 14: |
| BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, |
| &pData->SEBiquadTapsSide, |
| &CoeffsSide); |
| |
| pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01; |
| break; |
| case 15: |
| BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, |
| &pData->SEBiquadTapsSide, |
| &CoeffsSide); |
| |
| pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01; |
| break; |
| } |
| |
| } |
| |
| |
| return(LVCS_SUCCESS); |
| } |
| |
| /************************************************************************************/ |
| /* */ |
| /* FUNCTION: LVCS_StereoEnhance */ |
| /* */ |
| /* DESCRIPTION: */ |
| /* Enhance the stereo image in the input samples based on the following block */ |
| /* diagram: */ |
| /* */ |
| /* ________ */ |
| /* ________ | | ________ */ |
| /* | | Middle | Treble | | | */ |
| /* | |---------->| Boost |-------->| | */ |
| /* | Stereo | |________| | M & S | */ |
| /* -->| to | ________ | to |--> */ |
| /* | M & S | Side | | | Stereo | */ |
| /* | |---------->| Side |-------->| | */ |
| /* |________| | Boost | |________| */ |
| /* |________| */ |
| /* */ |
| /* */ |
| /* If the input signal is a mono signal there will be no side signal and hence */ |
| /* the side filter will not be run. In mobile speaker mode the middle filter is */ |
| /* not required and the Trebble boost filter is replaced by a simple gain block. */ |
| /* */ |
| /* */ |
| /* PARAMETERS: */ |
| /* hInstance Instance Handle */ |
| /* pInData Pointer to the input data */ |
| /* pOutData Pointer to the output data */ |
| /* NumSamples Number of samples to process */ |
| /* */ |
| /* RETURNS: */ |
| /* LVCS_Success Always succeeds */ |
| /* */ |
| /* NOTES: */ |
| /* 1. The side filter is not used in Mobile Speaker mode */ |
| /* */ |
| /************************************************************************************/ |
| |
| LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance, |
| const LVM_INT16 *pInData, |
| LVM_INT16 *pOutData, |
| LVM_UINT16 NumSamples) |
| { |
| |
| LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; |
| LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; |
| LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; |
| LVM_INT16 *pScratch = (LVM_INT16 *)pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress; |
| |
| /* |
| * Check if the Stereo Enhancer is enabled |
| */ |
| if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0) |
| { |
| /* |
| * Convert from stereo to middle and side |
| */ |
| From2iToMS_16x16(pInData, |
| pScratch, |
| pScratch+NumSamples, |
| (LVM_INT16)NumSamples); |
| |
| /* |
| * Apply filter to the middle signal |
| */ |
| if (pInstance->OutputDevice == LVCS_HEADPHONE) |
| { |
| (pConfig->pBiquadCallBack_Mid)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceMid, |
| (LVM_INT16 *)pScratch, |
| (LVM_INT16 *)pScratch, |
| (LVM_INT16)NumSamples); |
| } |
| else |
| { |
| Mult3s_16x16(pScratch, /* Source */ |
| (LVM_INT16)pConfig->MidGain, /* Gain */ |
| pScratch, /* Destination */ |
| (LVM_INT16)NumSamples); /* Number of samples */ |
| } |
| |
| /* |
| * Apply the filter the side signal only in stereo mode for headphones |
| * and in all modes for mobile speakers |
| */ |
| if (pInstance->Params.SourceFormat == LVCS_STEREO) |
| { |
| (pConfig->pBiquadCallBack_Side)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceSide, |
| (LVM_INT16 *)(pScratch + NumSamples), |
| (LVM_INT16 *)(pScratch + NumSamples), |
| (LVM_INT16)NumSamples); |
| } |
| |
| /* |
| * Convert from middle and side to stereo |
| */ |
| MSTo2i_Sat_16x16(pScratch, |
| pScratch+NumSamples, |
| pOutData, |
| (LVM_INT16)NumSamples); |
| |
| } |
| else |
| { |
| /* |
| * The stereo enhancer is disabled so just copy the data |
| */ |
| Copy_16((LVM_INT16 *)pInData, /* Source */ |
| (LVM_INT16 *)pOutData, /* Destination */ |
| (LVM_INT16)(2*NumSamples)); /* Left and right */ |
| |
| } |
| |
| return(LVCS_SUCCESS); |
| } |
| |
| |
| |
| |
