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
57LVCS_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     = (LVCS_Data_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_DATA].pBaseAddress;
65    LVCS_Coefficient_t  *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
66    BQ_C16_Coefs_t      Coeffs;
67    const BiquadA012B12CoefsSP_t *pEqualiserCoefTable;
68
69    /*
70     * If the sample rate changes re-initialise the filters
71     */
72    if ((pInstance->Params.SampleRate != pParams->SampleRate) ||
73        (pInstance->Params.SpeakerType != pParams->SpeakerType))
74    {
75        /*
76         * Setup the filter coefficients and clear the history
77         */
78        Offset = (LVM_UINT16)(pParams->SampleRate + (pParams->SpeakerType * (1+LVM_FS_48000)));
79        pEqualiserCoefTable = (BiquadA012B12CoefsSP_t*)&LVCS_EqualiserCoefTable[0];
80
81        /* Left and right filters */
82        /* Convert incoming coefficients to the required format/ordering */
83        Coeffs.A0 = (LVM_INT16) pEqualiserCoefTable[Offset].A0;
84        Coeffs.A1 = (LVM_INT16) pEqualiserCoefTable[Offset].A1;
85        Coeffs.A2 = (LVM_INT16) pEqualiserCoefTable[Offset].A2;
86        Coeffs.B1 = (LVM_INT16)-pEqualiserCoefTable[Offset].B1;
87        Coeffs.B2 = (LVM_INT16)-pEqualiserCoefTable[Offset].B2;
88
89        LoadConst_16((LVM_INT16)0,                                                       /* Value */
90                     (void *)&pData->EqualiserBiquadTaps,   /* Destination Cast to void:\
91                                                               no dereferencing in function*/
92                     (LVM_UINT16)(sizeof(pData->EqualiserBiquadTaps)/sizeof(LVM_INT16)));    /* Number of words */
93
94        BQ_2I_D16F32Css_TRC_WRA_01_Init(&pCoefficients->EqualiserBiquadInstance,
95                                        &pData->EqualiserBiquadTaps,
96                                        &Coeffs);
97
98        /* Callbacks */
99        switch(pEqualiserCoefTable[Offset].Scale)
100        {
101            case 13:
102                pConfig->pBiquadCallBack  = BQ_2I_D16F32C13_TRC_WRA_01;
103                break;
104            case 14:
105                pConfig->pBiquadCallBack  = BQ_2I_D16F32C14_TRC_WRA_01;
106                break;
107            case 15:
108                pConfig->pBiquadCallBack  = BQ_2I_D16F32C15_TRC_WRA_01;
109                break;
110        }
111    }
112
113    return(LVCS_SUCCESS);
114}
115
116/************************************************************************************/
117/*                                                                                  */
118/* FUNCTION:                LVCS_Equaliser                                          */
119/*                                                                                  */
120/* DESCRIPTION:                                                                     */
121/*  Apply the equaliser filter.                                                     */
122/*                                                                                  */
123/* PARAMETERS:                                                                      */
124/*  hInstance               Instance Handle                                         */
125/*  pInputOutput            Pointer to the input/output buffer                      */
126/*  NumSamples              The number of samples to process                        */
127/*                                                                                  */
128/* RETURNS:                                                                         */
129/*  LVCS_Success            Always succeeds                                         */
130/*                                                                                  */
131/* NOTES:                                                                           */
132/*  1.  Always processes in place.                                                  */
133/*                                                                                  */
134/************************************************************************************/
135
136LVCS_ReturnStatus_en LVCS_Equaliser(LVCS_Handle_t       hInstance,
137                                    LVM_INT16           *pInputOutput,
138                                    LVM_UINT16          NumSamples)
139{
140
141    LVCS_Instance_t     *pInstance = (LVCS_Instance_t  *)hInstance;
142    LVCS_Equaliser_t    *pConfig   = (LVCS_Equaliser_t  *)&pInstance->Equaliser;
143    LVCS_Coefficient_t  *pCoefficients = (LVCS_Coefficient_t *)pInstance->MemoryTable.Region[LVCS_MEMREGION_PERSISTENT_FAST_COEF].pBaseAddress;
144
145
146    /*
147     * Check if the equaliser is required
148     */
149    if ((pInstance->Params.OperatingMode & LVCS_EQUALISERSWITCH) != 0)
150    {
151        /* Apply filter to the left and right channels */
152        (pConfig->pBiquadCallBack)((Biquad_Instance_t*)&pCoefficients->EqualiserBiquadInstance,
153                                   (LVM_INT16 *)pInputOutput,
154                                   (LVM_INT16 *)pInputOutput,
155                                   (LVM_INT16)NumSamples);
156    }
157
158    return(LVCS_SUCCESS);
159}
160
161