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 /************************************************************************************/
19 /* */
20 /* Includes */
21 /* */
22 /************************************************************************************/
23
24 #include "LVCS.h"
25 #include "LVCS_Private.h"
26 #include "LVCS_StereoEnhancer.h"
27 #include "VectorArithmetic.h"
28 #include "LVCS_Tables.h"
29
30 /************************************************************************************/
31 /* */
32 /* FUNCTION: LVCS_StereoEnhanceInit */
33 /* */
34 /* DESCRIPTION: */
35 /* Initialises the stereo enhancement module based on the sample rate. */
36 /* */
37 /* The function selects the coefficients for the filters and clears the data */
38 /* history. It is also used for re-initialisation when one of the system control */
39 /* parameters changes but will only change the coefficients and clear the history */
40 /* if the sample rate or speaker type has changed. */
41 /* */
42 /* PARAMETERS: */
43 /* hInstance Instance Handle */
44 /* pParams Initialisation parameters */
45 /* */
46 /* RETURNS: */
47 /* LVCS_Success Always succeeds */
48 /* */
49 /* NOTES: */
50 /* */
51 /************************************************************************************/
52 #ifdef BUILD_FLOAT
LVCS_SEnhancerInit(LVCS_Handle_t hInstance,LVCS_Params_t * pParams)53 LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance,
54 LVCS_Params_t *pParams)
55 {
56
57 LVM_UINT16 Offset;
58 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
59 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
60 LVCS_Data_t *pData;
61 LVCS_Coefficient_t *pCoefficient;
62 FO_FLOAT_Coefs_t CoeffsMid;
63 BQ_FLOAT_Coefs_t CoeffsSide;
64 const BiquadA012B12CoefsSP_t *pSESideCoefs;
65
66
67 pData = (LVCS_Data_t *) \
68 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
69
70 pCoefficient = (LVCS_Coefficient_t *) \
71 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
72
73 /*
74 * If the sample rate or speaker type has changed update the filters
75 */
76 if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
77 (pInstance->Params.SpeakerType != pParams->SpeakerType))
78 {
79 /*
80 * Set the filter coefficients based on the sample rate
81 */
82 /* Mid filter */
83 Offset = (LVM_UINT16)pParams->SampleRate;
84
85 /* Convert incoming coefficients to the required format/ordering */
86 CoeffsMid.A0 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A0;
87 CoeffsMid.A1 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A1;
88 CoeffsMid.B1 = (LVM_FLOAT)-LVCS_SEMidCoefTable[Offset].B1;
89
90 /* Clear the taps */
91 LoadConst_Float(0, /* Value */
92 (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\
93 no dereferencing in function*/
94 /* Number of words */
95 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid) / sizeof(LVM_FLOAT)));
96
97 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid,
98 &pData->SEBiquadTapsMid,
99 &CoeffsMid);
100
101 /* Callbacks */
102 if(LVCS_SEMidCoefTable[Offset].Scale == 15)
103 {
104 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01;
105 }
106
107 Offset = (LVM_UINT16)(pParams->SampleRate);
108 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0];
109
110 /* Side filter */
111 /* Convert incoming coefficients to the required format/ordering */
112 CoeffsSide.A0 = (LVM_FLOAT) pSESideCoefs[Offset].A0;
113 CoeffsSide.A1 = (LVM_FLOAT) pSESideCoefs[Offset].A1;
114 CoeffsSide.A2 = (LVM_FLOAT) pSESideCoefs[Offset].A2;
115 CoeffsSide.B1 = (LVM_FLOAT)-pSESideCoefs[Offset].B1;
116 CoeffsSide.B2 = (LVM_FLOAT)-pSESideCoefs[Offset].B2;
117
118 /* Clear the taps */
119 LoadConst_Float(0, /* Value */
120 (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\
121 no dereferencing in function*/
122 /* Number of words */
123 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide) / sizeof(LVM_FLOAT)));
124 /* Callbacks */
125 switch(pSESideCoefs[Offset].Scale)
126 {
127 case 14:
128 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
129 &pData->SEBiquadTapsSide,
130 &CoeffsSide);
131
132 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01;
133 break;
134 case 15:
135 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
136 &pData->SEBiquadTapsSide,
137 &CoeffsSide);
138
139 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01;
140 break;
141 }
142
143 }
144
145
146 return(LVCS_SUCCESS);
147 }
148 #else
LVCS_SEnhancerInit(LVCS_Handle_t hInstance,LVCS_Params_t * pParams)149 LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance,
150 LVCS_Params_t *pParams)
151 {
152
153 LVM_UINT16 Offset;
154 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
155 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
156 LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
157 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
158 FO_C16_Coefs_t CoeffsMid;
159 BQ_C16_Coefs_t CoeffsSide;
160 const BiquadA012B12CoefsSP_t *pSESideCoefs;
161
162 /*
163 * If the sample rate or speaker type has changed update the filters
164 */
165 if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
166 (pInstance->Params.SpeakerType != pParams->SpeakerType))
167 {
168 /*
169 * Set the filter coefficients based on the sample rate
170 */
171 /* Mid filter */
172 Offset = (LVM_UINT16)pParams->SampleRate;
173
174 /* Convert incoming coefficients to the required format/ordering */
175 CoeffsMid.A0 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A0;
176 CoeffsMid.A1 = (LVM_INT16) LVCS_SEMidCoefTable[Offset].A1;
177 CoeffsMid.B1 = (LVM_INT16)-LVCS_SEMidCoefTable[Offset].B1;
178
179 /* Clear the taps */
180 LoadConst_16(0, /* Value */
181 (void *)&pData->SEBiquadTapsMid, /* Destination Cast to void:\
182 no dereferencing in function*/
183 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid)/sizeof(LVM_UINT16))); /* Number of words */
184
185 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid,
186 &pData->SEBiquadTapsMid,
187 &CoeffsMid);
188
189 /* Callbacks */
190 if(LVCS_SEMidCoefTable[Offset].Scale==15)
191 {
192 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01;
193 }
194
195 Offset = (LVM_UINT16)(pParams->SampleRate);
196 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0];
197
198 /* Side filter */
199 /* Convert incoming coefficients to the required format/ordering */
200 CoeffsSide.A0 = (LVM_INT16) pSESideCoefs[Offset].A0;
201 CoeffsSide.A1 = (LVM_INT16) pSESideCoefs[Offset].A1;
202 CoeffsSide.A2 = (LVM_INT16) pSESideCoefs[Offset].A2;
203 CoeffsSide.B1 = (LVM_INT16)-pSESideCoefs[Offset].B1;
204 CoeffsSide.B2 = (LVM_INT16)-pSESideCoefs[Offset].B2;
205
206 /* Clear the taps */
207 LoadConst_16(0, /* Value */
208 (void *)&pData->SEBiquadTapsSide, /* Destination Cast to void:\
209 no dereferencing in function*/
210 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide)/sizeof(LVM_UINT16))); /* Number of words */
211
212
213 /* Callbacks */
214 switch(pSESideCoefs[Offset].Scale)
215 {
216 case 14:
217 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
218 &pData->SEBiquadTapsSide,
219 &CoeffsSide);
220
221 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01;
222 break;
223 case 15:
224 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
225 &pData->SEBiquadTapsSide,
226 &CoeffsSide);
227
228 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01;
229 break;
230 }
231
232 }
233
234
235 return(LVCS_SUCCESS);
236 }
237 #endif
238 /************************************************************************************/
239 /* */
240 /* FUNCTION: LVCS_StereoEnhance */
241 /* */
242 /* DESCRIPTION: */
243 /* Enhance the stereo image in the input samples based on the following block */
244 /* diagram: */
245 /* */
246 /* ________ */
247 /* ________ | | ________ */
248 /* | | Middle | Treble | | | */
249 /* | |---------->| Boost |-------->| | */
250 /* | Stereo | |________| | M & S | */
251 /* -->| to | ________ | to |--> */
252 /* | M & S | Side | | | Stereo | */
253 /* | |---------->| Side |-------->| | */
254 /* |________| | Boost | |________| */
255 /* |________| */
256 /* */
257 /* */
258 /* If the input signal is a mono signal there will be no side signal and hence */
259 /* the side filter will not be run. In mobile speaker mode the middle filter is */
260 /* not required and the Trebble boost filter is replaced by a simple gain block. */
261 /* */
262 /* */
263 /* PARAMETERS: */
264 /* hInstance Instance Handle */
265 /* pInData Pointer to the input data */
266 /* pOutData Pointer to the output data */
267 /* NumSamples Number of samples to process */
268 /* */
269 /* RETURNS: */
270 /* LVCS_Success Always succeeds */
271 /* */
272 /* NOTES: */
273 /* 1. The side filter is not used in Mobile Speaker mode */
274 /* */
275 /************************************************************************************/
276 #ifdef BUILD_FLOAT
LVCS_StereoEnhancer(LVCS_Handle_t hInstance,const LVM_FLOAT * pInData,LVM_FLOAT * pOutData,LVM_UINT16 NumSamples)277 LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance,
278 const LVM_FLOAT *pInData,
279 LVM_FLOAT *pOutData,
280 LVM_UINT16 NumSamples)
281 {
282
283 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
284 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
285 LVCS_Coefficient_t *pCoefficient;
286 LVM_FLOAT *pScratch;
287
288 pCoefficient = (LVCS_Coefficient_t *) \
289 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
290
291 pScratch = (LVM_FLOAT *) \
292 pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress;
293 /*
294 * Check if the Stereo Enhancer is enabled
295 */
296 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0)
297 {
298 /*
299 * Convert from stereo to middle and side
300 */
301 From2iToMS_Float(pInData,
302 pScratch,
303 pScratch + NumSamples,
304 (LVM_INT16)NumSamples);
305
306 /*
307 * Apply filter to the middle signal
308 */
309 if (pInstance->OutputDevice == LVCS_HEADPHONE)
310 {
311 (pConfig->pBiquadCallBack_Mid)((Biquad_FLOAT_Instance_t*)\
312 &pCoefficient->SEBiquadInstanceMid,
313 (LVM_FLOAT *)pScratch,
314 (LVM_FLOAT *)pScratch,
315 (LVM_INT16)NumSamples);
316 }
317 else
318 {
319 Mult3s_Float(pScratch, /* Source */
320 (LVM_FLOAT)pConfig->MidGain, /* Gain */
321 pScratch, /* Destination */
322 (LVM_INT16)NumSamples); /* Number of samples */
323 }
324
325 /*
326 * Apply the filter the side signal only in stereo mode for headphones
327 * and in all modes for mobile speakers
328 */
329 if (pInstance->Params.SourceFormat == LVCS_STEREO)
330 {
331 (pConfig->pBiquadCallBack_Side)((Biquad_FLOAT_Instance_t*) \
332 &pCoefficient->SEBiquadInstanceSide,
333 (LVM_FLOAT *)(pScratch + NumSamples),
334 (LVM_FLOAT *)(pScratch + NumSamples),
335 (LVM_INT16)NumSamples);
336 }
337
338 /*
339 * Convert from middle and side to stereo
340 */
341 MSTo2i_Sat_Float(pScratch,
342 pScratch + NumSamples,
343 pOutData,
344 (LVM_INT16)NumSamples);
345
346 }
347 else
348 {
349 /*
350 * The stereo enhancer is disabled so just copy the data
351 */
352 Copy_Float((LVM_FLOAT *)pInData, /* Source */
353 (LVM_FLOAT *)pOutData, /* Destination */
354 (LVM_INT16)(2 * NumSamples)); /* Left and right */
355 }
356
357 return(LVCS_SUCCESS);
358 }
359 #else
LVCS_StereoEnhancer(LVCS_Handle_t hInstance,const LVM_INT16 * pInData,LVM_INT16 * pOutData,LVM_UINT16 NumSamples)360 LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance,
361 const LVM_INT16 *pInData,
362 LVM_INT16 *pOutData,
363 LVM_UINT16 NumSamples)
364 {
365
366 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
367 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
368 LVCS_Coefficient_t *pCoefficient = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
369 LVM_INT16 *pScratch = (LVM_INT16 *)pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress;
370
371 /*
372 * Check if the Stereo Enhancer is enabled
373 */
374 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0)
375 {
376 /*
377 * Convert from stereo to middle and side
378 */
379 From2iToMS_16x16(pInData,
380 pScratch,
381 pScratch+NumSamples,
382 (LVM_INT16)NumSamples);
383
384 /*
385 * Apply filter to the middle signal
386 */
387 if (pInstance->OutputDevice == LVCS_HEADPHONE)
388 {
389 (pConfig->pBiquadCallBack_Mid)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceMid,
390 (LVM_INT16 *)pScratch,
391 (LVM_INT16 *)pScratch,
392 (LVM_INT16)NumSamples);
393 }
394 else
395 {
396 Mult3s_16x16(pScratch, /* Source */
397 (LVM_INT16)pConfig->MidGain, /* Gain */
398 pScratch, /* Destination */
399 (LVM_INT16)NumSamples); /* Number of samples */
400 }
401
402 /*
403 * Apply the filter the side signal only in stereo mode for headphones
404 * and in all modes for mobile speakers
405 */
406 if (pInstance->Params.SourceFormat == LVCS_STEREO)
407 {
408 (pConfig->pBiquadCallBack_Side)((Biquad_Instance_t*)&pCoefficient->SEBiquadInstanceSide,
409 (LVM_INT16 *)(pScratch + NumSamples),
410 (LVM_INT16 *)(pScratch + NumSamples),
411 (LVM_INT16)NumSamples);
412 }
413
414 /*
415 * Convert from middle and side to stereo
416 */
417 MSTo2i_Sat_16x16(pScratch,
418 pScratch+NumSamples,
419 pOutData,
420 (LVM_INT16)NumSamples);
421
422 }
423 else
424 {
425 /*
426 * The stereo enhancer is disabled so just copy the data
427 */
428 Copy_16((LVM_INT16 *)pInData, /* Source */
429 (LVM_INT16 *)pOutData, /* Destination */
430 (LVM_INT16)(2*NumSamples)); /* Left and right */
431
432 }
433
434 return(LVCS_SUCCESS);
435 }
436 #endif
437