/* SPDX-License-Identifier: LGPL-2.1-only OR BSD-3-Clause */
/******************************************************************************
 *
 * This file is provided under a dual license.  When you use or
 * distribute this software, you may choose to be licensed under
 * version 2.1 of the GNU Lesser General Public License
 * ("LGPLv2.1 License") or BSD License.
 *
 * LGPLv2.1 License
 *
 * Copyright(C) 2019 MediaTek Inc.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of version 2.1 of the GNU Lesser General Public
 * License as published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See https://www.gnu.org/licenses/old-licenses/lgpl-2.1.html for more
 * details.
 *
 * BSD LICENSE
 *
 * Copyright(C) 2019 MediaTek Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *  * Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *****************************************************************************/

/*-----------------------------------------------------------------------------
 *
 * $Author: sakamoto $
 * $Date: 2017/10/16 $
 * $RCSfile: aud_cust_amp_if.c,v $
 * $Revision: #3 $
 *
 *---------------------------------------------------------------------------*/

#define AUD_EXTERN_FILE

//=======================
// [**CAUTION**]
// Please exclude Loader
//=======================
#if ((!defined(CC_MTK_PRELOADER)) && (!defined(CC_MTK_LOADER)))

//---------------------------------------------------------------------------
// Include files
//---------------------------------------------------------------------------

/* =======================================================
 [**CAUTION**]
 You can only include header files in
 \target\mtxxxx\xxxx_driver\drv_inc\
 or files in \vm_linux\output\sysbuild\xxxx\rel or debug\inc\drv_inc\
 or files in the same directory.
========================================================= */
#include <linux/slab.h>
#include <asm/io.h>

#include "aud_cust_amp_if.h"
#include "sif_if.h"
#include "aud_if.h"
#include "aud_cfg.h"
#include "drvcust_if.h"
#include "sakura_aud_aq_data.h"
#include "ostg_if.h"

//---------------------------------------------------------------------------
// Configurations
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// Constant definitions
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
// Type definitions
//---------------------------------------------------------------------------

#define AMP_DATA_NAME_MAX (32)

struct aud_cust_amp_func_tbl;

typedef struct
{
    unsigned char name[AMP_DATA_NAME_MAX];
    UINT16 u2ClkDivAQAmp;
    UINT8 u1DevAddrAQAmp;
    UINT8 u1AmpTable;
    UINT32 u4ErrCount;
    UINT32 u4ResetErrCount;
    UINT32 u4NotifyErrThreshold;
    AUD_CUST_AMP_MUTE_T eMuteState;
    struct aud_cust_amp_func_tbl* funcTbl;
    struct list_head list;
}AMP_DATA_T;

typedef void (*AMP_CUST_INIT)(AMP_DATA_T* pAmp);
typedef void (*AMP_CUST_SUSPEND)(AMP_DATA_T* pAmp);
typedef void (*AMP_CUST_RESUME)(AMP_DATA_T* pAmp);
typedef void (*AMP_CUST_MUTE)(AMP_DATA_T* pAmp, AUD_CUST_AMP_MUTE_T mute);
typedef void (*AMP_CUST_REFRESH)(AMP_DATA_T* pAmp);

struct aud_cust_amp_func_tbl
{
    AMP_CUST_INIT        pfnCustInit;
    AMP_CUST_SUSPEND     pfnCustSuspend;
    AMP_CUST_RESUME      pfnCustResume;
    AMP_CUST_MUTE        pfnCustMute;
    AMP_CUST_REFRESH     pfnCustRefresh;
};


//---------------------------------------------------------------------------
// Macro definitions
//---------------------------------------------------------------------------

#define AUD_CUST_AMP_INITIAL_THREAD_PRIORITY (100)
#define AUD_CUST_AMP_REFRESH_THREAD_PRIORITY (120)

#define AUD_CUST_AMP_I2C_CTRL0_ADDRESS       (0x10029240)
#define AUD_CUST_AMP_I2C_CTRL1_ADDRESS       (0x10029244)
#define AUD_CUST_AMP_I2C_CTRL2_ADDRESS       (0x10029248)
#define AUD_CUST_AMP_I2C_CTRL3_ADDRESS       (0x100292FC)
#define AUD_CUST_AMP_I2C_CTRL_SIZE           (4)

