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