// SPDX-License-Identifier: (GPL-2.0 OR BSD-3-Clause)
/*
 * Copyright (c) 2020 MediaTek Inc.
 */

#ifndef _HAL_MSB9121_C_
#define _HAL_MSB9121_C_

//-------------------------------------------------------------------------------------------------
//  Include Files
//-------------------------------------------------------------------------------------------------
// Common Definition
#include "mtk_msb9121_data_type.h"
#include "mtk_msb9121_hal.h"
#include "mtk_msb9121.h"
#include <linux/delay.h>
#include <linux/io.h>

#define MLP_CI_CORE_ENTRY(fmt, arg...)	//pr_info("[mdbgin_CADD_CI_HAL]" fmt, ##arg)

//-------------------------------------------------------------------------------------------------
//  Driver Compiler Options
//-------------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//  Local Defines
//-------------------------------------------------------------------------------------------------

#define MSPI_SPEED_1M   1000000
#define MSPI_SPEED_54M  54000000
#define MSPI_CMD_SIZE_3 3
#define MSPI_CMD_SIZE_4 4
#define MSPI_CMD_SIZE_5 5
#define H_3 0x03
#define EIGHT 8
#define SIXTEEN 16
#define MASK_BYTE 0xFF
#define TWO 0x02
#define D_2 2
#define THREE 0x03
#define D_3 3
#define D_4 4
#define D_5 5
#define D_8 8
#define D_16 16
#define D_24 24
#define DEVICE_ID 0xd3
#define TEST_PATTERN_0 0x55
#define TEST_PATTERN_1 0xaa
#define TEST_PATTERN_2 0x12
#define TEST_PATTERN_3 0x34
#define D_256 256

//-------------------------------------------------------------------------------------------------
//  Local Structures
//-------------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//  Global Variables
//-------------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//  Local Variables
//-------------------------------------------------------------------------------------------------
unsigned char _reg_ts0_tx_drv;
unsigned char _reg_clk_fromcard_c1_sample = 0;
static unsigned char u8MspiBuf[D_256];
//-------------------------------------------------------------------------------------------------
//  Debug Functions
//-------------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//  Local Functions
//-------------------------------------------------------------------------------------------------

//-------------------------------------------------------------------------------------------------
//  Global Functions
//-------------------------------------------------------------------------------------------------

// Put this function here because hwreg_utility2 only for hal.
unsigned char u8lestHightByte;
static void mtk_msb9121_Set_Bank_base(unsigned int u32Bank)
{
	unsigned char u8MSPICmd[MSPI_CMD_SIZE_3];

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check

	if (u8lestHightByte != ((u32Bank >> SIXTEEN) & MASK_BYTE)) {
		u8MSPICmd[0] = MSPI_CMD_CFG_W;
		u8MSPICmd[1] = TWO;
		u8MSPICmd[TWO] = (u32Bank >> SIXTEEN) & MASK_BYTE;	// bank = 0x10xxxx
		u8lestHightByte = (u32Bank >> SIXTEEN) & MASK_BYTE;
		mtk_msb9121_to_spi_write(u8MSPICmd, MSPI_CMD_SIZE_3, 0, 0, 0);
	}
}

static void _MSPI_Set_SSPI_Dual_Mode(void)
{
	unsigned char u8MSPICmd[MSPI_CMD_SIZE_3];

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check

	u8MSPICmd[0] = MSPI_CMD_CFG_W;
	u8MSPICmd[1] = H_3;
	u8MSPICmd[TWO] = 0x01;
	mtk_msb9121_to_spi_write(u8MSPICmd, MSPI_CMD_SIZE_3, 0, 0, 0);
	MTK_MSB9121_INFO("dual change msmset and cs change \n");
}

static unsigned char mtk_msb9121_Check_MSPI(void)
{
	unsigned char u16data;

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check
	//MS_U32 i;


	u8lestHightByte = 0x0;
	//for( i = 0; i<100000; i++)
	//{
		//LOG(0,"check MSPI %d\n", i);
		u16data = mtk_msb9121_Read_Byte(REG_DEVICE_ID);
		if (u16data != DEVICE_ID) {
			MTK_MSB9121_ERR("\033[0;33m error 0x101ECC=0x%x	(golden=0xd3)\n\033[m", u16data);
			//panic("%s %d\n", __func__, __LINE__);
			//while(1);
		}

		mtk_msb9121_Write_Byte(REG_TEST_ADDRESS_1, TEST_PATTERN_0);
		u16data = mtk_msb9121_Read_Byte(REG_TEST_ADDRESS_1);
		if (u16data != TEST_PATTERN_0) {
			MTK_MSB9121_ERR("\033[0;33m error 0x101EF2=0x%x	(golden=0x55)\n\033[m", u16data);
			//panic("%s %d\n", __func__, __LINE__);
			//while(1);
		}

		mtk_msb9121_Write_Byte(REG_TEST_ADDRESS_2, TEST_PATTERN_1);
		u16data = mtk_msb9121_Read_Byte(REG_TEST_ADDRESS_2);
		if (u16data != TEST_PATTERN_1) {
			MTK_MSB9121_ERR("\033[0;33m error 0x101EF3=0x%x	(golden=0xaa)\n\033[m", u16data);
			//panic("%s %d\n", __func__, __LINE__);
			//while(1);
		}

		mtk_msb9121_Write_Byte(REG_TEST_ADDRESS_1, TEST_PATTERN_2);
		u16data = mtk_msb9121_Read_Byte(REG_TEST_ADDRESS_1);
		if (u16data != TEST_PATTERN_2) {
			MTK_MSB9121_ERR("\033[0;33m error 0x101EF2=0x%x	(golden=012)\n\033[m", u16data);
			//panic("%s %d\n", __func__, __LINE__);
			//while(1);
		}

		mtk_msb9121_Write_Byte(REG_TEST_ADDRESS_2, TEST_PATTERN_3);
		u16data = mtk_msb9121_Read_Byte(REG_TEST_ADDRESS_2);
		if (u16data != TEST_PATTERN_3) {
			MTK_MSB9121_ERR("\033[0;33m error 0x101EF3=0x%x	(golden=0x34)\n\033[m", u16data);
			//panic("%s %d\n", __func__, __LINE__);
			//while(1);
		}
	//}
	MTK_MSB9121_INFO("PASS\n");
	return TRUE;
}


