/*********************************************************************
*    Project: MG82F5B32-DEMO
*			MG82F5B32 LQFP32_V10 EV Board (TH156A)
*			CpuCLK=12MHz, SysCLK=12MHz
*    Description:
*			TWSI0 (SCL/P40, SDA/P41) Master
*			TWSI1 (SCL/P10, SDA/P11) Master
*    Note:
*
*    Creat time:
*    Modify:
*    
*********************************************************************/
#define _MAIN_C

#include <Intrins.h>
#include <Absacc.h>

#include <Stdio.h>  // for printf

#include ".\include\REG_MG82F5Bxx.H"
#include ".\include\Type.h"
#include ".\include\API_Macro_MG82F5B32.H"
#include ".\include\API_Uart_BRGRL_MG82F5B32.H"

/*************************************************
*Set SysClk (MAX.50MHz)
*Selection:
*	11059200,12000000,
*	22118400,24000000,
*	29491200,32000000,
*	44236800,48000000
*************************************************/
#define MCU_SYSCLK		12000000
/*************************************************/
/*************************************************
*Set CpuClk (MAX.32MHz)
*	1) CpuCLK=SysCLK
*	2) CpuClk=SysClk/2
*************************************************/
#define MCU_CPUCLK		(MCU_SYSCLK)
//#define MCU_CPUCLK		(MCU_SYSCLK/2)

#define TIMER_1T_1ms_TH	((65536-(u16)(float)(1000*((float)(MCU_SYSCLK)/(float)(1000000)))) /256) 			
#define TIMER_1T_1ms_TL	((65536-(u16)(float)(1000*((float)(MCU_SYSCLK)/(float)(1000000)))) %256)

#define TIMER_12T_1ms_TH	((65536-(u16)(float)(1000*((float)(MCU_SYSCLK)/(float)(12000000)))) /256) 			
#define TIMER_12T_1ms_TL	((65536-(u16)(float)(1000*((float)(MCU_SYSCLK)/(float)(12000000)))) %256)

#define LED_G_0		P25
#define LED_R		P26
#define LED_G_1		P27


#define TWI0_SCL		P40
#define TWI0_SDA		P41

#define TWI1_SCL		P10
#define TWI1_SDA		P11

#define SLAVE_ADDRESS_A0	0xA0
#define SLAVE_ADDRESS_B0	0xB0
#define TEST_BUF_SIZE	16

u8	TWI0OvTime;
u8	TWI1OvTime;

u8 rand;

WordTypeDef TWI0TestAddr;
WordTypeDef TWI1TestAddr;

bit bES0;
u8 LedTime;
xdata u8 WriteBuf[TEST_BUF_SIZE];
xdata u8 ReadBuf[TEST_BUF_SIZE];
u8 TestBufLen;

/***********************************************************************************
*Function:   	void INT_T0(void)
*Description:	T0 Interrupt handler
*Input:   
*Output:     
*************************************************************************************/
void INT_T0(void) interrupt INT_VECTOR_T0
{
	TH0=TIMER_12T_1ms_TH;
	TL0=TIMER_12T_1ms_TL;
	if(TWI0OvTime!=0) TWI0OvTime--;
	if(TWI1OvTime!=0) TWI1OvTime--;
	if(LedTime!=0) LedTime--;
}

/***********************************************************************************
*Function:		void SendByte(u8 tByte)
*Description:	send byte
*Input:			u8 tByte: the data to be send
*Output:     
*************************************************************************************/
void SendByte(u8 tByte)
{
	bES0=ES0;
	ES0=0;
	S0BUF=tByte;
	while(TI0==0);
	TI0=0;
	ES0=bES0;
}

/***********************************************************************************
*Function:		void SendStr(BYTE* PStr)
*Description:	send string
*Input: 			u8* PStr:the string to be send
*Output:     
*************************************************************************************/
void SendStr(u8* PStr)
{
	while(*PStr != 0)
	{
		SendByte(*PStr);
		PStr ++;
	}
}

/***********************************************************************************
*Function:   	void SendHex(u8 ToSend)
*Description:	send data for HEX
*Input: 			u8 ToSend: the data to be send
*Output:     
*************************************************************************************/
void SendHex(u8 ToSend)
{
	u8 x;
	x = ToSend >> 4;
	if(x < 10)
	{
		SendByte('0'+x);
	}
	else
	{
		SendByte('A'-10+x);
	}
	x= ToSend&0x0F;
	if(x < 10)
	{
		SendByte('0'+x);
	}
	else
	{
		SendByte('A'-10+x);
	}
}