//---------------------------------------------------------------------------
// Imported variables
//---------------------------------------------------------------------------
extern BOOL _fgSkipMute;
extern BOOL _fgBEBoot;
//---------------------------------------------------------------------------
// Imported functions
//---------------------------------------------------------------------------

#ifdef CC_COMBINE_4K_2K
#if 1 // def CC_MT5891
extern UINT32 DRVCUST_Get_BE_Type(void);
extern BOOL IsPQICExists(void);
#endif
extern UINT32 DRVCUST_Get_2K_Model(void);
#endif

extern void AUD_GPIOOutput(UINT32 u4GPIONum, INT32 i4Polarity);
extern void _AUD_ResetDigiAmp(BOOL fgEnable);


#ifdef CUSTOM_SELF_DIAG_ENABLE
extern void SelfDiagTriggerAudI2CError(void);
#endif
extern BOOL SIF_IS_SUSPENDED(UINT8 u1CHannelID);

//---------------------------------------------------------------------------
// Static function forward declarations
//---------------------------------------------------------------------------

static void AUD_CUST_AMP_WaitInitialThreadExit(void);
static void AUD_CUST_AMP_ExitInitialThreadIfNeeded(void);
static void AUD_CUST_AMP_I2C_AMP_Init(void);
static void AUD_CUST_AMP_Refresh(void);
static void AUD_CUST_AMP_RefreshRequest(void);
static void _AMP_Mute(AUD_CUST_AMP_MUTE_T mute);

//---------------------------------------------------------------------------
// Utility function forward declarations
//---------------------------------------------------------------------------

void AUD_CUST_AMP_RegWrite(AMP_DATA_T* pAmp, UINT8 u1Add, UINT8* u1Data, UINT8 length);
UINT8 AUD_CUST_AMP_RegRead(AMP_DATA_T* pAmp, UINT8 u1Add, UINT8* readData);
void AUD_CUST_AMP_DataCopy(UINT8* data, UINT32 ref, UINT8 size);

//---------------------------------------------------------------------------
// Include each amp ctrl.
//---------------------------------------------------------------------------

//#include "aud_cust_amp_5538.c"
//#include "aud_cust_amp_5760.c"
#include "aud_cust_amp_5782.c"    // BE+TAS5782
#include "aud_cust_amp_RT9119.c"  // BE+RT9119
#include "aud_amp_RT9119.c"       // AQctrl(stand alone)
#include "aud_amp_RT9119.h"       // AQctrl(stand alone)
#include "aud_amp_RT9119Rom.c"    // AQctrl(stand alone)

//---------------------------------------------------------------------------
// Static variables
//---------------------------------------------------------------------------
static HANDLE_T _hAudAmpRefreshThread=NULL_HANDLE;
static HANDLE_T _hAudAmpRefreshSema=NULL_HANDLE;
static BOOL _hAudAmpRefreshEnable=TRUE;
static HANDLE_T _hAudAmpThread = NULL_HANDLE;

static AMP_DATA_T* _pAmpList=NULL;

static BOOL volatile _fgAmpInit = FALSE;

static BOOL _fgAmpThreadRefresh = FALSE;
static BOOL _fgAmpThreadMute = FALSE;
static AUD_CUST_AMP_MUTE_T _ampThreadMute = AUD_CUST_AMP_MUTE;

static HANDLE_T _hAudAmpCommandMutex = NULL_HANDLE;

static BOOL _fgAudInitThreadExitRequested = FALSE;
//---------------------------------------------------------------------------
// Static functions
//---------------------------------------------------------------------------