void mtk_msb9121_Write_Byte(size_t ptrAddr, unsigned char u8Val)
{
	unsigned char u8MSPICmd[MSPI_CMD_SIZE_4];

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check

	mtk_msb9121_Set_Bank_base(ptrAddr);
	u8MSPICmd[0] = MSPI_CMD_RIU_W1;
	u8MSPICmd[1] = ptrAddr & MASK_BYTE;
	u8MSPICmd[TWO] = (ptrAddr >> EIGHT) & MASK_BYTE;
	u8MSPICmd[THREE] = u8Val;
	mtk_msb9121_to_spi_write(u8MSPICmd, MSPI_CMD_SIZE_4, 0, 0, 0);

}
EXPORT_SYMBOL(mtk_msb9121_Write_Byte);
void mtk_msb9121_Write_2Byte(size_t ptrAddr, unsigned short u16Val)
{
	//MTK_CI_INFO("[%s][%d]\n", __func__, __LINE__);//check
	unsigned char u8MSPICmd[MSPI_CMD_SIZE_5];

	mtk_msb9121_Set_Bank_base(ptrAddr);
	u8MSPICmd[0] = MSPI_CMD_RIU_W;
	u8MSPICmd[1] = ptrAddr & MASK_BYTE;
	u8MSPICmd[D_2] = (ptrAddr >> EIGHT) & MASK_BYTE;
	u8MSPICmd[D_3] = u16Val & MASK_BYTE;;
	u8MSPICmd[D_4] = (u16Val >> EIGHT) & MASK_BYTE;
	mtk_msb9121_to_spi_write(u8MSPICmd, MSPI_CMD_SIZE_5, 0, 0, 0);
	//MTK_CI_INFO("[%s][%d]\n", __func__, __LINE__);
}
EXPORT_SYMBOL(mtk_msb9121_Write_2Byte);
void mtk_msb9121_Write_ByteMask(size_t ptrAddr, unsigned char u8Val, unsigned char u8Mask)
{
	unsigned char u8reg;

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check

	u8reg = mtk_msb9121_Read_Byte(ptrAddr) & ~u8Mask;

	u8Val &= u8Mask;
	u8reg |= u8Val;
	mtk_msb9121_Write_Byte(ptrAddr, u8reg);

}
EXPORT_SYMBOL(mtk_msb9121_Write_ByteMask);

