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_Equaliser.h" 27 #include "BIQUAD.h" 28 #include "VectorArithmetic.h" 29 #include "LVCS_Tables.h" 30 31 /************************************************************************************/ 32 /* */ 33 /* FUNCTION: LVCS_EqualiserInit */ 34 /* */ 35 /* DESCRIPTION: */ 36 /* Initialises the equaliser module */ 37 /* */ 38 /* The function selects the coefficients for the filters and clears the data */ 39 /* history. It is also used for re-initialisation when one of the system control */ 40 /* parameters changes but will only change the coefficients and clear the history */ 41 /* if the sample rate or speaker type has changed. */ 42 /* */ 43 /* To avoid excessive testing during the sample processing the biquad type is */ 44 /* set as a callback function in the init routine. */ 45 /* */ 46 /* PARAMETERS: */ 47 /* hInstance Instance Handle */ 48 /* pParams Initialisation parameters */ 49 /* */ 50 /* RETURNS: */ 51 /* LVCS_Success Always succeeds */ 52 /* */ 53 /* NOTES: */ 54 /* */ 55 /************************************************************************************/ 56 #ifdef BUILD_FLOAT 57 LVCS_ReturnStatus_en LVCS_EqualiserInit(LVCS_Handle_t hInstance, 58 LVCS_Params_t *pParams) 59 { 60 61 LVM_UINT16 Offset; 62 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 63 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 64 LVCS_Data_t *pData; 65 LVCS_Coefficient_t *pCoefficients; 66 BQ_FLOAT_Coefs_t Coeffs; 67 const BiquadA012B12CoefsSP_t *pEqualiserCoefTable; 68 69 pData = (LVCS_Data_t *) \ 70 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 71 72 pCoefficients = (LVCS_Coefficient_t *) \ 73 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 74 /* 75 * If the sample rate changes re-initialise the filters 76 */ 77 if ((pInstance->Params.SampleRate != pParams->SampleRate) || 78 (pInstance->Params.SpeakerType != pParams->SpeakerType)) 79 { 80 /* 81 * Setup the filter coefficients and clear the history 82 */ 83 Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1 + LVM_FS_48000))); 84 pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0]; 85 86 /* Left and right filters */ 87 /* Convert incoming coefficients to the required format/ordering */ 88 Coeffs.A0 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A0; 89 Coeffs.A1 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A1; 90 Coeffs.A2 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A2; 91 Coeffs.B1 = (LVM_FLOAT)-pEqualiserCoefTable[Offset].B1; 92 Coeffs.B2 = (LVM_FLOAT)-pEqualiserCoefTable[Offset].B2; 93 94 LoadConst_Float((LVM_INT16)0, /* Value */ 95 (void *)&pData->EqualiserBiquadTaps, /* Destination Cast to void:\ 96 no dereferencing in function*/ 97 /* Number of words */ 98 (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps) / sizeof(LVM_FLOAT))); 99 100 BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance, 101 &pData->EqualiserBiquadTaps, 102 &Coeffs); 103 104 /* Callbacks */ 105 switch(pEqualiserCoefTable[Offset].Scale) 106 { 107 case 13: 108 pConfig->pBiquadCallBack = BQ_2I_D16F32C13_TRC_WRA_01; 109 break; 110 case 14: 111 pConfig->pBiquadCallBack = BQ_2I_D16F32C14_TRC_WRA_01; 112 break; 113 case 15: 114 pConfig->pBiquadCallBack = BQ_2I_D16F32C15_TRC_WRA_01; 115 break; 116 } 117 } 118 119 return(LVCS_SUCCESS); 120 } 121 #else 122 LVCS_ReturnStatus_en LVCS_EqualiserInit(LVCS_Handle_t hInstance, 123 LVCS_Params_t *pParams) 124 { 125 126 LVM_UINT16 Offset; 127 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 128 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 129 LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 130 LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 131 BQ_C16_Coefs_t Coeffs; 132 const BiquadA012B12CoefsSP_t *pEqualiserCoefTable; 133 134 /* 135 * If the sample rate changes re-initialise the filters 136 */ 137 if ((pInstance->Params.SampleRate != pParams->SampleRate) || 138 (pInstance->Params.SpeakerType != pParams->SpeakerType)) 139 { 140 /* 141 * Setup the filter coefficients and clear the history 142 */ 143 Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1+LVM_FS_48000))); 144 pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0]; 145 146 /* Left and right filters */ 147 /* Convert incoming coefficients to the required format/ordering */ 148 Coeffs.A0 = (LVM_INT16) pEqualiserCoefTable[Offset].A0; 149 Coeffs.A1 = (LVM_INT16) pEqualiserCoefTable[Offset].A1; 150 Coeffs.A2 = (LVM_INT16) pEqualiserCoefTable[Offset].A2; 151 Coeffs.B1 = (LVM_INT16)-pEqualiserCoefTable[Offset].B1; 152 Coeffs.B2 = (LVM_INT16)-pEqualiserCoefTable[Offset].B2; 153 154 LoadConst_16((LVM_INT16)0, /* Value */ 155 (void *)&pData->EqualiserBiquadTaps, /* Destination Cast to void:\ 156 no dereferencing in function*/ 157 (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps)/sizeof(LVM_INT16))); /* Number of words */ 158 159 BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance, 160 &pData->EqualiserBiquadTaps, 161 &Coeffs); 162 163 /* Callbacks */ 164 switch(pEqualiserCoefTable[Offset].Scale) 165 { 166 case 13: 167 pConfig->pBiquadCallBack = BQ_2I_D16F32C13_TRC_WRA_01; 168 break; 169 case 14: 170 pConfig->pBiquadCallBack = BQ_2I_D16F32C14_TRC_WRA_01; 171 break; 172 case 15: 173 pConfig->pBiquadCallBack = BQ_2I_D16F32C15_TRC_WRA_01; 174 break; 175 } 176 } 177 178 return(LVCS_SUCCESS); 179 } 180 #endif 181 /************************************************************************************/ 182 /* */ 183 /* FUNCTION: LVCS_Equaliser */ 184 /* */ 185 /* DESCRIPTION: */ 186 /* Apply the equaliser filter. */ 187 /* */ 188 /* PARAMETERS: */ 189 /* hInstance Instance Handle */ 190 /* pInputOutput Pointer to the input/output buffer */ 191 /* NumSamples The number of samples to process */ 192 /* */ 193 /* RETURNS: */ 194 /* LVCS_Success Always succeeds */ 195 /* */ 196 /* NOTES: */ 197 /* 1. Always processes in place. */ 198 /* */ 199 /************************************************************************************/ 200 #ifdef BUILD_FLOAT 201 LVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t hInstance, 202 LVM_FLOAT *pInputOutput, 203 LVM_UINT16 NumSamples) 204 { 205 206 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 207 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 208 LVCS_Coefficient_t *pCoefficients; 209 210 211 pCoefficients = (LVCS_Coefficient_t *) \ 212 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 213 214 215 /* 216 * Check if the equaliser is required 217 */ 218 if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0) 219 { 220 /* Apply filter to the left and right channels */ 221 (pConfig->pBiquadCallBack)((Biquad_FLOAT_Instance_t*) \ 222 &pCoefficients->EqualiserBiquadInstance, 223 (LVM_FLOAT *)pInputOutput, 224 (LVM_FLOAT *)pInputOutput, 225 (LVM_INT16)NumSamples); 226 } 227 228 return(LVCS_SUCCESS); 229 } 230 #else 231 LVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t hInstance, 232 LVM_INT16 *pInputOutput, 233 LVM_UINT16 NumSamples) 234 { 235 236 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 237 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 238 LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 239 240 241 /* 242 * Check if the equaliser is required 243 */ 244 if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0) 245 { 246 /* Apply filter to the left and right channels */ 247 (pConfig->pBiquadCallBack)((Biquad_Instance_t*)&pCoefficients->EqualiserBiquadInstance, 248 (LVM_INT16 *)pInputOutput, 249 (LVM_INT16 *)pInputOutput, 250 (LVM_INT16)NumSamples); 251 } 252 253 return(LVCS_SUCCESS); 254 } 255 #endif 256
