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#include "LVPSA_QPD.h" 192c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent#include "LVPSA_Private.h" 202c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 212c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 222c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 232c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* FUNCTION: LVPSA_QPD_WritePeak */ 242c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 252c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* DESCRIPTION: */ 262c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Write a level value in the buffer in the corresponding band. */ 272c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 282c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* PARAMETERS: */ 292c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* pInst Pointer to the LVPSA instance */ 302c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* ppWrite Pointer to pointer to the buffer */ 312c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* CallNumber Number of the band the value should be written in */ 322c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Value Value to write in the buffer */ 332c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 342c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* RETURNS: void */ 352c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 362c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 372c87e9c923b0362fabf8c97ff63997542394c428Eric Laurentvoid LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 382c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT8 **ppWrite, 392c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 BandIndex, 402c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 Value ); 412c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 422c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 432c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 442c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 452c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 462c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* FUNCTION: LVPSA_QPD_Process */ 472c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 482c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* DESCRIPTION: */ 492c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Apply downsampling, post gain, quasi peak filtering and write the levels values */ 502c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* in the buffer every 20 ms. */ 512c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 522c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* PARAMETERS: */ 532c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 542c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* RETURNS: void */ 552c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 562c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 572c87e9c923b0362fabf8c97ff63997542394c428Eric Laurentvoid LVPSA_QPD_Process ( void *hInstance, 582c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 *pInSamps, 592c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 numSamples, 602c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 BandIndex) 612c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent{ 622c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 632c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /****************************************************************************** 642c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent PARAMETERS 652c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent *******************************************************************************/ 662c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 672c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent QPD_State_t *pQPDState = (QPD_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex]; 682c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 692c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Pointer to taps */ 702c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32* pDelay = pQPDState->pDelay; 712c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 722c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Parameters needed during quasi peak calculations */ 732c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 X0; 742c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 temp,temp2; 752c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 accu; 762c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 Xg0; 772c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 D0; 782c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 V0 = (LVM_INT16)(*pDelay); 792c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 802c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Filter's coef */ 812c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 Kp = pQPDState->Coefs[0]; 822c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 Km = pQPDState->Coefs[1]; 832c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 842c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 ii = numSamples; 852c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 862c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer; 872c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount; 882c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor; 892c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 902c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /****************************************************************************** 912c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent INITIALIZATION 922c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent *******************************************************************************/ 932c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Correct the pointer to take the first down sampled signal sample */ 942c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pInSamps += pLVPSA_Inst->DownSamplingCount; 952c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Correct also the number of samples */ 962c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount); 972c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 982c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent while (ii > 0) 992c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1002c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Apply post gain */ 1012c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent X0 = ((*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]) >> (LVPSA_GAINSHIFT-1); /* - 1 to compensate scaling in process function*/ 1022c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pInSamps = pInSamps + DownSamplingFactor; 1032c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1042c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Saturate and take absolute value */ 1052c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if(X0 < 0) 1062c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent X0 = -X0; 1072c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if (X0 > 0x7FFF) 1082c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Xg0 = 0x7FFF; 1092c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent else 1102c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent Xg0 = (LVM_INT16)(X0); 1112c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1122c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1132c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Quasi peak filter calculation */ 1142c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent D0 = (LVM_INT16)(Xg0 - V0); 1152c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1162c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent temp2 = (LVM_INT32)D0; 1172c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent MUL32x32INTO32(temp2,Kp,accu,31); 1182c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1192c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent D0 = (LVM_INT16)(D0>>1); 1202c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if (D0 < 0){ 1212c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent D0 = (LVM_INT16)(-D0); 1222c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1232c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1242c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent temp2 = (LVM_INT32)D0; 1252c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent MUL32x32INTO32((LVM_INT32)D0,Km,temp,31); 1262c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent accu +=temp + Xg0; 1272c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1282c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if (accu > 0x7FFF) 1292c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent accu = 0x7FFF; 1302c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent else if(accu < 0) 1312c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent accu = 0x0000; 1322c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1332c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent V0 = (LVM_INT16)accu; 1342c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1352c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor)) 1362c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1372c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVPSA_QPD_WritePeak( pLVPSA_Inst, 1382c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent &pWrite, 1392c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent BandIndex, 1402c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent V0); 1412c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate; 1422c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii); 1432c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1442c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent BufferUpdateSamplesCount+=DownSamplingFactor; 1452c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1462c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent ii = (LVM_INT16)(ii-DownSamplingFactor); 1472c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1482c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1492c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1502c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Store last taps in memory */ 1512c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent *pDelay = (LVM_INT32)(V0); 1522c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1532c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* If this is the last call to the function after last band processing, 1542c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent update the parameters. */ 1552c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if(BandIndex == (pLVPSA_Inst->nRelevantFilters-1)) 1562c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1572c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 1582c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Adjustment for 11025Hz input, 220,5 is normally 1592c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent the exact number of samples for 20ms.*/ 1602c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&(pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025)) 1612c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1622c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if(pLVPSA_Inst->nSamplesBufferUpdate == 220) 1632c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1642c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->nSamplesBufferUpdate = 221; 1652c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1662c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent else 1672c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 1682c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->nSamplesBufferUpdate = 220; 1692c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1702c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1712c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 1722c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount; 1732c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii); 1742c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 1752c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent} 1762c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 1772c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 1782c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1792c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* FUNCTION: LVPSA_QPD_WritePeak */ 1802c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1812c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* DESCRIPTION: */ 1822c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Write a level value in the spectrum data buffer in the corresponding band. */ 1832c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1842c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* PARAMETERS: */ 1852c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* pLVPSA_Inst Pointer to the LVPSA instance */ 1862c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* ppWrite Pointer to pointer to the buffer */ 1872c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* CallNumber Number of the band the value should be written in */ 1882c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* Value Value to write in the spectrum data buffer */ 1892c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1902c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* RETURNS: void */ 1912c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/* */ 1922c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent/************************************************************************************/ 1932c87e9c923b0362fabf8c97ff63997542394c428Eric Laurentvoid LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 1942c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT8 **ppWrite, 1952c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 BandIndex, 1962c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_INT16 Value ) 1972c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent{ 1982c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent LVM_UINT8 *pWrite = *ppWrite; 1992c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 2002c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 2012c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent /* Write the value and update the write pointer */ 2022c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent *(pWrite + BandIndex) = (LVM_UINT8)(Value>>7); 2032c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pWrite += pLVPSA_Inst->nBands; 2042c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * pLVPSA_Inst->SpectralDataBufferLength)) 2052c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent { 2062c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent pWrite = pLVPSA_Inst->pSpectralDataBufferStart; 2072c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent } 2082c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 2092c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent *ppWrite = pWrite; 2102c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 2112c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent} 2122c87e9c923b0362fabf8c97ff63997542394c428Eric Laurent 213