unsigned char mtk_msb9121_Read_Byte(unsigned int ptrAddr)
{
	unsigned char u8MSPICmd[MSPI_CMD_SIZE_4];
	unsigned char u8reg = 0;

	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check


	//MTK_MSB9121_INFO("ptrAddr=%x\n", ptrAddr);
	mtk_msb9121_Set_Bank_base(ptrAddr);
	u8MSPICmd[0] = MSPI_CMD_RIU_RIT1;
	u8MSPICmd[1] = ptrAddr & MASK_BYTE;
	u8MSPICmd[TWO] = (ptrAddr >> EIGHT) & MASK_BYTE;
	u8MSPICmd[THREE] = 0;	// turn around
	mtk_msb9121_to_spi_read(u8MSPICmd, MSPI_CMD_SIZE_4, &u8reg, 1, 0, 0);

	return u8reg;

}
EXPORT_SYMBOL(mtk_msb9121_Read_Byte);
unsigned char _MSPI_RWLong(unsigned char u8Cmd, unsigned short u16Addr, unsigned char *u8data,
				unsigned short u16Len, const unsigned char *u8pWriteBuffer)
{
	unsigned short i = 0;
	unsigned short Size = 0;
	unsigned short SizePcmBurst = 0;
	unsigned short LastSizePcmBurst = 0;
	unsigned char bRet = TRUE;
	unsigned char u8reg = 0;
	//MTK_CI_ERR("delay 20000ms.................\n");
	//MsOS_DelayTask(20000);
	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);
	if (u8Cmd == PCMCIA_ATTRIBMEMORY_WRITE || u8Cmd == PCMCIA_IO_WRITE) {
		while (u16Len) {
			SizePcmBurst = (u16Len > MAX_PCMCIA_BURST_WRITE_SIZE) ? MAX_PCMCIA_BURST_WRITE_SIZE : u16Len;
			//MTK_CI_INFO("PCM Burst %d, left %d\n", (int)SizePcmBurst, (int)u16Len);

			u16Len -= SizePcmBurst;
			i = 0;
			if (LastSizePcmBurst == 0) {
				//first set size and trigger
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = 0x0;	// reg_status_expect
				u8MspiBuf[i++] = 0x0;	// reg_status_mask
				u8MspiBuf[i++] = 0x0;	// reg_pcm_burst_addr_offset
				u8MspiBuf[i++] = 0x0;
				u8MspiBuf[i++] = u8Cmd;	// reg_pcm_cmd
				u8MspiBuf[i++] = 0x0;
				u8MspiBuf[i++] = u16Addr & MASK_BYTE;	// reg_adr
				u8MspiBuf[i++] = (u16Addr >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = SizePcmBurst;	// reg_total_burst_num
				u8MspiBuf[i++] = 0;
				u8MspiBuf[i++] = PCMBURST_WRITE & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_WRITE >> D_8) & MASK_BYTE;
			} else if (LastSizePcmBurst == SizePcmBurst) {
				//only trigger, the same size
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = PCMBURST_WRITE & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_WRITE >> D_8) & MASK_BYTE;
			} else {
				//change size and trigger
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = SizePcmBurst;	// reg_total_burst_num
				u8MspiBuf[i++] = 0;
				u8MspiBuf[i++] = PCMBURST_WRITE & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_WRITE >> D_8) & MASK_BYTE;
			}
			LastSizePcmBurst = SizePcmBurst;
			//MDrv_MSPI_Write_Channel(MOLESKINE_MSPI_CHANNEL, u8MspiBuf, i);
			//MTK_CI_ERR("delay wait data10000ms.................\n");
			//MsOS_DelayTask(10000);

			//MTK_CI_ERR("delay 1ms.................\n");
			//MsOS_DelayTask(1);
			//MDrv_MSPI_RWBytes(MSPI_READ_OPERATION, 0);	// clean rbf_size
			while (SizePcmBurst) {
				Size = (SizePcmBurst > MAX_MSPI_BURST_WRITE_SIZE) ? (MAX_MSPI_BURST_WRITE_SIZE) : SizePcmBurst;

				mtk_msb9121_to_spi_write(u8MspiBuf, i, u8pWriteBuffer, Size, 1);

				SizePcmBurst -= Size;
				u8pWriteBuffer += Size;

			}
			//MDrv_MSPI_Write_Channel(MOLESKINE_MSPI_CHANNEL, u8MspiBuf, i, u8pWriteBuffer, Size, 1);
			for (i = 0; i < MAX_MSPI_STATUS_COUNT; i++) {
				u8reg = mtk_msb9121_Read_Byte(REG_PCM_BURST_STATUS_0);
				if (u8reg & REG_PCM_BURST_WRITE_DONE) {
					//_mdrv_pcmcia_WriteRegMask(REG_PCM_BURST_STATUS_CLR, REG_PCM_WRITE_FINISH_CLR, REG_PCM_WRITE_FINISH_CLR);
					break;
				}
			}
			if (i == MAX_MSPI_STATUS_COUNT)
				MTK_MSB9121_ERR("[PCM] warning, BURST Write not finish\n");

			//MTK_CI_ERR("delay 10000ms.................\n");
			//MsOS_DelayTask(10000);

		}
	} else if (u8Cmd == PCMCIA_ATTRIBMEMORY_READ || u8Cmd == PCMCIA_IO_READ) {
		while (u16Len) {
			//unsigned short index = 0;
			SizePcmBurst = (u16Len > MAX_PCMCIA_BURST_READ_SIZE) ? MAX_PCMCIA_BURST_READ_SIZE : u16Len;
			//MTK_CI_INFO("PCM Burst %d, left %d\n", (int)SizePcmBurst, (int)u16Len);

			u16Len -= SizePcmBurst;

			i = 0;
			if (LastSizePcmBurst == 0) {
				//first set size and trigger
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_ADDR >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = 0x0;	// reg_status_expect
				u8MspiBuf[i++] = 0x0;	// reg_status_mask
				u8MspiBuf[i++] = 0x0;	// reg_pcm_burst_addr_offset
				u8MspiBuf[i++] = 0x0;
				u8MspiBuf[i++] = u8Cmd;	// reg_pcm_cmd
				u8MspiBuf[i++] = 0x0;
				u8MspiBuf[i++] = u16Addr & MASK_BYTE;	// reg_adr
				u8MspiBuf[i++] = (u16Addr >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = SizePcmBurst;	// reg_total_burst_num
				u8MspiBuf[i++] = 0;
				u8MspiBuf[i++] = PCMBURST_READ & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_READ >> D_8) & MASK_BYTE;
			} else if (LastSizePcmBurst == SizePcmBurst) {
				//only trigger, the same size
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_TRIGGER >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = PCMBURST_READ & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_READ >> D_8) & MASK_BYTE;
			} else {
				//change size and trigger
				u8MspiBuf[i++] = MSPI_CMD_MIU_W;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_SIZE >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = SizePcmBurst;	// reg_total_burst_num
				u8MspiBuf[i++] = 0;
				u8MspiBuf[i++] = PCMBURST_READ & MASK_BYTE;
				u8MspiBuf[i++] = (PCMBURST_READ >> D_8) & MASK_BYTE;
			}
			LastSizePcmBurst = SizePcmBurst;
			//MDrv_MSPI_RWBytes(MSPI_READ_OPERATION, 0);	// clean rbf_size
			//MDrv_MSPI_SlaveEnable_Channel(MOLESKINE_MSPI_CHANNEL,TRUE);
			mtk_msb9121_to_spi_write(u8MspiBuf, i, 0, 0, 0);
			//MDrv_MSPI_SlaveEnable_Channel(MOLESKINE_MSPI_CHANNEL,FALSE);

			for (i = 0; i < MAX_MSPI_STATUS_COUNT; i++) {
				u8reg = mtk_msb9121_Read_Byte(REG_PCM_BURST_STATUS_0);
				if (u8reg & REG_PCM_BURST_READ_DONE) {
					//_mdrv_pcmcia_WriteRegMask(REG_PCM_BURST_STATUS_CLR, REG_PCM_READ_FINISH_CLR, REG_PCM_READ_FINISH_CLR);
					break;

				}
				//MsOS_DelayTask(1);
			}
			if (i == MAX_MSPI_STATUS_COUNT) {
				MTK_MSB9121_INFO("[PCM] warning, BURST Write not finish\n");
			}

			while (SizePcmBurst) {
				unsigned char u8TmpBuf[MAX_MSPI_BURST_READ_SIZE + 1];

				Size = (SizePcmBurst > MAX_MSPI_BURST_READ_SIZE) ? MAX_MSPI_BURST_READ_SIZE : SizePcmBurst;
				//_MSPI_MIU_CheckDone();


				i = 0;
				u8MspiBuf[i++] = MSPI_CMD_MIU_R;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_RFIFO >> 0) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_RFIFO >> D_8) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_RFIFO >> D_16) & MASK_BYTE;
				u8MspiBuf[i++] = (REG_COMPANION_PCMBURST_RFIFO >> D_24) & MASK_BYTE;
				u8MspiBuf[i++] = Size + 1;	// 1 for MSPI status
				//MDrv_MSPI_RWBytes(MSPI_READ_OPERATION, 0);	// clean rbf_size
				//MDrv_MSPI_SlaveEnable_Channel(MOLESKINE_MSPI_CHANNEL, TRUE);
				mtk_msb9121_to_spi_write(u8MspiBuf, i, 0, 0, 0);
				//MDrv_MSPI_SlaveEnable_Channel(MOLESKINE_MSPI_CHANNEL, FALSE);

				//_MSPI_MIU_CheckDone();
				u8MspiBuf[0] = MSPI_CMD_MIU_ST;
				//MDrv_MSPI_SlaveEnable_Channel(MOLESKINE_MSPI_CHANNEL, TRUE);
				//MDrv_MSPI_Write_Channel(MOLESKINE_MSPI_CHANNEL, u8MspiBuf, 1);
				//MDrv_MSPI_RWBytes(MSPI_WRITE_OPERATION, 0);	// clean rbf_size
				mtk_msb9121_to_spi_read(u8MspiBuf, 1, u8TmpBuf, 1, u8data, Size);
				SizePcmBurst -= Size;
				u8data += Size;
				//MsOS_DelayTask(10);
			}
		}
	}
	return bRet;
}
EXPORT_SYMBOL(_MSPI_RWLong);
void _MSPI_BurstRst(void)
{
	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);
	mtk_msb9121_Write_ByteMask(REG_PCM_BURST_CTRL, REG_PCM_BURST_SW_RST_ON,
				  REG_PCM_BURST_SW_RST_MASK);
	mtk_msb9121_Write_ByteMask(REG_PCM_BURST_CTRL, REG_PCM_BURST_SW_RST_OFF,
				  REG_PCM_BURST_SW_RST_MASK);
}
EXPORT_SYMBOL(_MSPI_BurstRst);

