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 /************************************************************************************/ 37 void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 38 LVM_UINT8 **ppWrite, 39 LVM_INT16 BandIndex, 40 LVM_INT16 Value ); 41 42 #ifdef BUILD_FLOAT 43 void LVPSA_QPD_WritePeak_Float( pLVPSA_InstancePr_t pLVPSA_Inst, 44 LVM_UINT8 **ppWrite, 45 LVM_INT16 BandIndex, 46 LVM_FLOAT Value ); 47 #endif 48 /************************************************************************************/ 49 /* */ 50 /* FUNCTION: LVPSA_QPD_Process */ 51 /* */ 52 /* DESCRIPTION: */ 53 /* Apply downsampling, post gain, quasi peak filtering and write the levels values */ 54 /* in the buffer every 20 ms. */ 55 /* */ 56 /* PARAMETERS: */ 57 /* */ 58 /* RETURNS: void */ 59 /* */ 60 /************************************************************************************/ 61 #ifndef BUILD_FLOAT 62 void LVPSA_QPD_Process ( void *hInstance, 63 LVM_INT16 *pInSamps, 64 LVM_INT16 numSamples, 65 LVM_INT16 BandIndex) 66 { 67 68 /****************************************************************************** 69 PARAMETERS 70 *******************************************************************************/ 71 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 72 QPD_State_t *pQPDState = (QPD_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex]; 73 74 /* Pointer to taps */ 75 LVM_INT32* pDelay = pQPDState->pDelay; 76 77 /* Parameters needed during quasi peak calculations */ 78 LVM_INT32 X0; 79 LVM_INT32 temp,temp2; 80 LVM_INT32 accu; 81 LVM_INT16 Xg0; 82 LVM_INT16 D0; 83 LVM_INT16 V0 = (LVM_INT16)(*pDelay); 84 85 /* Filter's coef */ 86 LVM_INT32 Kp = pQPDState->Coefs[0]; 87 LVM_INT32 Km = pQPDState->Coefs[1]; 88 89 LVM_INT16 ii = numSamples; 90 91 LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer; 92 LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount; 93 LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor; 94 95 /****************************************************************************** 96 INITIALIZATION 97 *******************************************************************************/ 98 /* Correct the pointer to take the first down sampled signal sample */ 99 pInSamps += pLVPSA_Inst->DownSamplingCount; 100 /* Correct also the number of samples */ 101 ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount); 102 103 while (ii > 0) 104 { 105 /* Apply post gain */ 106 X0 = ((*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]) >> (LVPSA_GAINSHIFT-1); /* - 1 to compensate scaling in process function*/ 107 pInSamps = pInSamps + DownSamplingFactor; 108 109 /* Saturate and take absolute value */ 110 if(X0 < 0) 111 X0 = -X0; 112 if (X0 > 0x7FFF) 113 Xg0 = 0x7FFF; 114 else 115 Xg0 = (LVM_INT16)(X0); 116 117 118 /* Quasi peak filter calculation */ 119 D0 = (LVM_INT16)(Xg0 - V0); 120 121 temp2 = (LVM_INT32)D0; 122 MUL32x32INTO32(temp2,Kp,accu,31); 123 124 D0 = (LVM_INT16)(D0>>1); 125 if (D0 < 0){ 126 D0 = (LVM_INT16)(-D0); 127 } 128 129 temp2 = (LVM_INT32)D0; 130 MUL32x32INTO32((LVM_INT32)D0,Km,temp,31); 131 accu +=temp + Xg0; 132 133 if (accu > 0x7FFF) 134 accu = 0x7FFF; 135 else if(accu < 0) 136 accu = 0x0000; 137 138 V0 = (LVM_INT16)accu; 139 140 if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor)) 141 { 142 LVPSA_QPD_WritePeak( pLVPSA_Inst, 143 &pWrite, 144 BandIndex, 145 V0); 146 BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate; 147 pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii); 148 } 149 BufferUpdateSamplesCount+=DownSamplingFactor; 150 151 ii = (LVM_INT16)(ii-DownSamplingFactor); 152 153 } 154 155 /* Store last taps in memory */ 156 *pDelay = (LVM_INT32)(V0); 157 158 /* If this is the last call to the function after last band processing, 159 update the parameters. */ 160 if(BandIndex == (pLVPSA_Inst->nRelevantFilters-1)) 161 { 162 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 163 /* Adjustment for 11025Hz input, 220,5 is normally 164 the exact number of samples for 20ms.*/ 165 if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&(pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025)) 166 { 167 if(pLVPSA_Inst->nSamplesBufferUpdate == 220) 168 { 169 pLVPSA_Inst->nSamplesBufferUpdate = 221; 170 } 171 else 172 { 173 pLVPSA_Inst->nSamplesBufferUpdate = 220; 174 } 175 } 176 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 177 pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount; 178 pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii); 179 } 180 } 181 #else 182 void LVPSA_QPD_Process_Float ( void *hInstance, 183 LVM_FLOAT *pInSamps, 184 LVM_INT16 numSamples, 185 LVM_INT16 BandIndex) 186 { 187 188 /****************************************************************************** 189 PARAMETERS 190 *******************************************************************************/ 191 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 192 QPD_FLOAT_State_t *pQPDState = (QPD_FLOAT_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex]; 193 194 /* Pointer to taps */ 195 LVM_FLOAT* pDelay = pQPDState->pDelay; 196 197 /* Parameters needed during quasi peak calculations */ 198 LVM_FLOAT X0; 199 LVM_FLOAT temp,temp2; 200 LVM_FLOAT accu; 201 LVM_FLOAT Xg0; 202 LVM_FLOAT D0; 203 LVM_FLOAT V0 = (LVM_FLOAT)(*pDelay); 204 205 /* Filter's coef */ 206 LVM_FLOAT Kp = ((LVM_FLOAT)(pQPDState->Coefs[0])); 207 LVM_FLOAT Km = ((LVM_FLOAT)(pQPDState->Coefs[1])); 208 209 LVM_INT16 ii = numSamples; 210 211 LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer; 212 LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount; 213 LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor; 214 215 /****************************************************************************** 216 INITIALIZATION 217 *******************************************************************************/ 218 /* Correct the pointer to take the first down sampled signal sample */ 219 pInSamps += pLVPSA_Inst->DownSamplingCount; 220 /* Correct also the number of samples */ 221 ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount); 222 223 while (ii > 0) 224 { 225 /* Apply post gain */ 226 /* - 1 to compensate scaling in process function*/ 227 X0 = (*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]; 228 pInSamps = pInSamps + DownSamplingFactor; 229 230 /* Saturate and take absolute value */ 231 if(X0 < 0.0f) 232 X0 = -X0; 233 if (X0 > 1.0f) 234 Xg0 = 1.0f; 235 else 236 Xg0 =X0; 237 238 239 /* Quasi peak filter calculation */ 240 D0 = Xg0 - V0; 241 242 temp2 = D0; 243 244 accu = temp2 * Kp; 245 D0 = D0 / 2.0f; 246 if (D0 < 0.0f){ 247 D0 = -D0; 248 } 249 250 temp2 = D0; 251 252 temp = D0 * Km; 253 accu += temp + Xg0; 254 255 if (accu > 1.0f) 256 accu = 1.0f; 257 else if(accu < 0.0f) 258 accu = 0.0f; 259 260 V0 = accu; 261 262 if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor)) 263 { 264 LVPSA_QPD_WritePeak_Float( pLVPSA_Inst, 265 &pWrite, 266 BandIndex, 267 V0); 268 269 BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate; 270 pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii); 271 } 272 BufferUpdateSamplesCount += DownSamplingFactor; 273 274 ii = (LVM_INT16)(ii - DownSamplingFactor); 275 276 } 277 278 /* Store last taps in memory */ 279 *pDelay = V0; 280 281 /* If this is the last call to the function after last band processing, 282 update the parameters. */ 283 if(BandIndex == (pLVPSA_Inst->nRelevantFilters - 1)) 284 { 285 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 286 /* Adjustment for 11025Hz input, 220,5 is normally 287 the exact number of samples for 20ms.*/ 288 if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&& 289 (pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025)) 290 { 291 if(pLVPSA_Inst->nSamplesBufferUpdate == 220) 292 { 293 pLVPSA_Inst->nSamplesBufferUpdate = 221; 294 } 295 else 296 { 297 pLVPSA_Inst->nSamplesBufferUpdate = 220; 298 } 299 } 300 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite; 301 pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount; 302 pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii); 303 } 304 } 305 #endif 306 /************************************************************************************/ 307 /* */ 308 /* FUNCTION: LVPSA_QPD_WritePeak */ 309 /* */ 310 /* DESCRIPTION: */ 311 /* Write a level value in the spectrum data buffer in the corresponding band. */ 312 /* */ 313 /* PARAMETERS: */ 314 /* pLVPSA_Inst Pointer to the LVPSA instance */ 315 /* ppWrite Pointer to pointer to the buffer */ 316 /* CallNumber Number of the band the value should be written in */ 317 /* Value Value to write in the spectrum data buffer */ 318 /* */ 319 /* RETURNS: void */ 320 /* */ 321 /************************************************************************************/ 322 void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst, 323 LVM_UINT8 **ppWrite, 324 LVM_INT16 BandIndex, 325 LVM_INT16 Value ) 326 { 327 LVM_UINT8 *pWrite = *ppWrite; 328 329 330 /* Write the value and update the write pointer */ 331 *(pWrite + BandIndex) = (LVM_UINT8)(Value>>7); 332 pWrite += pLVPSA_Inst->nBands; 333 if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * pLVPSA_Inst->SpectralDataBufferLength)) 334 { 335 pWrite = pLVPSA_Inst->pSpectralDataBufferStart; 336 } 337 338 *ppWrite = pWrite; 339 340 } 341 #ifdef BUILD_FLOAT 342 void LVPSA_QPD_WritePeak_Float( pLVPSA_InstancePr_t pLVPSA_Inst, 343 LVM_UINT8 **ppWrite, 344 LVM_INT16 BandIndex, 345 LVM_FLOAT Value ) 346 { 347 LVM_UINT8 *pWrite = *ppWrite; 348 349 /* Write the value and update the write pointer */ 350 *(pWrite + BandIndex) = (LVM_UINT8)(Value * 256); 351 pWrite += pLVPSA_Inst->nBands; 352 if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * \ 353 pLVPSA_Inst->SpectralDataBufferLength)) 354 { 355 pWrite = pLVPSA_Inst->pSpectralDataBufferStart; 356 } 357 358 *ppWrite = pWrite; 359 } 360 #endif 361