// =================================================
// [**CAUTION**]
// 1. Use printf, not LOG
// 2. You can only call public functions in xxx_if.c
//==================================================

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_InitThread
//
/** Cust AMP Initialize Thread for BE boot waiting.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
static void AUD_CUST_AMP_InitThread(void *pvArgs)
{
    UINT32 cnt = 1000;
    UINT32 i=0;

    if( (AUD_GetDampTypeFromMVI() == AUD_DAMP_TYPE_RT) &&
        (DRVCUST_Get_BE_Type() == DRV_CUST_BE_TYPE_NONE)) {
        _hAudAmpThread = NULL_HANDLE;
        x_thread_exit();
    }

    LOG(0, "AUD_CUST_AMP_InitThread START\n");

#if 1 // def CC_MT5891
    if( DRVCUST_Get_BE_Type() == 0x00){ //4k model w/o BE
#else
    if( (DRVCUST_Get_2K_Model()&0x01) == 0x00){ //2k model
#endif
        //2k GPIO control is not done in this thread.
        if( IsPQICExists() ){ //4k model w/o BE
            //_AUD_ResetDigiAmp(FALSE);
            //_AUD_ResetDigiAmp(TRUE);

            x_thread_delay(15);
            AUD_CUST_AMP_I2C_AMP_Init();

            for(i=0; i<4; i++){ // 320ms wait
                x_thread_delay(80);
                AUD_CUST_AMP_ExitInitialThreadIfNeeded();
            }
            //AUD_GPIOOutput(205, 1);
            x_thread_delay(50);

            _AMP_Mute(AUD_CUST_AMP_UNMUTE);
            AUD_CUST_AMP_Refresh();
        }
    }
    else{ //4k model
        ADAC_GpioAmpEnable1(FALSE);
        _AUD_ResetDigiAmp(FALSE);

        while(1){
            if(_fgBEBoot == TRUE){
                x_os_drv_set_timestamp("AUD be chip booting");
                break;
            }
            if(cnt == 0){
                LOG(0, "AUD be chip timeout\n");
                x_os_drv_set_timestamp("AUD be chip timeout");
                break;
            }
            cnt--;
            x_thread_delay(100);
            AUD_CUST_AMP_ExitInitialThreadIfNeeded();
        }

        // Fail Safe
        // Collateral that X_DAMP_RESET and X_DAMP_MUTE is LOW.
        for(i=0; i<30; i++){//wait 3sec after BE chip boot
            ADAC_GpioAmpEnable1(FALSE);
            _AUD_ResetDigiAmp(FALSE);
            x_thread_delay(100);
            AUD_CUST_AMP_ExitInitialThreadIfNeeded();
        }

        AUD_SetBlockDigiAmpReset(FALSE);
        _AUD_ResetDigiAmp(TRUE);

        x_thread_delay(15);

        AUD_CUST_AMP_I2C_AMP_Init();

        if(_fgSkipMute == FALSE){
            x_os_drv_set_timestamp("aud panel on");
            for(i=0; i<4; i++){ // 320ms wait
                x_thread_delay(80);
                AUD_CUST_AMP_ExitInitialThreadIfNeeded();
            }
            AUD_SetBlockDigiAmpMute(FALSE);
            if(!Aud_IsHeadphoneExist()) {
                ADAC_GpioAmpEnable1(TRUE);
            }
            x_thread_delay(50);

            _AMP_Mute(AUD_CUST_AMP_UNMUTE);
            AUD_CUST_AMP_Refresh();
        }
    }
    _hAudAmpThread = NULL_HANDLE;
    AUD_SetBlockDigiAmpReset(FALSE);
    AUD_SetBlockDigiAmpMute(FALSE);
    LOG(0, "AUD_CUST_AMP_InitThread END\n");
    x_thread_exit();
}

static void AUD_CUST_AMP_WaitInitialThreadExit(void)
{
    _fgAudInitThreadExitRequested = TRUE;
    while(_hAudAmpThread != NULL_HANDLE){
        x_thread_delay(50);
    }
    _fgAudInitThreadExitRequested = FALSE;
}

static void AUD_CUST_AMP_ExitInitialThreadIfNeeded(void)
{
    if(_fgAudInitThreadExitRequested) {
        _hAudAmpThread = NULL_HANDLE;
        LOG(0, "AUD_CUST_AMP_ExitInitialThreadIfNeeded call x_thread_exit()\n");
        x_thread_exit();
    }
}

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_I2C_AMP_Init
//
/** Initial Audio I2S digital amplifier.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
static void AUD_CUST_AMP_I2C_AMP_Init(void)
{
#if 1 //def CC_MT5891
    if( DRVCUST_Get_BE_Type() != 0x00 || IsPQICExists() ){ //4k Backend or 4KPQ
#else
    if( (DRVCUST_Get_2K_Model()&0x01) != 0x00){ //4k model only
#endif
        _fgAmpInit = TRUE;
        LOG(1, "Audio I2C_AMP_Init Done!!!\n");

        AMP_DATA_T* pAmp;
        if(_pAmpList == NULL){
            return;
        }

        if (_hAudAmpCommandMutex == NULL_HANDLE) {
            VERIFY(x_sema_create(&_hAudAmpCommandMutex,
                                 X_SEMA_TYPE_MUTEX,
                                 X_SEMA_STATE_UNLOCK) == OSR_OK);
        }
        list_for_each_entry(pAmp, &_pAmpList->list, list){
            LOG(1, "Amp type is %s\n", pAmp->name);
            if(pAmp->funcTbl->pfnCustInit != NULL){
                x_sema_lock(_hAudAmpCommandMutex, X_SEMA_OPTION_WAIT);
                LOG(0, "[DAMP] Amp[%s]:Addr(%X) init start...\n", pAmp->name, pAmp->u1DevAddrAQAmp);
                pAmp->funcTbl->pfnCustInit(pAmp);
                LOG(0, "[DAMP] Amp[%s]:Addr(%X) init end...\n", pAmp->name, pAmp->u1DevAddrAQAmp);
                x_sema_unlock(_hAudAmpCommandMutex);
            }
            AUD_CUST_AMP_ExitInitialThreadIfNeeded();
        }
    }
}

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_RefreshThread
//
/** Thread for IIC refresh.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
static void AUD_CUST_AMP_RefreshThread(void* args)
{
    while (_hAudAmpRefreshEnable) {
        if (_fgAmpThreadMute == FALSE && _fgAmpThreadRefresh == FALSE) {
            if (x_sema_lock_timeout(_hAudAmpRefreshSema, 2000) == OSR_TIMEOUT) {
                _fgAmpThreadRefresh = TRUE;
            }
        } else {
            LOG(1, "[DAMP] There is remaining request... mute(%d) refresh(%d)\n",
                _fgAmpThreadMute, _fgAmpThreadRefresh);
        }
        if (_fgAmpInit == TRUE && _hAudAmpThread == NULL_HANDLE) {
            if (_fgAmpThreadMute == TRUE) {
                _AMP_Mute(_ampThreadMute);
                _fgAmpThreadMute = FALSE;
            }
            if (_fgAmpThreadRefresh == TRUE) {
                AUD_CUST_AMP_Refresh();
                _fgAmpThreadRefresh = FALSE;
            }
        } else {
            LOG(1, "[DAMP] ignore mute[%d] & refresh[%d] while initialize.\n",
                _fgAmpThreadMute, _fgAmpThreadRefresh);
            _fgAmpThreadMute = FALSE;
            _fgAmpThreadRefresh = FALSE;
        }
    }

    _hAudAmpRefreshThread = NULL_HANDLE;
    x_thread_exit();
}

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_Refresh
//
/** Refresh execute function call each amp refresh function.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
static void AUD_CUST_AMP_Refresh(void)
{
    AMP_DATA_T* pAmp;
    if(_pAmpList == NULL){
        return;
    }
    if (_hAudAmpCommandMutex == NULL_HANDLE) {
        VERIFY(x_sema_create(&_hAudAmpCommandMutex,
                             X_SEMA_TYPE_MUTEX,
                             X_SEMA_STATE_UNLOCK) == OSR_OK);
    }
    LOG(3, "[AUD_CUST] AMP_Refresh...\n");
    list_for_each_entry(pAmp, &_pAmpList->list, list){
        if(pAmp->funcTbl->pfnCustRefresh != NULL){
            x_sema_lock(_hAudAmpCommandMutex, X_SEMA_OPTION_WAIT);
            LOG(2, "[DAMP] Amp[%s]:Addr(%X) refresh start...\n", pAmp->name, pAmp->u1DevAddrAQAmp);
            pAmp->funcTbl->pfnCustRefresh(pAmp);
            LOG(2, "[DAMP] Amp[%s]:Addr(%X) refresh end...\n", pAmp->name, pAmp->u1DevAddrAQAmp);
            x_sema_unlock(_hAudAmpCommandMutex);
        }
    }
}

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_RefreshRequest
//
/** AMP refresh reqeust.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
static void AUD_CUST_AMP_RefreshRequest(void)
{
    if(_hAudAmpRefreshSema==NULL_HANDLE){
        VERIFY(x_sema_create(&_hAudAmpRefreshSema, X_SEMA_TYPE_BINARY, X_SEMA_STATE_UNLOCK) == OSR_OK);
    }

    if(_hAudAmpRefreshThread==NULL_HANDLE){
        VERIFY(x_thread_create(&_hAudAmpRefreshThread, "AUD_CUST_AMP_RefreshThread",
                        2048,
                        DRVCUST_OptGet(eReleaseWatchdogPriority),
                        AUD_CUST_AMP_RefreshThread, 0, NULL) == OSR_OK);

    }
    x_sema_unlock(_hAudAmpRefreshSema);
}

static void _AMP_Mute(AUD_CUST_AMP_MUTE_T mute)
{
    AMP_DATA_T* pAmp;
    if (_pAmpList == NULL) {
        return;
    }
    list_for_each_entry(pAmp, &_pAmpList->list, list) {
        if (pAmp->funcTbl->pfnCustMute != NULL) {
            LOG(0, "[AUD_CUST] AMP_Mute...[%d][%s][0x%x]\n", mute, pAmp->name, pAmp->u1DevAddrAQAmp);
            pAmp->eMuteState = mute;
            pAmp->funcTbl->pfnCustMute(pAmp, mute);
        }
    }
}

static void _AMP_Suspend(void)
{
    AMP_DATA_T *pAmp;
    if (_pAmpList == NULL) {
        return;
    }
    list_for_each_entry(pAmp, &_pAmpList->list, list) {
        if (pAmp->funcTbl->pfnCustSuspend != NULL) {
            LOG(0, "[AUD_CUST] AMP_Suspend...[%s][0x%x]\n", pAmp->name, pAmp->u1DevAddrAQAmp);
            pAmp->funcTbl->pfnCustSuspend(pAmp);
        }
    }
}

//---------------------------------------------------------------------------
// Public functions for Hook
//---------------------------------------------------------------------------
void AUD_CUST_AMP_Init(void)
{
    if (_hAudAmpThread != NULL_HANDLE) {
        LOG(0, "[DAMP] init thread already working...\n");
    }

    DrvCust_AQ_SetGet(AUD_DrvCust_AQ_Get_DA_5760_SET, 0, 0, _gu4CurrentAqKey/*key*/, 0);

    VERIFY(x_thread_create(&_hAudAmpThread, "AUD_CUST_AMPInitThread",
                    2048,
                    AUD_CUST_AMP_INITIAL_THREAD_PRIORITY,
                    AUD_CUST_AMP_InitThread, 0, NULL) == OSR_OK);
}