void mtk_msb9121_ClkCtrl(unsigned char bEnable)
{
	MLP_CI_CORE_ENTRY("[%s][%d]\n", __func__, __LINE__);//check

	if (bEnable) {
		MTK_MSB9121_INFO("enable\n");

		//config MSPI to 1M
		mtk_msb9121_set_spi_single_mode();
		mtk_msb9121_set_spi(MSPI_SPEED_1M);
		mtk_msb9121_Check_MSPI(); //Check MSPI

		//mtk_msb9121_Set_Bank_base(REG_COMPANION_ANA_MISC_BANK);
		// MPLL enable
		mtk_msb9121_Write_ByteMask(REG_MPLL_CLK_PD,
					  MPLL_PD_DISABLE_DISABLE,
					  MPLL_PD_DISABLE_MASK);
		mtk_msb9121_Write_ByteMask(REG_MPLL_CLK_PD_CTRL,
					  MPLL_PD_CTRL_DISABLE,
					  MPLL_PD_CTRL_DISABLE_MASK);
		mtk_msb9121_Write_ByteMask(REG_MPLL_CLK_PD_CTRL1,
					  MPLL_PD_CTRL_DISABLE,
					  MPLL_PD_CTRL_DISABLE_MASK);
		mtk_msb9121_Write_ByteMask(REG_MPLL_CLK_PD_CTRL2,
					  MPLL_PD_CTRL_DISABLE,
					  MPLL_PD_CTRL_DISABLE_MASK);
		mtk_msb9121_Write_ByteMask(REG_MPLL_CLK_PD_CTRL3,
					  MPLL_PD_CTRL_DISABLE,
					  MPLL_PD_CTRL_DISABLE_MASK);
		// moleskin driving
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_SSPI_DRV, PAD_SSPI_DO_2, PAD_SSPI_DO_MASK);
		//config SSPI to 54M (sync M5 code)
		mtk_msb9121_Write_Byte(REG_CLKGEN0_SW_MCU_CLK, SELECT_TO_CKG_MCU);
		mtk_msb9121_Write_Byte(REG_CLKGEN0_CKG_MCU, CKG_MCU_216M);
		mtk_msb9121_Write_Byte(REG_CKG_MCU, SW_MCU_CLK);
		mtk_msb9121_Write_Byte(REG_CLKGEN0_CKG_SSPI, CKG_SSPI_108M);
		// PADMUX ALL PAD IN SET must before set dual mode
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_ALLPAD_IN,
					  ALL_PAD_IN_DISABLE, ALL_PAD_IN_MASK);

		//config SSPI dual mode
		//0x1002 0x29[4]
		//_MSPI_Set_SSPI_Dual_Mode();
		//MTK_MSB9121_INFO("dual\n");
		//mtk_msb9121_set_spi_dual_mode();
		//_MDrv_PCMCIA_Check_MSPI(); //Check MSPI


		//config MSPI to 54M
		mtk_msb9121_set_spi(MSPI_SPEED_54M);
		mtk_msb9121_Check_MSPI(); //Check MSPI
		//config SSPI dual mode
		//0x1002 0x29[4]
		_MSPI_Set_SSPI_Dual_Mode();
		MTK_MSB9121_INFO("dual\n");
		mtk_msb9121_set_spi_dual_mode();
		mtk_msb9121_Check_MSPI(); //Check MSPI

		//mtk_msb9121_Set_Bank_base(REG_COMPANION_TOP_BANK);
		// PCM1 GPIO0
		mtk_msb9121_Write_ByteMask(REG_PCM1_PWR_OEN, PCM1_PWR_OUT,
					  PCM1_PWR_OEN_MASK);
		mtk_msb9121_Write_ByteMask(REG_PCM1_PWR_OUT, PCM1_PWR_OUT_LOW,
					  PCM1_PWR_OUT_MASK);

		if ((mtk_msb9121_Read_Byte(REG_PCM1_PWR_OUT) & PCM1_PWR_OUT_MASK)
		    != PCM1_PWR_OUT_LOW) {
			MTK_MSB9121_ERR
			    ("\033[0;33mREG_PCM1_PWR_OUT config error 0x%x\n\033[m",
			     (unsigned int)
			     mtk_msb9121_Read_Byte(REG_PCM1_PWR_OUT));

		}
		// PADMUX
		//mtk_msb9121_Write_ByteMask(REG_CHIPTOP_ALLPAD_IN,
					  //ALL_PAD_IN_DISABLE, ALL_PAD_IN_MASK);
		//Set GPIO mode
		//Direction
		// set card detect pin as input, and other command pads are output
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_36_L, 0x08);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_36_H, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_37_L, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_37_H, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_38_L, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OEN_38_H, 0x00);

		//PE
		//due to command pad's PE control are merged
		mtk_msb9121_Write_Byte(REG_CHIPTOP_PCM_PE, 0x07);

		//Output
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_33_L, 0x20);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_33_H, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_34_L, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_34_H, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_35_L, 0x00);
		mtk_msb9121_Write_Byte(REG_PCM_GPIO_OUT_35_H, 0x00);

		//CI link setting
		HAL_COMPANION_RXTX_EN(0, 0);
		HAL_COMPANION_GPIO_OEN(0, 1);
		HAL_COMPANION_RXTX_PE(0, 1);
		HAL_COMPANION_RXTX_PS(0, 0);

		mtk_msb9121_Write_ByteMask(REG_HST0_IRQ_MASK_15_0, IRQ_UNMASK,
					  REG_PCM_IRQ_BIT_MASK);
		mtk_msb9121_Write_Byte(REG_CLKGEN0_PCM, CLK_ON);
		MTK_MSB9121_INFO("cilink power ctrl(HAL_COMPANION_PowerCtrl)\n");
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_CILINK_EN,
			CILINK_TS_RXTX_EN, CILINK_TS_RXTX_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_CILINK_SYN,
			CILINK_SYN_2B_216M, CILINK_SYN_2B_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_CILINK_SYN,
			CILINK_SYN_S_216M, CILINK_SYN_S_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_2B,
			CKG_2B_RX_INIT_FROM_ATOP, CKG_2B_RX_INIT_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C2,
			CKG_2B_RX_EXT_ENABLE, CKG_2B_RX_EXT_MASK);
		MTK_MSB9121_INFO("UnSet Cilink CKG_2B_RX_EXT_CLKINV\n");
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_2B,
			(CKG_2B_TX_FROM_MAIN | CKG_2B_TX_CLKINV), CKG_2B_TX_MASK);
		MTK_MSB9121_INFO("Set Cilink CKG_2B_TX_CLKINV\n");
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_CILINK,
			CILINK_IF_36M, CILINK_IF_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_CILINK,
			CILINK_TRACING_216M, CILINK_TRACING_MASK);
		mtk_msb9121_Write_Byte(REG_ATOP_CISYN_PD,
			CISYN_PD_RST_DISABLE);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CIPLL_DIV,
			CIPLL_OUTPUT_DIV_72M, CIPLL_OUTPUT_DIV_MASK);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CISYN_SLD,
			CISYN_SLD, CISYN_SLD_MASK);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CISYN_SLD,
			~CISYN_SLD, CISYN_SLD_MASK);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CIPLL_PD,
			CIPLL_PD_POWERUP, CIPLL_PD_MASK);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CIPLL_PD_KP1,
			CIPLL_PD_KP1_POWERUP, CIPLL_PD_KP1_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TS_SAMPLE,
			CKG_TS_SAMPLE_216M, CKG_TS_SAMPLE_MASK);
		MTK_MSB9121_INFO("HAL_COMPANION_CardEnable\n");
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C1,
			CKG_TS_TX_C1_FROM_SOC, CKG_TOCARD_C1_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C2,
			CKG_TS_TX_C2_FROM_SOC, CKG_TOCARD_C2_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_FROMCARD,
			CKG_FROMCARD_C1_TS_SAMPLE_TOP, CKG_FROMCARD_C1_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_FROMCARD,
			CKG_FROMCARD_C2_TS_SAMPLE_TOP, CKG_FROMCARD_C2_MASK);
		MTK_MSB9121_INFO("to card ts invert.......\n");
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C1_EXT,
			(CKG_TOCARD_C1_EXT_ENABLE | CKG_TOCARD_C1_EXT_INVERT),
			CKG_TOCARD_C1_EXT_MASK);
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C2_EXT,
			CKG_TOCARD_C2_EXT_ENABLE, CKG_TOCARD_C2_EXT_MASK);
		mtk_msb9121_Write_ByteMask(REG_CILINK_CMP_IF_06,
			0., INTERLOOP_RX_2B_MASK);
		MTK_MSB9121_INFO("clk divide to 72MHz : 14.4MHz (low speed)\n");
		mtk_msb9121_Write_ByteMask(REG_CLKGEN0_TOCARD_C1_EXT,
			CKG_TOCARD_C1_DIVIN_72M, CKG_TOCARD_C1_DIVIN_MASK);
		mtk_msb9121_Write_ByteMask(REG_ATOP_CIPLL_DIV,
			CIPLL_OUTPUT_DIV_72M, CIPLL_OUTPUT_DIV_MASK);
		mtk_msb9121_Write_ByteMask(REG_CILINK_C1_DIV_N_L,
			0x5, REG_CILINK_C1_DIV_N_L_MASK);
		mtk_msb9121_Write_ByteMask(REG_CILINK_C1_DIV_M,
			0x1, REG_CILINK_C1_DIV_M_MASK);
		mtk_msb9121_Write_ByteMask(REG_CILINK_C1_DIV_N_H,
			REG_CILINK_C1_DIV_LOAD, REG_CILINK_C1_DIV_LOAD_MASK);
		mtk_msb9121_Write_ByteMask(REG_CILINK_C1_DIV_N_H,
			0, REG_CILINK_C1_DIV_LOAD_MASK);
		//mtk_msb9121_Write_ByteMask(REG_CHIPTOP_TS0_TX_DRV,
			//0x99, REG_CHIPTOP_TS0_TX_DRV_MASK);
		HAL_COMPANION_TX_DRVING();
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_TS0_RX_DRV,
			0, REG_CHIPTOP_TS0_RX_DRV_MASK);
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_TS1_TX_DRV,
			0x99, REG_CHIPTOP_TS1_TX_DRV_MASK);
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_TS1_RX_DRV,
			0, REG_CHIPTOP_TS1_RX_DRV_MASK);
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_CILINK_RX_PAD_DRV_LOW,
			0x00, REG_CHIPTOP_CILINK_RX_PAD_DRV_LOW_MASK);
		MTK_MSB9121_INFO("set CILINK_RX_CLK 4mA\n");
		mtk_msb9121_Write_ByteMask(REG_CHIPTOP_CILINK_RX_PAD_DRV_HIGH,
			0x3C, REG_CHIPTOP_CILINK_RX_PAD_DRV_HIGH_MASK);
		mtk_msb9121_Write_ByteMask(REG_FIX_TO_CARD_SYNC,
			FIX_TO_CARD_SYNC, REG_FIX_TO_CARD_SYNC_MASK);

	} else {
		MTK_MSB9121_INFO("disable\n");
		mtk_msb9121_Write_Byte(REG_CLKGEN0_PCM, CLK_OFF);
	}

}
EXPORT_SYMBOL(mtk_msb9121_ClkCtrl);
unsigned char mtk_msb9121_GPIO_Polling_CardDetect(void)
{
	//MTK_MSB9121_INFO("mtk_msb9121_GPIO_Polling_CardDetect!!");
	if ((mtk_msb9121_Read_Byte(REG_PCM_GPIO_IN_30) & REG_PCM_GPIO_CARDDETECT_MASK)
		== REG_PCM_GPIO_CARD_IN) {
		msleep_interruptible(500); //500ms
		//de-bounce check
		if ((mtk_msb9121_Read_Byte(REG_PCM_GPIO_IN_30) & REG_PCM_GPIO_CARDDETECT_MASK)
			== REG_PCM_GPIO_CARD_IN) {
			//MTK_MSB9121_INFO("mtk_msb9121_GPIO_Polling_CardDetect in!!");
			return TRUE;
		}
	}
	//MTK_MSB9121_INFO("mtk_msb9121_GPIO_Polling_CardDetect out!!");
	return FALSE;

}
EXPORT_SYMBOL(mtk_msb9121_GPIO_Polling_CardDetect);