/*************************************************
*Function:     	void DelayXus(u16 xUs)
*Description:   	delay,unit:us
*Input:     		u8 Us -> *1us  (1~255)
*Output:     
*************************************************/
void DelayXus(u8 xUs)
{
	while(xUs!=0)
	{
#if (MCU_CPUCLK>=11059200)
		_nop_();
#endif
#if (MCU_CPUCLK>=14745600)
		_nop_();
		_nop_();
		_nop_();
		_nop_();
#endif
#if (MCU_CPUCLK>=16000000)
		_nop_();
#endif

#if (MCU_CPUCLK>=22118400)
		_nop_();
		_nop_();
		_nop_();
		_nop_();
		_nop_();
		_nop_();
#endif
#if (MCU_CPUCLK>=24000000)
		_nop_();
		_nop_();
#endif		
#if (MCU_CPUCLK>=29491200)
		_nop_();
		_nop_();
		_nop_();
		_nop_();
		_nop_();
		_nop_();
#endif
#if (MCU_CPUCLK>=32000000)
		_nop_();
		_nop_();
#endif

		xUs--;
	}
}

/*************************************************
*Function:     	void DelayXms(u16 xMs)
*Description:    delay,unit:ms
*Input:     		u16 xMs -> *1ms  (1~65535)
*Output:     
*************************************************/
void DelayXms(u16 xMs)
{
	while(xMs!=0)
	{
		CLRWDT();
		DelayXus(200);
		DelayXus(200);
		DelayXus(200);
		DelayXus(200);
		DelayXus(200);
		xMs--;
		
	}
}

/***********************************************************************************
*Function:   	void InitUart0_S1BRG()
*Description:	Initialize Uart0, The source of baud rate was S1BRG
*Input:   
*Output:   		
*************************************************************************************/
void InitUart0_S1BRG(void)
{	
	UART0_SetMode8bitUARTVar();			// UART0 Mode: 8-bit, Variable B.R.
	UART0_EnReception();				// Enable reception
	UART0_SetBRGFromS1BRG();			// B.R. source: S1BRG
	UART0_SetS1BRGBaudRateX2();			// S1BRG x2
	UART1_SetS1BRGSelSYSCLK();			// S1BRG clock source: SYSCLK

    // Sets B.R. value
	UART1_SetS1BRGValue(S1BRG_BRGRL_9600_2X_12000000_1T);
	
	UART1_EnS1BRG();					// Enable S1BRG
}

/***********************************************************************************
*Function:   	void InitTimer0(void)
*Description:	Initialize Timer0
*Input:   
*Output:   		
*************************************************************************************/
void InitTimer0(void)
{
	TM_SetT0Mode_1_16BIT_TIMER();			// TIMER0 Mode: 16-bit
	TM_SetT0Clock_SYSCLKDiv12();			// TIMER0 Clock source: SYSCLK/12
	TM_SetT0Gate_Disable();

	TM_SetT0LowByte(TIMER_12T_1ms_TL);		// Set TL0 value
	TM_SetT0HighByte(TIMER_12T_1ms_TH);		// Set TH0 value

	TM_EnableT0();							// Enable TIMER0
}

/***********************************************************************************
*Function:   	void InitPort()
*Description:	Initialize IO Port
*Input:   
*Output:   		
*************************************************************************************/
void InitPort(void)
{
	PORT_SetP1OpenDrain(BIT0|BIT1|BIT2|BIT3);			// Set P10,P11,P12,P13 as open drain
	PORT_EnP10P11P12P13PullHigh();						// Enable P10,P11,P12,P13 pull-high

	PORT_SetP2OpenDrain(BIT4|BIT5|BIT6|BIT7);			// Set P24,P25,P26,P27 as open drain
	PORT_EnP24P25P26P27PullHigh();						// Enable P24,P25,P26,P27 pull-high

	PORT_SetP2OpenDrain(BIT0|BIT1);						// Set P40,P41 as open drain
	PORT_EnP40P41PullHigh();							// Enable P40,P41 pull-high

}

