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 53 LVCS_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 = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 61 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 62 FO_C16_Coefs_t CoeffsMid; 63 BQ_C16_Coefs_t CoeffsSide; 64 const BiquadA012B12CoefsSP_t *pSESideCoefs; 65 66 /* 67 * If the sample rate or speaker type has changed update the filters 68 */ 69 if ((pInstance->Params.SampleRate != pParams->SampleRate) || 70 (pInstance->Params.SpeakerType != pParams->SpeakerType)) 71 { 72 /* 73 * Set the filter coefficients based on the sample rate 74 */ 75 /* Mid filter */ 76 Offset = (LVM_UINT16)pParams->SampleRate; 77 78 /* Convert incoming coefficients to the required format/ordering */ 79 CoeffsMid.A0 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A0; 80 CoeffsMid.A1 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A1; 81 CoeffsMid.B1 = (LVM_INT16)-LVCS_SEMidCoefTable[Offset].B1; 82 83 /* Clear the taps */ 84 LoadConst_16(0, /* Value */ 85 (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\ 86 no dereferencing in function*/ 87 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid)/sizeof(LVM_UINT16))); /* Number of words */ 88 89 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid, 90 &pData->SEBiquadTapsMid, 91 &CoeffsMid); 92 93 /* Callbacks */ 94 if(LVCS_SEMidCoefTable[Offset].Scale==15) 95 { 96 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01; 97 } 98 99 Offset = (LVM_UINT16)(pParams->SampleRate); 100 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0]; 101 102 /* Side filter */ 103 /* Convert incoming coefficients to the required format/ordering */ 104 CoeffsSide.A0 = (LVM_INT16) pSESideCoefs[Offset].A0; 105 CoeffsSide.A1 = (LVM_INT16) pSESideCoefs[Offset].A1; 106 CoeffsSide.A2 = (LVM_INT16) pSESideCoefs[Offset].A2; 107 CoeffsSide.B1 = (LVM_INT16)-pSESideCoefs[Offset].B1; 108 CoeffsSide.B2 = (LVM_INT16)-pSESideCoefs[Offset].B2; 109 110 /* Clear the taps */ 111 LoadConst_16(0, /* Value */ 112 (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\ 113 no dereferencing in function*/ 114 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide)/sizeof(LVM_UINT16))); /* Number of words */ 115 116 117 /* Callbacks */ 118 switch(pSESideCoefs[Offset].Scale) 119 { 120 case 14: 121 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 122 &pData->SEBiquadTapsSide, 123 &CoeffsSide); 124 125 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01; 126 break; 127 case 15: 128 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide, 129 &pData->SEBiquadTapsSide, 130 &CoeffsSide); 131 132 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01; 133 break; 134 } 135 136 } 137 138 139 return(LVCS_SUCCESS); 140 } 141 142 /************************************************************************************/ 143 /* */ 144 /* FUNCTION: LVCS_StereoEnhance */ 145 /* */ 146 /* DESCRIPTION: */ 147 /* Enhance the stereo image in the input samples based on the following block */ 148 /* diagram: */ 149 /* */ 150 /* ________ */ 151 /* ________ | | ________ */ 152 /* | | Middle | Treble | | | */ 153 /* | |---------->| Boost |-------->| | */ 154 /* | Stereo | |________| | M & S | */ 155 /* -->| to | ________ | to |--> */ 156 /* | M & S | Side | | | Stereo | */ 157 /* | |---------->| Side |-------->| | */ 158 /* |________| | Boost | |________| */ 159 /* |________| */ 160 /* */ 161 /* */ 162 /* If the input signal is a mono signal there will be no side signal and hence */ 163 /* the side filter will not be run. In mobile speaker mode the middle filter is */ 164 /* not required and the Trebble boost filter is replaced by a simple gain block. */ 165 /* */ 166 /* */ 167 /* PARAMETERS: */ 168 /* hInstance Instance Handle */ 169 /* pInData Pointer to the input data */ 170 /* pOutData Pointer to the output data */ 171 /* NumSamples Number of samples to process */ 172 /* */ 173 /* RETURNS: */ 174 /* LVCS_Success Always succeeds */ 175 /* */ 176 /* NOTES: */ 177 /* 1. The side filter is not used in Mobile Speaker mode */ 178 /* */ 179 /************************************************************************************/ 180 181 LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance, 182 const LVM_INT16 *pInData, 183 LVM_INT16 *pOutData, 184 LVM_UINT16 NumSamples) 185 { 186 187 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 188 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer; 189 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 190 LVM_INT16 *pScratch = (LVM_INT16 *)pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress; 191 192 /* 193 * Check if the Stereo Enhancer is enabled 194 */ 195 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0) 196 { 197 /* 198 * Convert from stereo to middle and side 199 */ 200 From2iToMS_16x16(pInData, 201 pScratch, 202 pScratch+NumSamples, 203 (LVM_INT16)NumSamples); 204 205 /* 206 * Apply filter to the middle signal 207 */ 208 if (pInstance->OutputDevice == LVCS_HEADPHONE) 209 { 210 (pConfig->pBiquadCallBack_Mid)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceMid, 211 (LVM_INT16 *)pScratch, 212 (LVM_INT16 *)pScratch, 213 (LVM_INT16)NumSamples); 214 } 215 else 216 { 217 Mult3s_16x16(pScratch, /* Source */ 218 (LVM_INT16)pConfig->MidGain, /* Gain */ 219 pScratch, /* Destination */ 220 (LVM_INT16)NumSamples); /* Number of samples */ 221 } 222 223 /* 224 * Apply the filter the side signal only in stereo mode for headphones 225 * and in all modes for mobile speakers 226 */ 227 if (pInstance->Params.SourceFormat == LVCS_STEREO) 228 { 229 (pConfig->pBiquadCallBack_Side)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceSide, 230 (LVM_INT16 *)(pScratch + NumSamples), 231 (LVM_INT16 *)(pScratch + NumSamples), 232 (LVM_INT16)NumSamples); 233 } 234 235 /* 236 * Convert from middle and side to stereo 237 */ 238 MSTo2i_Sat_16x16(pScratch, 239 pScratch+NumSamples, 240 pOutData, 241 (LVM_INT16)NumSamples); 242 243 } 244 else 245 { 246 /* 247 * The stereo enhancer is disabled so just copy the data 248 */ 249 Copy_16((LVM_INT16 *)pInData, /* Source */ 250 (LVM_INT16 *)pOutData, /* Destination */ 251 (LVM_INT16)(2*NumSamples)); /* Left and right */ 252 253 } 254 255 return(LVCS_SUCCESS); 256 } 257 258 259 260 261