unsigned char mtk_msb9121_ChcekPCMCIAMode(void)
{

	if ((mtk_msb9121_Read_Byte(REG_CHIPTOP_PCM_EN) & PCM_ENABLE_MASK) != PCM_ENABLE)
		return FALSE;

	return TRUE;
}
EXPORT_SYMBOL(mtk_msb9121_ChcekPCMCIAMode);


unsigned char mtk_msb9121_SwitchPCMCIAMode(void)
{
	MTK_MSB9121_INFO("set msb9121 PCMCIA mode!!");
	mtk_msb9121_Write_ByteMask(REG_CHIPTOP_PCM_EN, PCM_ENABLE, PCM_ENABLE_MASK);
	if ((mtk_msb9121_Read_Byte(REG_CHIPTOP_PCM_EN) & PCM_ENABLE_MASK) != PCM_ENABLE) {
		MTK_MSB9121_ERR("set PCMCIA mode Error0!!");
		//return FALSE;
	}

	MTK_MSB9121_INFO("TS PAD: CI mode!!");
	//TS PAD: CI mode
	HAL_COMPANION_RXTX_EN(0, 1);
	HAL_COMPANION_RXTX_PE(0, 0);
	HAL_COMPANION_RXTX_PS(0, 0);
	return TRUE;

}
EXPORT_SYMBOL(mtk_msb9121_SwitchPCMCIAMode);