void AUD_CUST_AMP_Mute(AUD_CUST_AMP_MUTE_T mute)
{
    if (_hAudAmpRefreshThread == NULL_HANDLE) {
        _AMP_Mute(mute);
    } else {
        _fgAmpThreadMute = TRUE;
        _ampThreadMute = mute;
        AUD_CUST_AMP_RefreshRequest();
    }
}

void AUD_CUST_AMP_Suspend( void )
{
    LOG(0, "AUD_CUST_AMP_Suspend START\n");

    AUD_CUST_AMP_WaitInitialThreadExit();

    AUD_CUST_AMP_Mute(AUD_CUST_AMP_MUTE);
    x_thread_delay(50);

    _hAudAmpRefreshEnable=FALSE;
    while(_hAudAmpRefreshThread!=NULL){
        x_sema_unlock(_hAudAmpRefreshSema);
        x_thread_delay(10);
    }

    _fgAmpInit=FALSE;
    _AMP_Suspend();
    LOG(0, "AUD_CUST_AMP_Suspend END\n");
}

void AUD_CUST_AMP_Resume( void )
{
    _hAudAmpRefreshEnable = TRUE;
    _fgAmpThreadRefresh = TRUE;
    AUD_CUST_AMP_RefreshRequest();
}

void AUD_CUST_AMP_Update( void )
{
    _fgAmpThreadRefresh = TRUE;
    AUD_CUST_AMP_RefreshRequest();
}

