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 void LVPSA_QPD_WritePeak_Float( pLVPSA_InstancePr_t pLVPSA_Inst,
43 LVM_UINT8 **ppWrite,
44 LVM_INT16 BandIndex,
45 LVM_FLOAT Value );
46 /************************************************************************************/
47 /* */
48 /* FUNCTION: LVPSA_QPD_Process */
49 /* */
50 /* DESCRIPTION: */
51 /* Apply downsampling, post gain, quasi peak filtering and write the levels values */
52 /* in the buffer every 20 ms. */
53 /* */
54 /* PARAMETERS: */
55 /* */
56 /* RETURNS: void */
57 /* */
58 /************************************************************************************/
LVPSA_QPD_Process_Float(void * hInstance,LVM_FLOAT * pInSamps,LVM_INT16 numSamples,LVM_INT16 BandIndex)59 void LVPSA_QPD_Process_Float ( void *hInstance,
60 LVM_FLOAT *pInSamps,
61 LVM_INT16 numSamples,
62 LVM_INT16 BandIndex)
63 {
64
65 /******************************************************************************
66 PARAMETERS
67 *******************************************************************************/
68 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance;
69 QPD_FLOAT_State_t *pQPDState = (QPD_FLOAT_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex];
70
71 /* Pointer to taps */
72 LVM_FLOAT* pDelay = pQPDState->pDelay;
73
74 /* Parameters needed during quasi peak calculations */
75 LVM_FLOAT X0;
76 LVM_FLOAT temp,temp2;
77 LVM_FLOAT accu;
78 LVM_FLOAT Xg0;
79 LVM_FLOAT D0;
80 LVM_FLOAT V0 = (LVM_FLOAT)(*pDelay);
81
82 /* Filter's coef */
83 LVM_FLOAT Kp = ((LVM_FLOAT)(pQPDState->Coefs[0]));
84 LVM_FLOAT Km = ((LVM_FLOAT)(pQPDState->Coefs[1]));
85
86 LVM_INT16 ii = numSamples;
87
88 LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer;
89 LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount;
90 LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor;
91
92 /******************************************************************************
93 INITIALIZATION
94 *******************************************************************************/
95 /* Correct the pointer to take the first down sampled signal sample */
96 pInSamps += pLVPSA_Inst->DownSamplingCount;
97 /* Correct also the number of samples */
98 ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount);
99
100 while (ii > 0)
101 {
102 /* Apply post gain */
103 /* - 1 to compensate scaling in process function*/
104 X0 = (*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex];
105 pInSamps = pInSamps + DownSamplingFactor;
106
107 /* Saturate and take absolute value */
108 if(X0 < 0.0f)
109 X0 = -X0;
110 if (X0 > 1.0f)
111 Xg0 = 1.0f;
112 else
113 Xg0 =X0;
114
115 /* Quasi peak filter calculation */
116 D0 = Xg0 - V0;
117
118 temp2 = D0;
119
120 accu = temp2 * Kp;
121 D0 = D0 / 2.0f;
122 if (D0 < 0.0f){
123 D0 = -D0;
124 }
125
126 temp2 = D0;
127
128 temp = D0 * Km;
129 accu += temp + Xg0;
130
131 if (accu > 1.0f)
132 accu = 1.0f;
133 else if(accu < 0.0f)
134 accu = 0.0f;
135
136 V0 = accu;
137
138 if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor))
139 {
140 LVPSA_QPD_WritePeak_Float( pLVPSA_Inst,
141 &pWrite,
142 BandIndex,
143 V0);
144
145 BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate;
146 pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii);
147 }
148 BufferUpdateSamplesCount += DownSamplingFactor;
149
150 ii = (LVM_INT16)(ii - DownSamplingFactor);
151
152 }
153
154 /* Store last taps in memory */
155 *pDelay = V0;
156
157 /* If this is the last call to the function after last band processing,
158 update the parameters. */
159 if(BandIndex == (pLVPSA_Inst->nRelevantFilters - 1))
160 {
161 pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite;
162 /* Adjustment for 11025Hz input, 220,5 is normally
163 the exact number of samples for 20ms.*/
164 if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&
165 (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 /************************************************************************************/
182 /* */
183 /* FUNCTION: LVPSA_QPD_WritePeak */
184 /* */
185 /* DESCRIPTION: */
186 /* Write a level value in the spectrum data buffer in the corresponding band. */
187 /* */
188 /* PARAMETERS: */
189 /* pLVPSA_Inst Pointer to the LVPSA instance */
190 /* ppWrite Pointer to pointer to the buffer */
191 /* CallNumber Number of the band the value should be written in */
192 /* Value Value to write in the spectrum data buffer */
193 /* */
194 /* RETURNS: void */
195 /* */
196 /************************************************************************************/
LVPSA_QPD_WritePeak(pLVPSA_InstancePr_t pLVPSA_Inst,LVM_UINT8 ** ppWrite,LVM_INT16 BandIndex,LVM_INT16 Value)197 void LVPSA_QPD_WritePeak( pLVPSA_InstancePr_t pLVPSA_Inst,
198 LVM_UINT8 **ppWrite,
199 LVM_INT16 BandIndex,
200 LVM_INT16 Value )
201 {
202 LVM_UINT8 *pWrite = *ppWrite;
203
204 /* Write the value and update the write pointer */
205 *(pWrite + BandIndex) = (LVM_UINT8)(Value>>7);
206 pWrite += pLVPSA_Inst->nBands;
207 if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * pLVPSA_Inst->SpectralDataBufferLength))
208 {
209 pWrite = pLVPSA_Inst->pSpectralDataBufferStart;
210 }
211
212 *ppWrite = pWrite;
213
214 }
LVPSA_QPD_WritePeak_Float(pLVPSA_InstancePr_t pLVPSA_Inst,LVM_UINT8 ** ppWrite,LVM_INT16 BandIndex,LVM_FLOAT Value)215 void LVPSA_QPD_WritePeak_Float( pLVPSA_InstancePr_t pLVPSA_Inst,
216 LVM_UINT8 **ppWrite,
217 LVM_INT16 BandIndex,
218 LVM_FLOAT Value )
219 {
220 LVM_UINT8 *pWrite = *ppWrite;
221
222 /* Write the value and update the write pointer */
223 *(pWrite + BandIndex) = (LVM_UINT8)(Value * 256);
224 pWrite += pLVPSA_Inst->nBands;
225 if (pWrite == (pLVPSA_Inst->pSpectralDataBufferStart + pLVPSA_Inst->nBands * \
226 pLVPSA_Inst->SpectralDataBufferLength))
227 {
228 pWrite = pLVPSA_Inst->pSpectralDataBufferStart;
229 }
230
231 *ppWrite = pWrite;
232 }
233