unsigned char mtk_msb9121_SwitchGPIOMode(void)
{

	MTK_MSB9121_INFO("set msb9121 GPIO mode!!");
	//disable pcm mode to set default GPIO mode
	mtk_msb9121_Write_ByteMask(REG_CHIPTOP_PCM_EN, 0x00, PCM_ENABLE_MASK);
	if ((mtk_msb9121_Read_Byte(REG_CHIPTOP_PCM_EN) & PCM_ENABLE_MASK) != 0x00) {
		MTK_MSB9121_ERR("set GPIO mode Error!!");
		return FALSE;
	}
	MTK_MSB9121_INFO("input pull low!!");
	//TS PAD: input pull low
	HAL_COMPANION_RXTX_EN(0, 0);
	HAL_COMPANION_GPIO_OEN(0, 1);
	HAL_COMPANION_RXTX_PE(0, 1);
	HAL_COMPANION_RXTX_PS(0, 0);
	return TRUE;
}
EXPORT_SYMBOL(mtk_msb9121_SwitchGPIOMode);


void HAL_COMPANION_RXTX_EN(unsigned char u8Eng, unsigned char bEnable)
{
	if (u8Eng == 0) {
		if (bEnable) {
			// CHIP 0x51[5] = 1(reg_ts0_tx_en), CHIP 0x51[4] = 1(reg_ts0_rx_en)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_CILINK_EN,
				(TS0_RX_EN|TS0_TX_EN), TS0_RXTX_MASK);
		} else {
			// CHIP 0x51[5] = 0(reg_ts0_tx_en), CHIP 0x51[4] = 0(reg_ts0_rx_en)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_CILINK_EN,
				RXTX_DISABLE, TS0_RXTX_MASK);
		}
	} else {
		if (bEnable) {
			// CHIP 0x51[7] = 1(reg_ts1_tx_en), CHIP 0x51[6] = 1(reg_ts1_rx_en)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_CILINK_EN,
				(TS1_RX_EN|TS1_TX_EN), TS1_RXTX_MASK);
		} else {
			// CHIP 0x51[7] = 0(reg_ts1_tx_en), CHIP 0x51[6] = 0(reg_ts1_rx_en)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_CILINK_EN,
				RXTX_DISABLE, TS1_RXTX_MASK);
		}
	}
}

