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