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.h"
19 #include "LVPSA_Private.h"
20 #include "LVM_Macros.h"
21 #include "VectorArithmetic.h"
22
23 #define LVM_MININT_32 0x80000000
24
mult32x32in32_shiftr(LVM_INT32 a,LVM_INT32 b,LVM_INT32 c)25 static LVM_INT32 mult32x32in32_shiftr(LVM_INT32 a, LVM_INT32 b, LVM_INT32 c) {
26 LVM_INT64 result = ((LVM_INT64)a * b) >> c;
27
28 if (result >= INT32_MAX) {
29 return INT32_MAX;
30 } else if (result <= INT32_MIN) {
31 return INT32_MIN;
32 } else {
33 return (LVM_INT32)result;
34 }
35 }
36
37 /************************************************************************************/
38 /* */
39 /* FUNCTION: LVPSA_Process */
40 /* */
41 /* DESCRIPTION: */
42 /* The process applies band pass filters to the signal. Each output */
43 /* feeds a quasi peak filter for level detection. */
44 /* */
45 /* PARAMETERS: */
46 /* hInstance Pointer to the instance */
47 /* pLVPSA_InputSamples Pointer to the input samples buffer */
48 /* InputBlockSize Number of mono samples to process */
49 /* AudioTime Playback time of the input samples */
50 /* */
51 /* */
52 /* RETURNS: */
53 /* LVPSA_OK Succeeds */
54 /* otherwise Error due to bad parameters */
55 /* */
56 /************************************************************************************/
57 #ifdef BUILD_FLOAT
LVPSA_Process(pLVPSA_Handle_t hInstance,LVM_FLOAT * pLVPSA_InputSamples,LVM_UINT16 InputBlockSize,LVPSA_Time AudioTime)58 LVPSA_RETURN LVPSA_Process ( pLVPSA_Handle_t hInstance,
59 LVM_FLOAT *pLVPSA_InputSamples,
60 LVM_UINT16 InputBlockSize,
61 LVPSA_Time AudioTime )
62
63 {
64 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance;
65 LVM_FLOAT *pScratch;
66 LVM_INT16 ii;
67 LVM_INT32 AudioTimeInc;
68 extern LVM_UINT32 LVPSA_SampleRateInvTab[];
69 LVM_UINT8 *pWrite_Save; /* Position of the write pointer
70 at the beginning of the process */
71
72 /******************************************************************************
73 CHECK PARAMETERS
74 *******************************************************************************/
75 if(hInstance == LVM_NULL || pLVPSA_InputSamples == LVM_NULL)
76 {
77 return(LVPSA_ERROR_NULLADDRESS);
78 }
79 if(InputBlockSize == 0 || InputBlockSize > pLVPSA_Inst->MaxInputBlockSize)
80 {
81 return(LVPSA_ERROR_INVALIDPARAM);
82 }
83
84 pScratch = (LVM_FLOAT*)pLVPSA_Inst->MemoryTable.Region[LVPSA_MEMREGION_SCRATCH].pBaseAddress;
85 pWrite_Save = pLVPSA_Inst->pSpectralDataBufferWritePointer;
86
87 /******************************************************************************
88 APPLY NEW SETTINGS IF NEEDED
89 *******************************************************************************/
90 if (pLVPSA_Inst->bControlPending == LVM_TRUE)
91 {
92 pLVPSA_Inst->bControlPending = 0;
93 LVPSA_ApplyNewSettings( pLVPSA_Inst);
94 }
95
96 /******************************************************************************
97 PROCESS SAMPLES
98 *******************************************************************************/
99 /* Put samples in range [-0.5;0.5[ for BP filters (see Biquads documentation) */
100 Copy_Float(pLVPSA_InputSamples, pScratch, (LVM_INT16)InputBlockSize);
101 Shift_Sat_Float(-1, pScratch, pScratch, (LVM_INT16)InputBlockSize);
102
103 for (ii = 0; ii < pLVPSA_Inst->nRelevantFilters; ii++)
104 {
105 switch(pLVPSA_Inst->pBPFiltersPrecision[ii])
106 {
107 case LVPSA_SimplePrecisionFilter:
108 BP_1I_D16F16C14_TRC_WRA_01 ( &pLVPSA_Inst->pBP_Instances[ii],
109 pScratch,
110 pScratch + InputBlockSize,
111 (LVM_INT16)InputBlockSize);
112 break;
113
114 case LVPSA_DoublePrecisionFilter:
115 BP_1I_D16F32C30_TRC_WRA_01 ( &pLVPSA_Inst->pBP_Instances[ii],
116 pScratch,
117 pScratch + InputBlockSize,
118 (LVM_INT16)InputBlockSize);
119 break;
120 default:
121 break;
122 }
123
124
125 LVPSA_QPD_Process_Float ( pLVPSA_Inst,
126 pScratch + InputBlockSize,
127 (LVM_INT16)InputBlockSize,
128 ii);
129 }
130
131 /******************************************************************************
132 UPDATE SpectralDataBufferAudioTime
133 *******************************************************************************/
134
135 if(pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite_Save)
136 {
137 AudioTimeInc = mult32x32in32_shiftr(
138 (AudioTime + ((LVM_INT32)pLVPSA_Inst->LocalSamplesCount * 1000)),
139 (LVM_INT32)LVPSA_SampleRateInvTab[pLVPSA_Inst->CurrentParams.Fs],
140 LVPSA_FsInvertShift);
141 pLVPSA_Inst->SpectralDataBufferAudioTime = AudioTime + AudioTimeInc;
142 }
143
144 return(LVPSA_OK);
145 }
146 #else
LVPSA_Process(pLVPSA_Handle_t hInstance,LVM_INT16 * pLVPSA_InputSamples,LVM_UINT16 InputBlockSize,LVPSA_Time AudioTime)147 LVPSA_RETURN LVPSA_Process ( pLVPSA_Handle_t hInstance,
148 LVM_INT16 *pLVPSA_InputSamples,
149 LVM_UINT16 InputBlockSize,
150 LVPSA_Time AudioTime )
151
152 {
153 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance;
154 LVM_INT16 *pScratch;
155 LVM_INT16 ii;
156 LVM_INT32 AudioTimeInc;
157 extern LVM_UINT32 LVPSA_SampleRateInvTab[];
158 LVM_UINT8 *pWrite_Save; /* Position of the write pointer at the beginning of the process */
159
160 /******************************************************************************
161 CHECK PARAMETERS
162 *******************************************************************************/
163 if(hInstance == LVM_NULL || pLVPSA_InputSamples == LVM_NULL)
164 {
165 return(LVPSA_ERROR_NULLADDRESS);
166 }
167 if(InputBlockSize == 0 || InputBlockSize > pLVPSA_Inst->MaxInputBlockSize)
168 {
169 return(LVPSA_ERROR_INVALIDPARAM);
170 }
171
172 pScratch = (LVM_INT16*)pLVPSA_Inst->MemoryTable.Region[LVPSA_MEMREGION_SCRATCH].pBaseAddress;
173 pWrite_Save = pLVPSA_Inst->pSpectralDataBufferWritePointer;
174
175 /******************************************************************************
176 APPLY NEW SETTINGS IF NEEDED
177 *******************************************************************************/
178 if (pLVPSA_Inst->bControlPending == LVM_TRUE)
179 {
180 pLVPSA_Inst->bControlPending = 0;
181 LVPSA_ApplyNewSettings( pLVPSA_Inst);
182 }
183
184 /******************************************************************************
185 PROCESS SAMPLES
186 *******************************************************************************/
187 /* Put samples in range [-0.5;0.5[ for BP filters (see Biquads documentation) */
188 Copy_16( pLVPSA_InputSamples,pScratch,(LVM_INT16)InputBlockSize);
189 Shift_Sat_v16xv16(-1,pScratch,pScratch,(LVM_INT16)InputBlockSize);
190
191 for (ii = 0; ii < pLVPSA_Inst->nRelevantFilters; ii++)
192 {
193 switch(pLVPSA_Inst->pBPFiltersPrecision[ii])
194 {
195 case LVPSA_SimplePrecisionFilter:
196 BP_1I_D16F16C14_TRC_WRA_01 ( &pLVPSA_Inst->pBP_Instances[ii],
197 pScratch,
198 pScratch + InputBlockSize,
199 (LVM_INT16)InputBlockSize);
200 break;
201
202 case LVPSA_DoublePrecisionFilter:
203 BP_1I_D16F32C30_TRC_WRA_01 ( &pLVPSA_Inst->pBP_Instances[ii],
204 pScratch,
205 pScratch + InputBlockSize,
206 (LVM_INT16)InputBlockSize);
207 break;
208 default:
209 break;
210 }
211
212
213 LVPSA_QPD_Process ( pLVPSA_Inst,
214 pScratch + InputBlockSize,
215 (LVM_INT16)InputBlockSize,
216 ii);
217 }
218
219 /******************************************************************************
220 UPDATE SpectralDataBufferAudioTime
221 *******************************************************************************/
222
223 if(pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite_Save)
224 {
225 MUL32x32INTO32((AudioTime + (LVM_INT32)((LVM_INT32)pLVPSA_Inst->LocalSamplesCount*1000)),
226 (LVM_INT32)LVPSA_SampleRateInvTab[pLVPSA_Inst->CurrentParams.Fs],
227 AudioTimeInc,
228 LVPSA_FsInvertShift)
229 pLVPSA_Inst->SpectralDataBufferAudioTime = AudioTime + AudioTimeInc;
230 }
231
232 return(LVPSA_OK);
233 }
234 #endif
235
236 /************************************************************************************/
237 /* */
238 /* FUNCTION: LVPSA_GetSpectrum */
239 /* */
240 /* DESCRIPTION: */
241 /* Gets the levels values at a certain point in time */
242 /* */
243 /* */
244 /* PARAMETERS: */
245 /* hInstance Pointer to the instance */
246 /* GetSpectrumAudioTime Retrieve the values at this time */
247 /* pCurrentValues Pointer to a buffer that will contain levels' values */
248 /* pMaxValues Pointer to a buffer that will contain max levels' values */
249 /* */
250 /* */
251 /* RETURNS: */
252 /* LVPSA_OK Succeeds */
253 /* otherwise Error due to bad parameters */
254 /* */
255 /************************************************************************************/
LVPSA_GetSpectrum(pLVPSA_Handle_t hInstance,LVPSA_Time GetSpectrumAudioTime,LVM_UINT8 * pCurrentValues,LVM_UINT8 * pPeakValues)256 LVPSA_RETURN LVPSA_GetSpectrum ( pLVPSA_Handle_t hInstance,
257 LVPSA_Time GetSpectrumAudioTime,
258 LVM_UINT8 *pCurrentValues,
259 LVM_UINT8 *pPeakValues )
260
261 {
262
263 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance;
264 LVM_INT32 StatusDelta, ii;
265 LVM_UINT8 *pRead;
266
267 if(hInstance == LVM_NULL || pCurrentValues == LVM_NULL || pPeakValues == LVM_NULL)
268 {
269 return(LVPSA_ERROR_NULLADDRESS);
270 }
271
272
273 /* First find the place where to look in the status buffer */
274 if(GetSpectrumAudioTime <= pLVPSA_Inst->SpectralDataBufferAudioTime)
275 {
276 MUL32x32INTO32((pLVPSA_Inst->SpectralDataBufferAudioTime - GetSpectrumAudioTime),LVPSA_InternalRefreshTimeInv,StatusDelta,LVPSA_InternalRefreshTimeShift);
277 if((StatusDelta * LVPSA_InternalRefreshTime) != (pLVPSA_Inst->SpectralDataBufferAudioTime - GetSpectrumAudioTime))
278 {
279 StatusDelta += 1;
280 }
281 }
282 else
283 {
284 /* This part handles the wrap around */
285 MUL32x32INTO32(((pLVPSA_Inst->SpectralDataBufferAudioTime - (LVM_INT32)LVM_MININT_32) + ((LVM_INT32)LVM_MAXINT_32 - GetSpectrumAudioTime)),LVPSA_InternalRefreshTimeInv,StatusDelta,LVPSA_InternalRefreshTimeShift)
286 if(((LVM_INT32)(StatusDelta * LVPSA_InternalRefreshTime)) != ((LVM_INT32)((pLVPSA_Inst->SpectralDataBufferAudioTime - (LVM_INT32)LVM_MININT_32) + ((LVM_INT32)LVM_MAXINT_32 - GetSpectrumAudioTime))))
287 {
288 StatusDelta += 1;
289 }
290 }
291 /* Check whether the desired level is not too "old" (see 2.10 in LVPSA_DesignNotes.doc)*/
292 if(
293 ((GetSpectrumAudioTime < pLVPSA_Inst->SpectralDataBufferAudioTime)&&
294 ((GetSpectrumAudioTime<0)&&(pLVPSA_Inst->SpectralDataBufferAudioTime>0))&&
295 (((LVM_INT32)(-GetSpectrumAudioTime + pLVPSA_Inst->SpectralDataBufferAudioTime))>LVM_MAXINT_32))||
296
297 ((GetSpectrumAudioTime > pLVPSA_Inst->SpectralDataBufferAudioTime)&&
298 (((GetSpectrumAudioTime>=0)&&(pLVPSA_Inst->SpectralDataBufferAudioTime>=0))||
299 ((GetSpectrumAudioTime<=0)&&(pLVPSA_Inst->SpectralDataBufferAudioTime<=0))||
300 (((GetSpectrumAudioTime>=0)&&(pLVPSA_Inst->SpectralDataBufferAudioTime<=0))&&
301 (((LVM_INT32)(GetSpectrumAudioTime - pLVPSA_Inst->SpectralDataBufferAudioTime))<LVM_MAXINT_32))))||
302
303 (StatusDelta > (LVM_INT32)pLVPSA_Inst->SpectralDataBufferLength) ||
304 (!StatusDelta))
305 {
306 for(ii = 0; ii < pLVPSA_Inst->nBands; ii++)
307 {
308 pCurrentValues[ii] = 0;
309 pPeakValues[ii] = 0;
310 }
311 return(LVPSA_OK);
312 }
313 /* Set the reading pointer */
314 if((LVM_INT32)(StatusDelta * pLVPSA_Inst->nBands) > (pLVPSA_Inst->pSpectralDataBufferWritePointer - pLVPSA_Inst->pSpectralDataBufferStart))
315 {
316 pRead = pLVPSA_Inst->pSpectralDataBufferWritePointer + (pLVPSA_Inst->SpectralDataBufferLength - (LVM_UINT32)StatusDelta) * pLVPSA_Inst->nBands;
317 }
318 else
319 {
320 pRead = pLVPSA_Inst->pSpectralDataBufferWritePointer - StatusDelta * pLVPSA_Inst->nBands;
321 }
322
323
324 /* Read the status buffer and fill the output buffers */
325 for(ii = 0; ii < pLVPSA_Inst->nBands; ii++)
326 {
327 pCurrentValues[ii] = pRead[ii];
328 if(pLVPSA_Inst->pPreviousPeaks[ii] <= pRead[ii])
329 {
330 pLVPSA_Inst->pPreviousPeaks[ii] = pRead[ii];
331 }
332 else if(pLVPSA_Inst->pPreviousPeaks[ii] != 0)
333 {
334 LVM_INT32 temp;
335 /*Re-compute max values for decay */
336 temp = (LVM_INT32)(LVPSA_MAXUNSIGNEDCHAR - pLVPSA_Inst->pPreviousPeaks[ii]);
337 temp = ((temp * LVPSA_MAXLEVELDECAYFACTOR)>>LVPSA_MAXLEVELDECAYSHIFT);
338 /* If the gain has no effect, "help" the value to increase */
339 if(temp == (LVPSA_MAXUNSIGNEDCHAR - pLVPSA_Inst->pPreviousPeaks[ii]))
340 {
341 temp += 1;
342 }
343 /* Saturate */
344 temp = (temp > LVPSA_MAXUNSIGNEDCHAR) ? LVPSA_MAXUNSIGNEDCHAR : temp;
345 /* Store new max level */
346 pLVPSA_Inst->pPreviousPeaks[ii] = (LVM_UINT8)(LVPSA_MAXUNSIGNEDCHAR - temp);
347 }
348
349 pPeakValues[ii] = pLVPSA_Inst->pPreviousPeaks[ii];
350 }
351
352 return(LVPSA_OK);
353 }
354