12c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* 22c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Copyright (C) 2004-2010 NXP Software 32c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Copyright (C) 2010 The Android Open Source Project 42c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * 52c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Licensed under the Apache License, Version 2.0 (the "License"); 62c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * you may not use this file except in compliance with the License. 72c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * You may obtain a copy of the License at 82c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * 92c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * http://www.apache.org/licenses/LICENSE-2.0 102c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * 112c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Unless required by applicable law or agreed to in writing, software 122c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * distributed under the License is distributed on an "AS IS" BASIS, 132c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 142c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * See the License for the specific language governing permissions and 152c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * limitations under the License. 162c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent */ 172c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 182c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 192c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 202c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Includes */ 212c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 222c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 232c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 242c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "LVCS.h" 252c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "LVCS_Private.h" 262c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "LVCS_Equaliser.h" 272c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "BIQUAD.h" 282c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "VectorArithmetic.h" 292c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "LVCS_Tables.h" 302c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 312c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 322c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 332c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* FUNCTION: LVCS_EqualiserInit */ 342c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 352c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* DESCRIPTION: */ 362c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Initialises the equaliser module */ 372c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 382c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* The function selects the coefficients for the filters and clears the data */ 392c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* history. It is also used for re-initialisation when one of the system control */ 402c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* parameters changes but will only change the coefficients and clear the history */ 412c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* if the sample rate or speaker type has changed. */ 422c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 432c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* To avoid excessive testing during the sample processing the biquad type is */ 442c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* set as a callback function in the init routine. */ 452c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 462c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* PARAMETERS: */ 472c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* hInstance Instance Handle */ 482c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* pParams Initialisation parameters */ 492c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 502c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* RETURNS: */ 512c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* LVCS_Success Always succeeds */ 522c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 532c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* NOTES: */ 542c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 552c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 562c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 572c87e9c923b0362fabf8c97ff63997542394c428Eric LaurentLVCS_ReturnStatus_en LVCS_EqualiserInit(LVCS_Handle_t hInstance, 582c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Params_t *pParams) 592c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent{ 602c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 612c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT16 Offset; 622c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 632c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 642c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress; 652c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 662c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent BQ_C16_Coefs_t Coeffs; 672c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent const BiquadA012B12CoefsSP_t *pEqualiserCoefTable; 682c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 692c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* 702c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * If the sample rate changes re-initialise the filters 712c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent */ 722c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if ((pInstance->Params.SampleRate != pParams->SampleRate) || 732c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (pInstance->Params.SpeakerType != pParams->SpeakerType)) 742c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 752c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* 762c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Setup the filter coefficients and clear the history 772c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent */ 782c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1+LVM_FS_48000))); 792c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0]; 802c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 812c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Left and right filters */ 822c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Convert incoming coefficients to the required format/ordering */ 832c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Coeffs.A0 = (LVM_INT16) pEqualiserCoefTable[Offset].A0; 842c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Coeffs.A1 = (LVM_INT16) pEqualiserCoefTable[Offset].A1; 852c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Coeffs.A2 = (LVM_INT16) pEqualiserCoefTable[Offset].A2; 862c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Coeffs.B1 = (LVM_INT16)-pEqualiserCoefTable[Offset].B1; 872c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Coeffs.B2 = (LVM_INT16)-pEqualiserCoefTable[Offset].B2; 882c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 893374105cc0660c468c4d74de94e4546495843376Eric Laurent LoadConst_16((LVM_INT16)0, /* Value */ 903374105cc0660c468c4d74de94e4546495843376Eric Laurent (void *)&pData->EqualiserBiquadTaps, /* Destination Cast to void:\ 913374105cc0660c468c4d74de94e4546495843376Eric Laurent no dereferencing in function*/ 922c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps)/sizeof(LVM_INT16))); /* Number of words */ 932c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 942c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance, 952c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent &pData->EqualiserBiquadTaps, 962c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent &Coeffs); 972c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 982c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Callbacks */ 992c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent switch(pEqualiserCoefTable[Offset].Scale) 1002c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1012c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent case 13: 1022c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pConfig->pBiquadCallBack = BQ_2I_D16F32C13_TRC_WRA_01; 1032c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent break; 1042c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent case 14: 1052c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pConfig->pBiquadCallBack = BQ_2I_D16F32C14_TRC_WRA_01; 1062c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent break; 1072c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent case 15: 1082c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pConfig->pBiquadCallBack = BQ_2I_D16F32C15_TRC_WRA_01; 1092c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent break; 1102c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1112c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1122c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1132c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent return(LVCS_SUCCESS); 1142c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent} 1152c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1162c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 1172c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1182c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* FUNCTION: LVCS_Equaliser */ 1192c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1202c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* DESCRIPTION: */ 1212c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Apply the equaliser filter. */ 1222c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1232c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* PARAMETERS: */ 1242c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* hInstance Instance Handle */ 1252c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* pInputOutput Pointer to the input/output buffer */ 1262c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* NumSamples The number of samples to process */ 1272c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1282c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* RETURNS: */ 1292c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* LVCS_Success Always succeeds */ 1302c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1312c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* NOTES: */ 1322c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* 1. Always processes in place. */ 1332c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1342c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 1352c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1362c87e9c923b0362fabf8c97ff63997542394c428Eric LaurentLVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t hInstance, 1372c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 *pInputOutput, 1382c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT16 NumSamples) 1392c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent{ 1402c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1412c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance; 1422c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser; 1432c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress; 1442c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1452c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1462c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* 1472c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent * Check if the equaliser is required 1482c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent */ 1492c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0) 1502c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1512c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Apply filter to the left and right channels */ 1522c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (pConfig->pBiquadCallBack)((Biquad_Instance_t*)&pCoefficients->EqualiserBiquadInstance, 1532c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (LVM_INT16 *)pInputOutput, 1542c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (LVM_INT16 *)pInputOutput, 1552c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent (LVM_INT16)NumSamples); 1562c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1572c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1582c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent return(LVCS_SUCCESS); 1592c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent} 1602c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 161