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 /************************************************************************************/
LVCS_SEnhancerInit(LVCS_Handle_t hInstance,LVCS_Params_t * pParams)52 LVCS_ReturnStatus_en LVCS_SEnhancerInit(LVCS_Handle_t hInstance,
53 LVCS_Params_t *pParams)
54 {
55
56 LVM_UINT16 Offset;
57 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
58 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
59 LVCS_Data_t *pData;
60 LVCS_Coefficient_t *pCoefficient;
61 FO_FLOAT_Coefs_t CoeffsMid;
62 BQ_FLOAT_Coefs_t CoeffsSide;
63 const BiquadA012B12CoefsSP_t *pSESideCoefs;
64
65 pData = (LVCS_Data_t *) \
66 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
67
68 pCoefficient = (LVCS_Coefficient_t *) \
69 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
70
71 /*
72 * If the sample rate or speaker type has changed update the filters
73 */
74 if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
75 (pInstance->Params.SpeakerType != pParams->SpeakerType))
76 {
77 /*
78 * Set the filter coefficients based on the sample rate
79 */
80 /* Mid filter */
81 Offset = (LVM_UINT16)pParams->SampleRate;
82
83 /* Convert incoming coefficients to the required format/ordering */
84 CoeffsMid.A0 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A0;
85 CoeffsMid.A1 = (LVM_FLOAT) LVCS_SEMidCoefTable[Offset].A1;
86 CoeffsMid.B1 = (LVM_FLOAT)-LVCS_SEMidCoefTable[Offset].B1;
87
88 /* Clear the taps */
89 LoadConst_Float(0, /* Value */
90 (LVM_FLOAT *)&pData->SEBiquadTapsMid, /* Destination */
91 /* Number of words */
92 (LVM_UINT16)(sizeof(pData->SEBiquadTapsMid) / sizeof(LVM_FLOAT)));
93
94 FO_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceMid,
95 &pData->SEBiquadTapsMid,
96 &CoeffsMid);
97
98 /* Callbacks */
99 if(LVCS_SEMidCoefTable[Offset].Scale == 15)
100 {
101 pConfig->pBiquadCallBack_Mid = FO_1I_D16F16C15_TRC_WRA_01;
102 }
103
104 Offset = (LVM_UINT16)(pParams->SampleRate);
105 pSESideCoefs = (BiquadA012B12CoefsSP_t*)&LVCS_SESideCoefTable[0];
106
107 /* Side filter */
108 /* Convert incoming coefficients to the required format/ordering */
109 CoeffsSide.A0 = (LVM_FLOAT) pSESideCoefs[Offset].A0;
110 CoeffsSide.A1 = (LVM_FLOAT) pSESideCoefs[Offset].A1;
111 CoeffsSide.A2 = (LVM_FLOAT) pSESideCoefs[Offset].A2;
112 CoeffsSide.B1 = (LVM_FLOAT)-pSESideCoefs[Offset].B1;
113 CoeffsSide.B2 = (LVM_FLOAT)-pSESideCoefs[Offset].B2;
114
115 /* Clear the taps */
116 LoadConst_Float(0, /* Value */
117 (LVM_FLOAT *)&pData->SEBiquadTapsSide, /* Destination */
118 /* Number of words */
119 (LVM_UINT16)(sizeof(pData->SEBiquadTapsSide) / sizeof(LVM_FLOAT)));
120 /* Callbacks */
121 switch(pSESideCoefs[Offset].Scale)
122 {
123 case 14:
124 BQ_1I_D16F32Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
125 &pData->SEBiquadTapsSide,
126 &CoeffsSide);
127
128 pConfig->pBiquadCallBack_Side = BQ_1I_D16F32C14_TRC_WRA_01;
129 break;
130 case 15:
131 BQ_1I_D16F16Css_TRC_WRA_01_Init(&pCoefficient->SEBiquadInstanceSide,
132 &pData->SEBiquadTapsSide,
133 &CoeffsSide);
134
135 pConfig->pBiquadCallBack_Side = BQ_1I_D16F16C15_TRC_WRA_01;
136 break;
137 }
138
139 }
140
141 return(LVCS_SUCCESS);
142 }
143 /************************************************************************************/
144 /* */
145 /* FUNCTION: LVCS_StereoEnhance */
146 /* */
147 /* DESCRIPTION: */
148 /* Enhance the stereo image in the input samples based on the following block */
149 /* diagram: */
150 /* */
151 /* ________ */
152 /* ________ | | ________ */
153 /* | | Middle | Treble | | | */
154 /* | |---------->| Boost |-------->| | */
155 /* | Stereo | |________| | M & S | */
156 /* -->| to | ________ | to |--> */
157 /* | M & S | Side | | | Stereo | */
158 /* | |---------->| Side |-------->| | */
159 /* |________| | Boost | |________| */
160 /* |________| */
161 /* */
162 /* */
163 /* If the input signal is a mono signal there will be no side signal and hence */
164 /* the side filter will not be run. In mobile speaker mode the middle filter is */
165 /* not required and the Trebble boost filter is replaced by a simple gain block. */
166 /* */
167 /* */
168 /* PARAMETERS: */
169 /* hInstance Instance Handle */
170 /* pInData Pointer to the input data */
171 /* pOutData Pointer to the output data */
172 /* NumSamples Number of samples to process */
173 /* */
174 /* RETURNS: */
175 /* LVCS_Success Always succeeds */
176 /* */
177 /* NOTES: */
178 /* 1. The side filter is not used in Mobile Speaker mode */
179 /* */
180 /************************************************************************************/
LVCS_StereoEnhancer(LVCS_Handle_t hInstance,const LVM_FLOAT * pInData,LVM_FLOAT * pOutData,LVM_UINT16 NumSamples)181 LVCS_ReturnStatus_en LVCS_StereoEnhancer(LVCS_Handle_t hInstance,
182 const LVM_FLOAT *pInData,
183 LVM_FLOAT *pOutData,
184 LVM_UINT16 NumSamples)
185 {
186
187 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
188 LVCS_StereoEnhancer_t *pConfig = (LVCS_StereoEnhancer_t *)&pInstance->StereoEnhancer;
189 LVCS_Coefficient_t *pCoefficient;
190 LVM_FLOAT *pScratch;
191
192 pCoefficient = (LVCS_Coefficient_t *) \
193 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
194
195 pScratch = (LVM_FLOAT *) \
196 pInstance->MemoryTable.Region[LVCS_MEMREGION_TEMPORARY_FAST].pBaseAddress;
197 /*
198 * Check if the Stereo Enhancer is enabled
199 */
200 if ((pInstance->Params.OperatingMode & LVCS_STEREOENHANCESWITCH) != 0)
201 {
202 /*
203 * Convert from stereo to middle and side
204 */
205 From2iToMS_Float(pInData,
206 pScratch,
207 pScratch + NumSamples,
208 (LVM_INT16)NumSamples);
209
210 /*
211 * Apply filter to the middle signal
212 */
213 if (pInstance->OutputDevice == LVCS_HEADPHONE)
214 {
215 (pConfig->pBiquadCallBack_Mid)((Biquad_FLOAT_Instance_t*)\
216 &pCoefficient->SEBiquadInstanceMid,
217 (LVM_FLOAT *)pScratch,
218 (LVM_FLOAT *)pScratch,
219 (LVM_INT16)NumSamples);
220 }
221 else
222 {
223 Mult3s_Float(pScratch, /* Source */
224 (LVM_FLOAT)pConfig->MidGain, /* Gain */
225 pScratch, /* Destination */
226 (LVM_INT16)NumSamples); /* Number of samples */
227 }
228
229 /*
230 * Apply the filter the side signal only in stereo mode for headphones
231 * and in all modes for mobile speakers
232 */
233 if (pInstance->Params.SourceFormat == LVCS_STEREO)
234 {
235 (pConfig->pBiquadCallBack_Side)((Biquad_FLOAT_Instance_t*) \
236 &pCoefficient->SEBiquadInstanceSide,
237 (LVM_FLOAT *)(pScratch + NumSamples),
238 (LVM_FLOAT *)(pScratch + NumSamples),
239 (LVM_INT16)NumSamples);
240 }
241
242 /*
243 * Convert from middle and side to stereo
244 */
245 MSTo2i_Sat_Float(pScratch,
246 pScratch + NumSamples,
247 pOutData,
248 (LVM_INT16)NumSamples);
249
250 }
251 else
252 {
253 /*
254 * The stereo enhancer is disabled so just copy the data
255 */
256 Copy_Float((LVM_FLOAT *)pInData, /* Source */
257 (LVM_FLOAT *)pOutData, /* Destination */
258 (LVM_INT16)(2 * NumSamples)); /* Left and right */
259 }
260
261 return(LVCS_SUCCESS);
262 }
263