void AUD_CUST_AMP_StartRefresh(void)
{
    _hAudAmpRefreshEnable = TRUE;
    AUD_CUST_AMP_Update();
}

void AUD_CUST_AMP_StopRefresh(void)
{
    _hAudAmpRefreshEnable = FALSE;
    while (_hAudAmpRefreshThread != NULL_HANDLE) {
        x_sema_unlock(_hAudAmpRefreshSema);
        x_thread_delay(10);
    }
}

//---------------------------------------------------------------------------
// Other public functions
//---------------------------------------------------------------------------

void AUD_CUST_AMP_RegistAmp(eAudCustAmpType type, eAudCustAmpI2CBus bus, UINT8 addr, UINT8 table)
{
    AMP_DATA_T* pAmp;
    if (_pAmpList == NULL) {
        _pAmpList = (AMP_DATA_T*)x_mem_alloc(sizeof(AMP_DATA_T));
        if (_pAmpList == NULL) {
            LOG(0, "[AUD_CUST] 1:x_mem_alloc() failed...\n");
            return;
        }
        INIT_LIST_HEAD(&_pAmpList->list);
    }

    list_for_each_entry(pAmp, &_pAmpList->list,list) {
        if (pAmp->u2ClkDivAQAmp == bus && pAmp->u1DevAddrAQAmp == addr) {
            /*Already registered ignore.*/
            return;
        }
    }

    pAmp = (AMP_DATA_T*)x_mem_alloc(sizeof(AMP_DATA_T));
    if (pAmp == NULL) {
        LOG(0, "[AUD_CUST] 2:x_mem_alloc() failed...\n");
        return;
    }
    INIT_LIST_HEAD(&pAmp->list);

    pAmp->u2ClkDivAQAmp = bus;
    pAmp->u1DevAddrAQAmp = addr;
    pAmp->u1AmpTable = table;
    pAmp->u4ErrCount = 0;
    pAmp->u4ResetErrCount = 0;
    pAmp->u4NotifyErrThreshold = 0;

    switch (type) {
    case AUD_CUST_AMP_TYPE_5782:
        AUD_CUST_AMP_RegistAmp_5782(pAmp);
        break;
    case AUD_CUST_AMP_TYPE_RT9119:
        AUD_CUST_AMP_RegistAmp_RT9119(pAmp);
        break;
    default:
        LOG(0, "[AUD_CUST] Unknown AMP type.\n");
        x_mem_free(pAmp);
        return;
    }
    list_add_tail(&pAmp->list, &_pAmpList->list);
}