void HAL_COMPANION_GPIO_OEN(unsigned char u8Eng, unsigned char bEnable)
{
	if (u8Eng == 0) {
		if (bEnable) {
			// set CHIP 0x24 = 16’hffff(reg_ts0_gpio_oen),
			// set CHIP 0x25 = 16’h003f(reg_ts0_gpio_oen)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_GPIO_OEN_LSB0,
				GPIO_OEN_EN, GPIO_OEN_MASK_L);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_GPIO_OEN_LSB1,
				GPIO_OEN_EN, GPIO_OEN_MASK_L);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_GPIO_OEN_MSB0,
				GPIO_OEN_EN, GPIO_OEN_MASK_H);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_GPIO_OEN_MSB1,
				GPIO_OEN_EN_00, GPIO_OEN_MASK_L);
		}
	} else {
		if (bEnable) {
			// set CHIP 0x2a = 16’hffff(reg_ts1_gpio_oen),
			// set CHIP 0x2b = 16’h003f(reg_ts1_gpio_oen)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_GPIO_OEN_LSB0,
				GPIO_OEN_EN, GPIO_OEN_MASK_L);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_GPIO_OEN_LSB1,
				GPIO_OEN_EN, GPIO_OEN_MASK_L);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_GPIO_OEN_MSB0,
				GPIO_OEN_EN, GPIO_OEN_MASK_H);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_GPIO_OEN_MSB1,
				GPIO_OEN_EN_00, GPIO_OEN_MASK_L);
		}
	}
}

