Home | History | Annotate | Download | only in src
      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 
     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 
    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