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