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
LVPSA_QPD_Process(void * hInstance,LVM_INT16 * pInSamps,LVM_INT16 numSamples,LVM_INT16 BandIndex)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
LVPSA_QPD_Process_Float(void * hInstance,LVM_FLOAT * pInSamps,LVM_INT16 numSamples,LVM_INT16 BandIndex)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 /************************************************************************************/
LVPSA_QPD_WritePeak(pLVPSA_InstancePr_t pLVPSA_Inst,LVM_UINT8 ** ppWrite,LVM_INT16 BandIndex,LVM_INT16 Value)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
LVPSA_QPD_WritePeak_Float(pLVPSA_InstancePr_t pLVPSA_Inst,LVM_UINT8 ** ppWrite,LVM_INT16 BandIndex,LVM_FLOAT Value)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