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_Equaliser.h"
27 #include "BIQUAD.h"
28 #include "VectorArithmetic.h"
29 #include "LVCS_Tables.h"
30
31 /************************************************************************************/
32 /* */
33 /* FUNCTION: LVCS_EqualiserInit */
34 /* */
35 /* DESCRIPTION: */
36 /* Initialises the equaliser module */
37 /* */
38 /* The function selects the coefficients for the filters and clears the data */
39 /* history. It is also used for re-initialisation when one of the system control */
40 /* parameters changes but will only change the coefficients and clear the history */
41 /* if the sample rate or speaker type has changed. */
42 /* */
43 /* To avoid excessive testing during the sample processing the biquad type is */
44 /* set as a callback function in the init routine. */
45 /* */
46 /* PARAMETERS: */
47 /* hInstance Instance Handle */
48 /* pParams Initialisation parameters */
49 /* */
50 /* RETURNS: */
51 /* LVCS_Success Always succeeds */
52 /* */
53 /* NOTES: */
54 /* */
55 /************************************************************************************/
56 #ifdef BUILD_FLOAT
LVCS_EqualiserInit(LVCS_Handle_t hInstance,LVCS_Params_t * pParams)57 LVCS_ReturnStatus_en LVCS_EqualiserInit(LVCS_Handle_t hInstance,
58 LVCS_Params_t *pParams)
59 {
60
61 LVM_UINT16 Offset;
62 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
63 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser;
64 LVCS_Data_t *pData;
65 LVCS_Coefficient_t *pCoefficients;
66 BQ_FLOAT_Coefs_t Coeffs;
67 const BiquadA012B12CoefsSP_t *pEqualiserCoefTable;
68
69 pData = (LVCS_Data_t *) \
70 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
71
72 pCoefficients = (LVCS_Coefficient_t *) \
73 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
74 /*
75 * If the sample rate changes re-initialise the filters
76 */
77 if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
78 (pInstance->Params.SpeakerType != pParams->SpeakerType))
79 {
80 /*
81 * Setup the filter coefficients and clear the history
82 */
83 Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1 + LVM_FS_48000)));
84 pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0];
85
86 /* Left and right filters */
87 /* Convert incoming coefficients to the required format/ordering */
88 Coeffs.A0 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A0;
89 Coeffs.A1 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A1;
90 Coeffs.A2 = (LVM_FLOAT) pEqualiserCoefTable[Offset].A2;
91 Coeffs.B1 = (LVM_FLOAT)-pEqualiserCoefTable[Offset].B1;
92 Coeffs.B2 = (LVM_FLOAT)-pEqualiserCoefTable[Offset].B2;
93
94 LoadConst_Float((LVM_INT16)0, /* Value */
95 (void *)&pData->EqualiserBiquadTaps, /* Destination Cast to void:\
96 no dereferencing in function*/
97 /* Number of words */
98 (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps) / sizeof(LVM_FLOAT)));
99
100 BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance,
101 &pData->EqualiserBiquadTaps,
102 &Coeffs);
103
104 /* Callbacks */
105 switch(pEqualiserCoefTable[Offset].Scale)
106 {
107 case 13:
108 pConfig->pBiquadCallBack = BQ_2I_D16F32C13_TRC_WRA_01;
109 break;
110 case 14:
111 pConfig->pBiquadCallBack = BQ_2I_D16F32C14_TRC_WRA_01;
112 break;
113 case 15:
114 pConfig->pBiquadCallBack = BQ_2I_D16F32C15_TRC_WRA_01;
115 break;
116 }
117 }
118
119 return(LVCS_SUCCESS);
120 }
121 #else
LVCS_EqualiserInit(LVCS_Handle_t hInstance,LVCS_Params_t * pParams)122 LVCS_ReturnStatus_en LVCS_EqualiserInit(LVCS_Handle_t hInstance,
123 LVCS_Params_t *pParams)
124 {
125
126 LVM_UINT16 Offset;
127 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
128 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser;
129 LVCS_Data_t *pData = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
130 LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
131 BQ_C16_Coefs_t Coeffs;
132 const BiquadA012B12CoefsSP_t *pEqualiserCoefTable;
133
134 /*
135 * If the sample rate changes re-initialise the filters
136 */
137 if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
138 (pInstance->Params.SpeakerType != pParams->SpeakerType))
139 {
140 /*
141 * Setup the filter coefficients and clear the history
142 */
143 Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1+LVM_FS_48000)));
144 pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0];
145
146 /* Left and right filters */
147 /* Convert incoming coefficients to the required format/ordering */
148 Coeffs.A0 = (LVM_INT16) pEqualiserCoefTable[Offset].A0;
149 Coeffs.A1 = (LVM_INT16) pEqualiserCoefTable[Offset].A1;
150 Coeffs.A2 = (LVM_INT16) pEqualiserCoefTable[Offset].A2;
151 Coeffs.B1 = (LVM_INT16)-pEqualiserCoefTable[Offset].B1;
152 Coeffs.B2 = (LVM_INT16)-pEqualiserCoefTable[Offset].B2;
153
154 LoadConst_16((LVM_INT16)0, /* Value */
155 (void *)&pData->EqualiserBiquadTaps, /* Destination Cast to void:\
156 no dereferencing in function*/
157 (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps)/sizeof(LVM_INT16))); /* Number of words */
158
159 BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance,
160 &pData->EqualiserBiquadTaps,
161 &Coeffs);
162
163 /* Callbacks */
164 switch(pEqualiserCoefTable[Offset].Scale)
165 {
166 case 13:
167 pConfig->pBiquadCallBack = BQ_2I_D16F32C13_TRC_WRA_01;
168 break;
169 case 14:
170 pConfig->pBiquadCallBack = BQ_2I_D16F32C14_TRC_WRA_01;
171 break;
172 case 15:
173 pConfig->pBiquadCallBack = BQ_2I_D16F32C15_TRC_WRA_01;
174 break;
175 }
176 }
177
178 return(LVCS_SUCCESS);
179 }
180 #endif
181 /************************************************************************************/
182 /* */
183 /* FUNCTION: LVCS_Equaliser */
184 /* */
185 /* DESCRIPTION: */
186 /* Apply the equaliser filter. */
187 /* */
188 /* PARAMETERS: */
189 /* hInstance Instance Handle */
190 /* pInputOutput Pointer to the input/output buffer */
191 /* NumSamples The number of samples to process */
192 /* */
193 /* RETURNS: */
194 /* LVCS_Success Always succeeds */
195 /* */
196 /* NOTES: */
197 /* 1. Always processes in place. */
198 /* */
199 /************************************************************************************/
200 #ifdef BUILD_FLOAT
LVCS_Equaliser(LVCS_Handle_t hInstance,LVM_FLOAT * pInputOutput,LVM_UINT16 NumSamples)201 LVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t hInstance,
202 LVM_FLOAT *pInputOutput,
203 LVM_UINT16 NumSamples)
204 {
205
206 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
207 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser;
208 LVCS_Coefficient_t *pCoefficients;
209
210
211 pCoefficients = (LVCS_Coefficient_t *) \
212 pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
213
214
215 /*
216 * Check if the equaliser is required
217 */
218 if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0)
219 {
220 /* Apply filter to the left and right channels */
221 (pConfig->pBiquadCallBack)((Biquad_FLOAT_Instance_t*) \
222 &pCoefficients->EqualiserBiquadInstance,
223 (LVM_FLOAT *)pInputOutput,
224 (LVM_FLOAT *)pInputOutput,
225 (LVM_INT16)NumSamples);
226 }
227
228 return(LVCS_SUCCESS);
229 }
230 #else
LVCS_Equaliser(LVCS_Handle_t hInstance,LVM_INT16 * pInputOutput,LVM_UINT16 NumSamples)231 LVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t hInstance,
232 LVM_INT16 *pInputOutput,
233 LVM_UINT16 NumSamples)
234 {
235
236 LVCS_Instance_t *pInstance = (LVCS_Instance_t *)hInstance;
237 LVCS_Equaliser_t *pConfig = (LVCS_Equaliser_t *)&pInstance->Equaliser;
238 LVCS_Coefficient_t *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
239
240
241 /*
242 * Check if the equaliser is required
243 */
244 if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0)
245 {
246 /* Apply filter to the left and right channels */
247 (pConfig->pBiquadCallBack)((Biquad_Instance_t*)&pCoefficients->EqualiserBiquadInstance,
248 (LVM_INT16 *)pInputOutput,
249 (LVM_INT16 *)pInputOutput,
250 (LVM_INT16)NumSamples);
251 }
252
253 return(LVCS_SUCCESS);
254 }
255 #endif
256