/***********************************************************************************
*Function:   void InitTWI0_Master(void)
*Description:Initialize TWI0 for Master
*Input:   
*Output:     
*************************************************************************************/
void InitTWI0_Master()
{
	TWI0_Clear();
	TWI0_SetClock(TWI0_CLK_SYSCLK_32);
	TWI0_SetUseP40P41();
	TWI0_Enable();
}

/***********************************************************************************
*Function:   void InitTWI1_Master(void)
*Description:Initialize TWI1 for Master
*Input:   
*Output:     
*************************************************************************************/
void InitTWI1_Master()
{
	TWI1_Clear();
	TWI1_SetClock(TWI0_CLK_SYSCLK_32);
	TWI1_SetUseP10P11();
	TWI1_Enable();
}

/***********************************************************************************
*Function:   	void InitClock()
*Description:	Initialize clock
*Input:   
*Output:   		
*************************************************************************************/
void InitClock(void)
{
#if (MCU_SYSCLK==11059200)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// SysClk=11.0592MHz CpuClk=11.0592MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1);
	
#else
	// SysClk=11.0592MHz CpuClk=5.5296MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1);
#endif
#endif

#if (MCU_SYSCLK==12000000)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// SysClk=12MHz CpuClk=12MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1);
	
#else
	// SysClk=12MHz CpuClk=6MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1);
#endif
#endif

#if (MCU_SYSCLK==22118400)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// SysClk=22.1184MHz CpuClk=22.1184MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx4, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X4|OSCIn_IHRCO);
#else
	// SysClk=22.1184MHz CpuClk=11.0592MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx4, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X4|OSCIn_IHRCO);
#endif
#endif

#if (MCU_SYSCLK==24000000)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// SysClk=24MHz CpuClk=24MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx4, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X4|OSCIn_IHRCO);
#else
	// SysClk=24MHz CpuClk=12MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx4, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X4|OSCIn_IHRCO);
#endif
#endif

#if (MCU_SYSCLK==29491200)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// Cpuclk high speed
	CLK_SetCpuCLK_HighSpeed();
	// SysClk=29.491200MHz CpuClk=29.491200MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx5.33, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X533|OSCIn_IHRCO);
#else
	// SysClk=29.491200MHz CpuClk=14.7456MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx5.33, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X533|OSCIn_IHRCO);
#endif
#endif

#if (MCU_SYSCLK==32000000)
#if (MCU_CPUCLK==MCU_SYSCLK)
	// Cpuclk high speed
	CLK_SetCpuCLK_HighSpeed();
	// SysClk=32MHz CpuClk=32MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx5.33, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X533|OSCIn_IHRCO);
#else
	// SysClk=32MHz CpuClk=16MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx5.33, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X533|OSCIn_IHRCO);
#endif
#endif

#if (MCU_SYSCLK==44236800)
	// SysClk=44.2368MHz CpuClk=22.1184MHz
	CLK_SetCKCON0(IHRCO_110592MHz|CPUCLK_SYSCLK_DIV_1|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx8, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X8|OSCIn_IHRCO);
#endif

#if (MCU_SYSCLK==48000000)
	// SysClk=48MHz CpuClk=24MHz
	CLK_SetCKCON0(IHRCO_12MHz|CPUCLK_SYSCLK_DIV_2|SYSCLK_MCKDO_DIV_1|ENABLE_CKM|CKM_OSCIN_DIV_2);
	DelayXus(100);
	// IHRCO, MCK=CKMIx8, OSCin=IHRCO
	CLK_SetCKCON2(ENABLE_IHRCO|MCK_CKMI_X8|OSCIn_IHRCO);
#endif

	// P60 output MCK/4
	//CLK_P60OC_MCKDiv4();
}

/***********************************************************************************
*Function:   	void InitInterrupt()
*Description:	Initialize Interrupt
*Input:   
*Output:   		
*************************************************************************************/
void InitInterrupt(void)
{
	INT_EnTIMER0();						// Enable T0 interrupt
}	

/***********************************************************************************
*Function:       void InitSystem(void)
*Description:    Initialize MCU
*Input:   
*Output:     
*************************************************************************************/
void InitSystem(void)
{
	InitPort();
	InitClock();
	InitTWI0_Master();
	InitTWI1_Master();
	InitTimer0();
	InitUart0_S1BRG();
	InitInterrupt();

	INT_EnAll();						// Enable global interrupt
}