//-----------------------------------------------------------------------------
// AUD_CUST_AMP_ShowStatus
//
/** for debugging. show registed AMP list.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
void AUD_CUST_AMP_ShowStatus()
{
    AMP_DATA_T* pAmp;
    if (_pAmpList == NULL) {
        LOG(0, "_pAmpList Not found return\n");
        return;
    }

    Printf("---------------------------------------\n");
    list_for_each_entry(pAmp, &_pAmpList->list, list) {
        Printf("AMP[%s] :\n", pAmp->name);
        Printf("    BUS:0x%x, SlaveAddr:0x%x,  Table:0x%x\n"
               ,pAmp->u2ClkDivAQAmp, pAmp->u1DevAddrAQAmp, pAmp->u1AmpTable);
        Printf("    Mute:%s\n", (pAmp->eMuteState==AUD_CUST_AMP_MUTE)?"MUTE":"UNMUTE");
        Printf("    ErrorCount: %d, ResetErrorCount: %d Threshold: %d\n",
               pAmp->u4ErrCount, pAmp->u4ResetErrCount, pAmp->u4NotifyErrThreshold);
        Printf("");
    }
    Printf("---------------------------------------\n");
}

//-----------------------------------------------------------------------------
// AUD_AMP_GetDrvCustFunTbl
//
/** prepareing for old AMP interface support.
 *
 *  @retval void
 */
//-----------------------------------------------------------------------------
AMP_T* AUD_AMP_GetDrvCustFunTbl(void){
    /*Custom amp ctrl don't use AMP_T structure.*/
    return NULL;
}