void HAL_COMPANION_RXTX_PE(unsigned char u8Eng, unsigned char bEnable)
{
	if (u8Eng == 0) {
		if (bEnable) {
			// CHIP 0x48 = 16’hffff(reg_ts0_tx_pe), CHIP, 0x49 = 16’hffff(reg_ts0_rx_pe)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PE_LSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PE_MSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PE_LSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PE_MSB,
				PE_EN, PE_MASK);
		} else {
			// CHIP 0x48 = 16’h0000(reg_ts0_tx_pe), CHIP 0x49 = 16’h0000(reg_ts0_rx_pe)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PE_LSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PE_MSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PE_LSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PE_MSB,
				PE_EN_00, PE_MASK);
		}
	} else {
		if (bEnable) {
			// CHIP 0x4a = 16’hffff(reg_ts1_tx_pe), CHIP 0x4b = 16’hffff(reg_ts1_rx_pe)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PE_LSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PE_MSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PE_LSB,
				PE_EN, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PE_MSB,
				PE_EN, PE_MASK);
		} else {
			// CHIP 0x4a = 16’h0000(reg_ts1_tx_pe), CHIP 0x4b = 16’h0000(reg_ts1_rx_pe)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PE_LSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PE_MSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PE_LSB,
				PE_EN_00, PE_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PE_MSB,
				PE_EN_00, PE_MASK);
		}
	}
}

void HAL_COMPANION_RXTX_PS(unsigned char u8Eng, unsigned char bEnable)
{
	if (u8Eng == 0) {
		if (bEnable) {
			// CHIP 0x4c = 16’hffff(reg_ts0_tx_ps), CHIP 0x4d = 16’hffff(reg_ts0_rx_ps)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PS_LSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PS_MSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PS_LSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PS_MSB,
				PS_EN, PS_MASK);
		} else {
			// CHIP 0x4c = 16’h0000(reg_ts0_tx_ps), CHIP 0x4d = 16’h0000(reg_ts0_rx_ps)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PS_LSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_TX_PS_MSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PS_LSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS0_RX_PS_MSB,
				PS_EN_00, PS_MASK);
		}
	} else {
		if (bEnable) {
			// CHIP 0x4e = 16’hffff(reg_ts1_tx_ps), CHIP 0x4f = 16’hffff(reg_ts1_rx_ps)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PS_LSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PS_MSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PS_LSB,
				PS_EN, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PS_MSB,
				PS_EN, PS_MASK);
		} else {
			// CHIP 0x4e = 16’h0000(reg_ts1_tx_ps), CHIP 0x4f = 16’h0000(reg_ts1_rx_ps)
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PS_LSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_TX_PS_MSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PS_LSB,
				PS_EN_00, PS_MASK);
			mtk_msb9121_Write_ByteMask(REG_COMPANION_CHIPTOP_TS1_RX_PS_MSB,
				PS_EN_00, PS_MASK);
		}
	}
}
void HAL_COMPANION_TX_DRVING(void)
{
	mtk_msb9121_Write_ByteMask(REG_CHIPTOP_TS0_TX_DRV,
		_reg_ts0_tx_drv, REG_CHIPTOP_TS0_TX_DRV_MASK);
	//MTK_MSB9121_INFO("_reg_ts0_tx_drv=0x%x\n",_reg_ts0_tx_drv);
}
void mtk_msb9121_set_ts0_tx_drv(int reg_ts0_tx_drv)
{
	_reg_ts0_tx_drv = (unsigned char)reg_ts0_tx_drv;
}
void HAL_COMPANION_FROMCARD_C1_SAMPLE(void)
{
	if(_reg_clk_fromcard_c1_sample) {
		mtk_msb9121_Write_ByteMask(REG_COMPANION_SAMPLE_FROMCARD_C1,
			_reg_clk_fromcard_c1_sample, REG_COMPANION_SAMPLE_FROMCARD_C1_MASK);
		//MTK_MSB9121_INFO("_reg_clk_fromcard_c1_sample=0x%x\n",_reg_clk_fromcard_c1_sample);
	}
}
void mtk_msb9121_set_reg_clk_fromcard_c1_sample(int reg_clk_fromcard_c1_sample)
{
	_reg_clk_fromcard_c1_sample = (unsigned char)reg_clk_fromcard_c1_sample;
}

#endif