/***********************************************************************************
*Function:       u8 TWI0_WriteBuf(u8 DevAddr,u16 RegStartAddr,u8 *pByte,u8 Len)
*Description:    Send data to slave through TWI0
*Input:   		
*				u8 DevAddr: Slave Device address
*				u16 RegStartAddr: Slave data address
*				u8 *pBuf: Address of the buffer to be send
*				u8 Len: Data length
*Output:     
*			BYTE: 0->succeed, other->failure
*************************************************************************************/
u8 TWI0_WriteBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
{
	u8 i;
	u8 Flag;
	Flag=1;

	SICON |=STA;						// send I2C START
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.
	SICON &=~STA;

	SICON=SICON|(AA);	   

	Flag++;
	SIDAT = DevAddr&0xFE;				// send Slave Device address 
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	SIDAT = HIBYTE(RegStartAddr);		// send Slave data address high byte
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	SIDAT = LOBYTE(RegStartAddr);		// send Slave data address low byte
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.


	Flag++;
	i=0;
	while(i<Len)
	{
		if(i==(Len-1))
		{
			SICON=SICON&(~AA);	   
		}
		else
		{
			SICON=SICON|(AA);	   
		}
		SIDAT=pBuf[i];			// send data
		TWI0OvTime=5;
		SICON &=~SI;			
		while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.
		i++;
	}
	
	Flag++;
	SICON |= STO;				// send I2C STOP
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&STO)==STO){if(TWI0OvTime==0) goto TWI0_WRITE_ERR;}		// wait completed, if time overflow,then return fail.
	SICON &=~STO;

	SICON = SICON &(~SI);		// Clear flag

	return 0;
	
TWI0_WRITE_ERR:
	return Flag;
}

/***********************************************************************************
*Function:   u8 TWI0_ReadBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
*Description:
*			I2C Read data from Slave
*Input: 
*		u8 DevAddr: Slave Device address
*		u16 RegStartAddr: Slave data address
*		u8 *pBuf: Address of the buffer to be save
*		u8 Len: Data length
*Output:
*		u8: 0->succeed, other->failure
*************************************************************************************/
u8 TWI0_ReadBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
{
	u8 i;
	u8 Flag;
	Flag=1;
	
	SICON |=STA;				// send I2C START
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.
	SICON &=~STA;

	Flag++;
	SICON=SICON|(AA);	   

	SIDAT = DevAddr&0xFE;		// send Slave Device address 
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	SIDAT = HIBYTE(RegStartAddr);		// send Slave data address high byte
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	SIDAT = LOBYTE(RegStartAddr);		// send Slave data address low byte
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	SICON |= STA;				// resend I2C START
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.
	SICON &=~STA;

	Flag++;
	SIDAT = DevAddr|0x01;		// send Slave Device address,enter read mode
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.

	Flag++;
	i=0;
	while(i<Len)
	{
		if(i==(Len-1))
		{
			SICON=SICON&(~AA);	   
		}
		else
		{
			SICON=SICON|(AA);	   
		}
		TWI0OvTime=5;
		SICON &=~SI;			
		while((SICON&SI)!=SI){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.
		pBuf[i] = SIDAT; 			// read data
		i++;
	}
	
	Flag++;
	SICON |= STO;				// send I2C STOP
	TWI0OvTime=5;
	SICON &=~SI;			
	while((SICON&STO)==STO){if(TWI0OvTime==0) goto TWI0_READ_ERR;}		// wait completed, if time overflow,then return fail.
	SICON &=~STO;

	SICON = SICON &(~SI);		// clear Flag

	return 0;
	
TWI0_READ_ERR:
	SICON |= STO;				// send I2C STOP
	SICON = SICON &(~SI);		// clear Flag
	return Flag;
}

/***********************************************************************************
*Function:       u8 TWI1_WriteBuf(u8 DevAddr,u16 RegStartAddr,u8 *pByte,u8 Len)
*Description:    Send data to slave through TWI1
*Input:   		
*				u8 DevAddr: Slave Device address
*				u16 RegStartAddr: Slave data address
*				u8 *pBuf: Address of the buffer to be send
*				u8 Len: Data length
*Output:     
*			BYTE: 0->succeed, other->failure
*************************************************************************************/
u8 TWI1_WriteBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
{
	u8 i;
	u8 Flag;
	Flag=1;

	SFR_SetPage(1);

	SI1CON |=STA1;						// send I2C START
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail
	SI1CON &=~STA;

	SI1CON=SI1CON|(AA1);	   

	Flag++;
	SI1DAT = DevAddr&0xFE;				// send Slave Device address
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	SI1DAT = HIBYTE(RegStartAddr);		// send Slave data address high byte
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	SI1DAT = LOBYTE(RegStartAddr);		// send Slave data address low byte
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail


	Flag++;
	i=0;
	while(i<Len)
	{
		if(i==(Len-1))
		{
			SI1CON=SI1CON&(~AA1);	   
		}
		else
		{
			SI1CON=SI1CON|(AA1);	   
		}
		SI1DAT=pBuf[i];					// send data
		TWI1OvTime=5;
		SI1CON &=~SI1;			
		while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail
		i++;
	}
	
	Flag++;
	SI1CON |= STO1;						// send I2C STOP
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&STO1)==STO1){if(TWI1OvTime==0) goto TWI1_WRITE_ERR;}		// wait completed, if time overflow,then return fail
	SI1CON &=~STO1;

	SI1CON = SI1CON &(~SI1);			// Clear flag
	SFR_SetPage(0);
	return 0;