//---------------------------------------------------------------------------
// Utility functions for AUD_CUST_AMP
//---------------------------------------------------------------------------
void AUD_CUST_AMP_RegWrite(AMP_DATA_T* pAmp, UINT8 u1Addr, UINT8* u1Data, UINT8 length){
    UINT32 u4WriteCount = 0;
    static BOOL isDetectedError = FALSE;

    if (SIF_IS_SUSPENDED(pAmp->u2ClkDivAQAmp))
    {
        Printf("[AUD_CUST_AMP_RegWrite] I2C is not ready.\n");
        return;
    }
    u4WriteCount = SIF_X_Write(pAmp->u2ClkDivAQAmp, SIF_CLK_DIV, pAmp->u1DevAddrAQAmp, 1 ,u1Addr, u1Data, length);
    if (u4WriteCount == 0)
    {
        if (_fgAmpInit == TRUE) {
            pAmp->u4ErrCount++;
            pAmp->u4ResetErrCount=0;

#ifdef CUSTOM_SELF_DIAG_ENABLE
            if(pAmp->u4ErrCount >= pAmp->u4NotifyErrThreshold){
                if(!isDetectedError) {
                    isDetectedError = TRUE;
                    LOG(0, "%s I2C error %s u4ErrCount=%d u2ClkDivAQAmp=%x u1DevAddrAQAmp=%x u1Addr=%x length=%x\n",
                        __FUNCTION__, pAmp->name, pAmp->u4ErrCount, pAmp->u2ClkDivAQAmp, pAmp->u1DevAddrAQAmp, u1Addr, length);
                    {
                        UINT32 reg = 0;
                        void* mem = NULL;

                        mem = ioremap(AUD_CUST_AMP_I2C_CTRL0_ADDRESS, AUD_CUST_AMP_I2C_CTRL_SIZE);
                        reg = readl(mem);
                        LOG(0, " =====> I2C_CTRL0:%08x\n", reg);
                        iounmap(mem);
                        mem = ioremap(AUD_CUST_AMP_I2C_CTRL1_ADDRESS, AUD_CUST_AMP_I2C_CTRL_SIZE);
                        reg = readl(mem);
                        LOG(0, " =====> I2C_CTRL1:%08x\n", reg);
                        iounmap(mem);
                        mem = ioremap(AUD_CUST_AMP_I2C_CTRL2_ADDRESS, AUD_CUST_AMP_I2C_CTRL_SIZE);
                        reg = readl(mem);
                        LOG(0, " =====> I2C_CTRL2:%08x\n", reg);
                        iounmap(mem);
                        mem = ioremap(AUD_CUST_AMP_I2C_CTRL3_ADDRESS, AUD_CUST_AMP_I2C_CTRL_SIZE);
                        reg = readl(mem);
                        LOG(0, " =====> I2C_CTRL3:%08x\n", reg);
                        iounmap(mem);
                    }
                }
                SelfDiagTriggerAudI2CError();
            }
#endif
        }
    }
    else{
    //        LOG(0, "AMP[%s]: %d %d %d\n", pAmp->name, u1Addr, *u1Data, length);
        pAmp->u4ResetErrCount++;
        if(pAmp->u4ResetErrCount > 1){
            pAmp->u4ErrCount=0;
            pAmp->u4ResetErrCount=0;
            isDetectedError = FALSE;
        }
    }
}

UINT8 AUD_CUST_AMP_RegRead(AMP_DATA_T* pAmp, UINT8 u1Addr, UINT8* readData){
    UINT32 u4ReadCount = 0;
    UINT8 u1Data;
    if (SIF_IS_SUSPENDED(pAmp->u2ClkDivAQAmp))
    {
        Printf("[AUD_CUST_AMP_RegRead] I2C is not ready.\n");
        return 0;
    }
    u4ReadCount = SIF_X_Read(pAmp->u2ClkDivAQAmp, SIF_CLK_DIV, pAmp->u1DevAddrAQAmp, 1 , u1Addr, &u1Data, 1);

    if (u4ReadCount == 0)
    {
        if (_fgAmpInit == TRUE){
            pAmp->u4ErrCount++;
#ifdef CUSTOM_SELF_DIAG_ENABLE
            if(pAmp->u4ErrCount >= pAmp->u4NotifyErrThreshold){
                SelfDiagTriggerAudI2CError();
                LOG(0, "%s %s FAIL\n", __FUNCTION__, pAmp->name);
            }
#endif
        }
        return 0;
    }
    else{
        pAmp->u4ResetErrCount++;
        if(pAmp->u4ResetErrCount > 1){
            pAmp->u4ErrCount = 0;
            pAmp->u4ResetErrCount = 0;
        }
        *readData = u1Data;
        return 1;
    }
}

void AUD_CUST_AMP_DataCopy(UINT8* data, UINT32 ref, UINT8 size)
{
    UINT32 i;
    UINT32 mask;
    UINT32 sf;


    mask = (0xFF << ((size - 1) * 8));
    sf = (size - 1) * 8;

    for(i=0; i<size; i++){

        data[i] = ((ref & mask) >> sf);
        mask = mask >> 8;
        sf = sf - 8;
    }
}


#endif //CC_MTK_PRELOADER, CC_MTK_LOADER
