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