TWI1_WRITE_ERR:
	SI1CON |= STO1;						// send I2C STOP
	SI1CON = SI1CON &(~SI1);			// Clear flag
	SFR_SetPage(0);
	return Flag;

}

/***********************************************************************************
*Function:   u8 TWI1_ReadBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
*Description:
*			I2C Read data from Slave
*Input: 
*		u8 DevAddr: Slave Device address
*		u16 RegStartAddr: Slave data address
*		u8 *pBuf: Address of the buffer to be save
*		u8 Len: Data length
*Output:
*		u8: 0->succeed, other->failure
*************************************************************************************/
u8 TWI1_ReadBuf(u8 DevAddr,u16 RegStartAddr,u8 *pBuf,u8 Len)
{
	u8 i;
	u8 Flag;
	Flag=1;

	SFR_SetPage(1);
	
	SI1CON |=STA1;				// send I2C START
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail
	SI1CON &=~STA1;

	SI1CON=SI1CON|(AA1);	   

	Flag++;
	SI1DAT = DevAddr&0xFE;		// send Slave Device address
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	SI1DAT = HIBYTE(RegStartAddr);		// send Slave data address high byte
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	SI1DAT = LOBYTE(RegStartAddr);		// send Slave data address low byte
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	SI1CON |= STA1;				// resend I2C START
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail
	SI1CON &=~STA1;

	Flag++;
	SI1DAT = DevAddr|0x01;		// send Slave Device address,enter read mode
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail

	Flag++;
	i=0;
	while(i<Len)
	{
		if(i==(Len-1))
		{
			SI1CON=SI1CON&(~AA1);	   
		}
		else
		{
			SI1CON=SI1CON|(AA1);	   
		}
		TWI1OvTime=5;
		SI1CON &=~SI1;			
		while((SI1CON&SI1)!=SI1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail
		pBuf[i] = SI1DAT; 			// read data
		i++;
	}
	
	Flag++;
	SI1CON |= STO1;				// send I2C STOP
	TWI1OvTime=5;
	SI1CON &=~SI1;			
	while((SI1CON&STO1)==STO1){if(TWI1OvTime==0) goto TWI1_READ_ERR;}		// wait completed, if time overflow,then return fail
	SI1CON &=~STO1;

	SI1CON = SI1CON &(~SI1);		// clear Flag
	SFR_SetPage(0);
	return 0;
	
TWI1_READ_ERR:
	SI1CON |= STO1; 					// send I2C STOP
	SI1CON = SI1CON &(~SI1);			// clear Flag
	SFR_SetPage(0);
	return Flag;
}


void TWI0_TestProc(void)
{
	u8 S0Ret;
	u8 i;
	
	SendStr("\nTWI0 Test ");
	SendHex(TWI0TestAddr.B.BHigh);
	SendHex(TWI0TestAddr.B.BLow);
	
	for(i=0;i<TEST_BUF_SIZE;i++)
	{
		WriteBuf[i]=0x78+i+rand;
	}
	
	S0Ret=TWI0_WriteBuf(SLAVE_ADDRESS_A0, TWI0TestAddr.W, &WriteBuf[0],TEST_BUF_SIZE);
	if(S0Ret==0)
	{
		DelayXms(5); // EEPROM writing delay
		S0Ret=TWI0_ReadBuf(SLAVE_ADDRESS_A0,TWI0TestAddr.W,&ReadBuf[0],TEST_BUF_SIZE);
		if(S0Ret==0)
		{
			for(i=0;i<TEST_BUF_SIZE;i++)
			{
				if(WriteBuf[i] != ReadBuf[i]) break;
			}
			if(i<TEST_BUF_SIZE)
	{
				SendStr("...Err--Verify:");
				SendHex(i);
				SendByte(0x20);
				SendHex(WriteBuf[i]);
				SendByte(0x20);
				SendHex(ReadBuf[i]);
			}
			else
			{
				LED_G_0=0;
				SendStr("...SUCCESS!");
				DelayXms(50);
				LED_G_0=1;
			}
		}
		else
		{
			SendStr("...Err--R:");
			SendHex(S0Ret);
		}
	}
	else
	{
		SendStr("...Err--W:");
		SendHex(S0Ret);
	}
	if(S0Ret!=0)
	{
		// Error,reset TWI0
		InitTWI0_Master();
	
	}
	TWI0TestAddr.W=TWI0TestAddr.W+TEST_BUF_SIZE;
	if(TWI0TestAddr.W>=0x1000)
	{
		TWI0TestAddr.W=0;
	}
}

void TWI1_TestProc(void)
{
	u8 S0Ret;
	u8 i;
	
	SendStr("\nTWI1 Test ");
	SendHex(TWI1TestAddr.B.BHigh);
	SendHex(TWI1TestAddr.B.BLow);
	
	for(i=0;i<TEST_BUF_SIZE;i++)
	{
		WriteBuf[i]=0x78+i+rand;
	}
	
	S0Ret=TWI1_WriteBuf(SLAVE_ADDRESS_A0, TWI1TestAddr.W, &WriteBuf[0],TEST_BUF_SIZE);
	if(S0Ret==0)
	{
		DelayXms(5); // EEPROM writing delay
		S0Ret=TWI1_ReadBuf(SLAVE_ADDRESS_A0,TWI1TestAddr.W,&ReadBuf[0],TEST_BUF_SIZE);
		if(S0Ret==0)
		{
			for(i=0;i<TEST_BUF_SIZE;i++)
			{
				if(WriteBuf[i] != ReadBuf[i]) break;
			}
			if(i<TEST_BUF_SIZE)
			{
				SendStr("...Err--Verify:");
				SendHex(i);
				SendByte(0x20);
				SendHex(WriteBuf[i]);
				SendByte(0x20);
				SendHex(ReadBuf[i]);
			}
			else
			{
				LED_G_1=0;
				SendStr("...SUCCESS!");
				DelayXms(50);
				LED_G_1=1;
			}
		}
		else
		{
			SendStr("...Err--R:");
			SendHex(S0Ret);
		}
	}
	else
	{
		SendStr("...Err--W:");
		SendHex(S0Ret);
	}
	if(S0Ret!=0)
	{
		// Error,reset TWI1
		InitTWI1_Master();
	}
	TWI1TestAddr.W=TWI1TestAddr.W+TEST_BUF_SIZE;
	if(TWI1TestAddr.W>=0x1000)
	{
		TWI1TestAddr.W=0;
	}
}


void main()
{
	
    InitSystem();
	
 	SendStr("\nStart");
	LED_G_0=0;LED_G_1=0;LED_R=0;
	DelayXms(1000);
	LED_G_0=1;LED_G_1=1;LED_R=1;

	rand = 0x00;
	TWI0TestAddr.W=0x0000;
	TWI1TestAddr.W=0x0000;

    while(1)
    {
		rand++;
    	if(LedTime==0)
    	{
			LED_R = ~LED_R;
			LedTime=200;
			TWI0_TestProc();
			TWI1_TestProc();
		}
    }
}


