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#include "LVPSA_QPD.h" 19#include "LVPSA_Private.h" 20 21/************************************************************************************/ 22/* */ 23/* FUNCTION: LVPSA_QPD_WritePeak */ 24/* */ 25/* DESCRIPTION: */ 26/* Write a level value in the buffer in the corresponding band. */ 27/* */ 28/* PARAMETERS: */ 29/* pInst Pointer to the LVPSA instance */ 30/* ppWrite Pointer to pointer to the buffer */ 31/* CallNumber Number of the band the value should be written in */ 32/* Value Value to write in the buffer */ 33/* */ 34/* RETURNS: void */ 35/* */ 36/************************************************************************************/ 37void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 38 LVM_UINT8 **ppWrite, 39 LVM_INT16 BandIndex, 40 LVM_INT16 Value ); 41 42 43 44/************************************************************************************/ 45/* */ 46/* FUNCTION: LVPSA_QPD_Process */ 47/* */ 48/* DESCRIPTION: */ 49/* Apply downsampling, post gain, quasi peak filtering and write the levels values */ 50/* in the buffer every 20 ms. */ 51/* */ 52/* PARAMETERS: */ 53/* */ 54/* RETURNS: void */ 55/* */ 56/************************************************************************************/ 57void LVPSA_QPD_Process ( void *hInstance, 58 LVM_INT16 *pInSamps, 59 LVM_INT16 numSamples, 60 LVM_INT16 BandIndex) 61{ 62 63 /****************************************************************************** 64 PARAMETERS 65 *******************************************************************************/ 66 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 67 QPD_State_t *pQPDState = (QPD_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex]; 68 69 /* Pointer to taps */ 70 LVM_INT32* pDelay = pQPDState->pDelay; 71 72 /* Parameters needed during quasi peak calculations */ 73 LVM_INT32 X0; 74 LVM_INT32 temp,temp2; 75 LVM_INT32 accu; 76 LVM_INT16 Xg0; 77 LVM_INT16 D0; 78 LVM_INT16 V0 = (LVM_INT16)(*pDelay); 79 80 /* Filter's coef */ 81 LVM_INT32 Kp = pQPDState->Coefs[0]; 82 LVM_INT32 Km = pQPDState->Coefs[1]; 83 84 LVM_INT16 ii = numSamples; 85 86 LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer; 87 LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount; 88 LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor; 89 90 /****************************************************************************** 91 INITIALIZATION 92 *******************************************************************************/ 93 /* Correct the pointer to take the first down sampled signal sample */ 94 pInSamps += pLVPSA_Inst->DownSamplingCount; 95 /* Correct also the number of samples */ 96 ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount); 97 98 while (ii > 0) 99 { 100 /* Apply post gain */ 101 X0 = ((*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]) >> (LVPSA_GAINSHIFT-1); /* - 1 to compensate scaling in process function*/ 102 pInSamps = pInSamps + DownSamplingFactor; 103 104 /* Saturate and take absolute value */ 105 if(X0 < 0) 106 X0 = -X0; 107 if (X0 > 0x7FFF) 108 Xg0 = 0x7FFF; 109 else 110 Xg0 = (LVM_INT16)(X0); 111 112 113 /* Quasi peak filter calculation */ 114 D0 = (LVM_INT16)(Xg0 - V0); 115 116 temp2 = (LVM_INT32)D0; 117 MUL32x32INTO32(temp2,Kp,accu,31); 118 119 D0 = (LVM_INT16)(D0>>1); 120 if (D0 < 0){ 121 D0 = (LVM_INT16)(-D0); 122 } 123 124 temp2 = (LVM_INT32)D0; 125 MUL32x32INTO32((LVM_INT32)D0,Km,temp,31); 126 accu +=temp + Xg0; 127 128 if (accu > 0x7FFF) 129 accu = 0x7FFF; 130 else if(accu < 0) 131 accu = 0x0000; 132 133 V0 = (LVM_INT16)accu; 134 135 if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor)) 136 { 137 LVPSA_QPD_WritePeak( pLVPSA_Inst, 138 &pWrite, 139 BandIndex, 140 V0); 141 BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate; 142 pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii); 143 } 144 BufferUpdateSamplesCount+=DownSamplingFactor; 145 146 ii = (LVM_INT16)(ii-DownSamplingFactor); 147 148 } 149 150 /* Store last taps in memory */ 151 *pDelay = (LVM_INT32)(V0); 152 153 /* If this is the last call to the function after last band processing, 154 update the parameters. */ 155 if(BandIndex == (pLVPSA_Inst->nRelevantFilters-1)) 156 { 157 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 158 /* Adjustment for 11025Hz input, 220,5 is normally 159 the exact number of samples for 20ms.*/ 160 if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&(pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025)) 161 { 162 if(pLVPSA_Inst->nSamplesBufferUpdate == 220) 163 { 164 pLVPSA_Inst->nSamplesBufferUpdate = 221; 165 } 166 else 167 { 168 pLVPSA_Inst->nSamplesBufferUpdate = 220; 169 } 170 } 171 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 172 pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount; 173 pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii); 174 } 175} 176 177/************************************************************************************/ 178/* */ 179/* FUNCTION: LVPSA_QPD_WritePeak */ 180/* */ 181/* DESCRIPTION: */ 182/* Write a level value in the spectrum data buffer in the corresponding band. */ 183/* */ 184/* PARAMETERS: */ 185/* pLVPSA_Inst Pointer to the LVPSA instance */ 186/* ppWrite Pointer to pointer to the buffer */ 187/* CallNumber Number of the band the value should be written in */ 188/* Value Value to write in the spectrum data buffer */ 189/* */ 190/* RETURNS: void */ 191/* */ 192/************************************************************************************/ 193void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 194 LVM_UINT8 **ppWrite, 195 LVM_INT16 BandIndex, 196 LVM_INT16 Value ) 197{ 198 LVM_UINT8 *pWrite = *ppWrite; 199 200 201 /* Write the value and update the write pointer */ 202 *(pWrite + BandIndex) = (LVM_UINT8)(Value>>7); 203 pWrite += pLVPSA_Inst->nBands; 204 if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * pLVPSA_Inst->SpectralDataBufferLength)) 205 { 206 pWrite = pLVPSA_Inst->pSpectralDataBufferStart; 207 } 208 209 *ppWrite = pWrite; 210 211} 212 213