/* * Copyright (C) 2004-2010 NXP Software * Copyright (C) 2010 The Android Open Source Project * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include "LVPSA_QPD.h" #include "LVPSA_Private.h" /************************************************************************************/ /* */ /* FUNCTION: LVPSA_QPD_WritePeak */ /* */ /* DESCRIPTION: */ /* Write a level value in the buffer in the corresponding band. */ /* */ /* PARAMETERS: */ /* pInst Pointer to the LVPSA instance */ /* ppWrite Pointer to pointer to the buffer */ /* CallNumber Number of the band the value should be written in */ /* Value Value to write in the buffer */ /* */ /* RETURNS: void */ /* */ /************************************************************************************/ void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, LVM_UINT8 **ppWrite, LVM_INT16 BandIndex, LVM_INT16 Value ); /************************************************************************************/ /* */ /* FUNCTION: LVPSA_QPD_Process */ /* */ /* DESCRIPTION: */ /* Apply downsampling, post gain, quasi peak filtering and write the levels values */ /* in the buffer every 20 ms. */ /* */ /* PARAMETERS: */ /* */ /* RETURNS: void */ /* */ /************************************************************************************/ void LVPSA_QPD_Process ( void *hInstance, LVM_INT16 *pInSamps, LVM_INT16 numSamples, LVM_INT16 BandIndex) { /****************************************************************************** PARAMETERS *******************************************************************************/ LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; QPD_State_t *pQPDState = (QPD_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex]; /* Pointer to taps */ LVM_INT32* pDelay = pQPDState->pDelay; /* Parameters needed during quasi peak calculations */ LVM_INT32 X0; LVM_INT32 temp,temp2; LVM_INT32 accu; LVM_INT16 Xg0; LVM_INT16 D0; LVM_INT16 V0 = (LVM_INT16)(*pDelay); /* Filter's coef */ LVM_INT32 Kp = pQPDState->Coefs[0]; LVM_INT32 Km = pQPDState->Coefs[1]; LVM_INT16 ii = numSamples; LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer; LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount; LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor; /****************************************************************************** INITIALIZATION *******************************************************************************/ /* Correct the pointer to take the first down sampled signal sample */ pInSamps += pLVPSA_Inst->DownSamplingCount; /* Correct also the number of samples */ ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount); while (ii > 0) { /* Apply post gain */ X0 = ((*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]) >> (LVPSA_GAINSHIFT-1); /* - 1 to compensate scaling in process function*/ pInSamps = pInSamps + DownSamplingFactor; /* Saturate and take absolute value */ if(X0 < 0) X0 = -X0; if (X0 > 0x7FFF) Xg0 = 0x7FFF; else Xg0 = (LVM_INT16)(X0); /* Quasi peak filter calculation */ D0 = (LVM_INT16)(Xg0 - V0); temp2 = (LVM_INT32)D0; MUL32x32INTO32(temp2,Kp,accu,31); D0 = (LVM_INT16)(D0>>1); if (D0 < 0){ D0 = (LVM_INT16)(-D0); } temp2 = (LVM_INT32)D0; MUL32x32INTO32((LVM_INT32)D0,Km,temp,31); accu +=temp + Xg0; if (accu > 0x7FFF) accu = 0x7FFF; else if(accu < 0) accu = 0x0000; V0 = (LVM_INT16)accu; if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor)) { LVPSA_QPD_WritePeak( pLVPSA_Inst, &pWrite, BandIndex, V0); BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate; pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii); } BufferUpdateSamplesCount+=DownSamplingFactor; ii = (LVM_INT16)(ii-DownSamplingFactor); } /* Store last taps in memory */ *pDelay = (LVM_INT32)(V0); /* If this is the last call to the function after last band processing, update the parameters. */ if(BandIndex == (pLVPSA_Inst->nRelevantFilters-1)) { pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; /* Adjustment for 11025Hz input, 220,5 is normally the exact number of samples for 20ms.*/ if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&(pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025)) { if(pLVPSA_Inst->nSamplesBufferUpdate == 220) { pLVPSA_Inst->nSamplesBufferUpdate = 221; } else { pLVPSA_Inst->nSamplesBufferUpdate = 220; } } pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount; pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii); } } /************************************************************************************/ /* */ /* FUNCTION: LVPSA_QPD_WritePeak */ /* */ /* DESCRIPTION: */ /* Write a level value in the spectrum data buffer in the corresponding band. */ /* */ /* PARAMETERS: */ /* pLVPSA_Inst Pointer to the LVPSA instance */ /* ppWrite Pointer to pointer to the buffer */ /* CallNumber Number of the band the value should be written in */ /* Value Value to write in the spectrum data buffer */ /* */ /* RETURNS: void */ /* */ /************************************************************************************/ void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, LVM_UINT8 **ppWrite, LVM_INT16 BandIndex, LVM_INT16 Value ) { LVM_UINT8 *pWrite = *ppWrite; /* Write the value and update the write pointer */ *(pWrite + BandIndex) = (LVM_UINT8)(Value>>7); pWrite += pLVPSA_Inst->nBands; if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * pLVPSA_Inst->SpectralDataBufferLength)) { pWrite = pLVPSA_Inst->pSpectralDataBufferStart; } *ppWrite = pWrite; }