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