#pragma GCC optimize ("O0")
#include "Galois_memmap.h"
#include "global.h"
#include "SysMgr.h"
#include "util.h"
#include "apb_perf_base.h"
#include "apbRegBase.h"
#include "lgpl_printf.h"

//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////PINMUX API///////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////


//Four pinmux registers, 10 pins per register, 3 bits per pin
#define		BITS_PER_PIN			3
#define     PINMUX_VALUE_MAX        ((1 << BITS_PER_PIN) -1)
#define		PINS_PER_REG			10
#define		PINMUX_CTRL_REG_NUM		4

#ifndef I2C_OK
#define I2C_OK                  0
#endif
#ifndef I2C_ERROR
#define I2C_ERROR               1
#endif
#ifndef I2C_EBADPARAM
#define I2C_EBADPARAM   2
#endif
#ifndef I2C_EBUSRW
#define I2C_EBUSRW              3
#endif

#define dbg_printf(...) lgpl_printf(__VA_ARGS__)

enum I2C_INSTANCES
{
        SOC_I2C0,
        SOC_I2C1,
        SM_I2C0,
        SM_I2C1,
        I2C_MAX
};

// I2C standard bus speed
enum
{
        I2C_STANDARD_SPEED_100K = 100,
        I2C_STANDARD_SPEED_400K = 400,
};

// I2C speed mode registers, note our IP doesn't support high speed mode
enum
{
        I2C_STANDARD_SPEED_MODE = 1, // 100K is the default
        I2C_FAST_SPEED_MODE   = 2    // 400K is the default

};

// I2C slave address type
enum
{
        I2C_7BIT_SLAVE_ADDR     = 0,
        I2C_10BIT_SLAVE_ADDR = 1
};

enum pinmuxIndexList
{
/*00*/  URT0_RXD,
/*01*/  URT0_TXD,
/*02*/  SPI1_SS0n,
/*03*/  SPI1_SS1n,
/*04*/  SPI1_SS2n,
/*05*/  SPI1_SCLK,
/*06*/  SPI1_SDO,
/*07*/  SPI1_SDI,
/*08*/  USB1_DRV_VBUS,
/*09*/  TW1_SCL,
/*10*/  TW1_SDA,
/*11*/  HDMI_CEC,
/*12*/  HDMI_HPD,
/*13*/  NAND_IO0,
/*14*/  NAND_IO1,
/*15*/  NAND_IO2,
/*16*/  NAND_IO3,
/*17*/  NAND_IO4,
/*18*/  NAND_IO5,
/*19*/  NAND_IO6,
/*20*/  NAND_IO7,
/*21*/  NAND_ALE,
/*22*/  NAND_CLE,
/*23*/  NAND_WEn,
/*24*/  NAND_REn,
/*25*/  NAND_WPn,
/*26*/  NAND_CEn,
/*27*/  NAND_RDY,
/*28*/  SD0_CLK,
/*29*/  SD0_DAT0,
/*30*/  SD0_DAT1,
/*31*/  SD0_DAT2,
/*32*/  SD0_DAT3,
/*33*/  SD0_CDn,
/*34*/  SD0_CMD,
/*35*/  SD0_WP,
/*36*/  STS0_CLK,
/*37*/  STS0_SOP,
/*38*/  STS0_VALD,
/*39*/  STS0_SD,
/*40*/  SM_TMS,
/*41*/  SM_TDI,
/*42*/  SM_TDO,
/*43*/  PINMUX_INDEX_MAX
};

static const char* pinmuxNameList[PINMUX_INDEX_MAX] =
{
	"URT0_RXD",
	"URT0_TXD",
	"SPI1_SS0n",
	"SPI1_SS1n",
	"SPI1_SS2n",
	"SPI1_SCLK",
	"SPI1_SDO",
	"SPI1_SDI",
	"USB1_DRV_VBUS",
	"TW1_SCL",
	"TW1_SDA",
	"HDMI_CEC",
	"HDMI_HPD",
	"NAND_IO0",
	"NAND_IO1",
	"NAND_IO2",
	"NAND_IO3",
	"NAND_IO4",
	"NAND_IO5",
	"NAND_IO6",
	"NAND_IO7",
	"NAND_ALE",
	"NAND_CLE",
	"NAND_WEn",
	"NAND_REn",
	"NAND_WPn",
	"NAND_CEn",
	"NAND_RDY",
	"SD0_CLK",
	"SD0_DAT0",
	"SD0_DAT1",
	"SD0_DAT2",
	"SD0_DAT3",
	"SD0_CDn",
	"SD0_CMD",
	"SD0_WP",
	"STS0_CLK",
	"STS0_SOP",
	"STS0_VALD",
	"STS0_SD",
	"SM_TMS",
	"SM_TDI",
	"SM_TDO"

};

static int diag_i2c_master_set_speed(int master_id, int mode, int speed, int hold_time);
static void set_transfer_mode(int master_id, int mode);

int pinmux_write(unsigned int index, unsigned int value)
{
        unsigned int addr, field, pinmux;

        if(index >= PINMUX_INDEX_MAX)
        {
                dbg_printf("Pinmux Index error ! MAX number is %d",PINMUX_INDEX_MAX);
                return -1;
        }

    if (value > PINMUX_VALUE_MAX)
    {
                dbg_printf("Pinmux value error ! MAX number is %d",PINMUX_VALUE_MAX);
                return -1;

    }

    if ((index == URT0_RXD) || (index == URT0_TXD))
                dbg_printf("You are changing the UART pinmux!\n");


        if(index < SM_TMS)// SOC pinmux
        {
                addr = MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + ((index / PINS_PER_REG) << 2);
                field = (index % PINS_PER_REG) * BITS_PER_PIN;
        }
        else    //SM pinmux
        {
                index -= SM_TMS;
                addr = SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus ;
                field = (index % PINS_PER_REG) * BITS_PER_PIN;
        }

        BFM_HOST_Bus_Read32(addr, &pinmux); // read current pinmux

        pinmux &= ~(0x7 << field);
        pinmux |= (value << field);

        BFM_HOST_Bus_Write32(addr, pinmux);
    return 0;
}

int pinmux_read(unsigned int index)
{
	unsigned int addr, field, pinmux;

	if(index >= PINMUX_INDEX_MAX)
	{
		dbg_printf("Pinmux Index error ! MAX number is %d",PINMUX_INDEX_MAX);
		return -1;
	}

	if(index < SM_TMS)// SOC pinmux
	{
		addr = MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + ((index / PINS_PER_REG) << 2);
		field = (index % PINS_PER_REG) * BITS_PER_PIN;
	}
	else	//SM pinmux
	{
		index -= SM_TMS;
		addr = SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus ;
		field = (index % PINS_PER_REG) * BITS_PER_PIN;
	}

	BFM_HOST_Bus_Read32(addr, &pinmux); // read current pinmux

	pinmux = (pinmux >> field) & 0x7;

	return pinmux;
}


void pinmux_rdall(void)
{
	unsigned int data1;

	for (data1 = 0; data1 < PINMUX_INDEX_MAX; data1++)
		dbg_printf( "%2d Pin Name: %s\t :: Value = %d\n", data1, pinmuxNameList[data1], pinmux_read(data1));
}

//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////GPIO API/////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////

//#define APB_GPIO_VER		0x3230342a
#define APB_GPIO_VER		0x3230392a  //CD+ Z1
int gpioSet(unsigned int gpio_instance, unsigned int direction, unsigned int state)
{
        unsigned int read;
        unsigned int gpio_base;

        switch (gpio_instance)
        {
                case 0:
                        gpio_base = APB_GPIO0_BASE;
                        break;
                case 1:
                        gpio_base = APB_GPIO1_BASE;
                        break;
                case 2:
                        gpio_base = APB_GPIO2_BASE;
                        break;
                case 3:
                        gpio_base = APB_GPIO3_BASE;
                        break;
/*              case 4:
                        gpio_base = SM_APB_GPIO0_BASE;
                        break;
                case 5:
                        gpio_base = SM_APB_GPIO1_BASE;
                        break;*/
                default:
                        dbg_printf(" invalid gpio instance\n");
                        return 1;
        }
        // check GPIO ID
        BFM_HOST_Bus_Read32((gpio_base + 0x6C), &read);
        if (read != APB_GPIO_VER)
        {
                dbg_printf(" GPIO%d ID = 0x%X incorrect, abort\n", gpio_instance, read);
                return 1;
        }

        //ctrl settings
        BFM_HOST_Bus_Write32((gpio_base + 0x08), 0x00);         // software control

        //output for all the port D pins
        BFM_HOST_Bus_Write32((gpio_base + 0x04), direction);// 1 - output, 0 - input

        BFM_HOST_Bus_Write32((gpio_base + 0x00), state);        // set state

        //BFM_HOST_Bus_Write32((PIN_MUX_REG), pinmux);          // restore pinmuxes
        //BFM_HOST_Bus_Write32((PIN_MUX1_REG), pinmux1);

        dbg_printf(" APB GPIO%d set to direction = 0x%X, state = 0x%X\n", gpio_instance, direction, state);

        return 0;
}

//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////I2C  API///////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////



// number of I2C masters in system
#define I2C_MASTER_NUM	I2C_MAX

// I2C master system clock
// minimum sysmte clock are required
// Standard Mode:	2 MHz
// Fast Mode:		10 MHz
// High-Speed Mode:	100 MHz
//#ifdef BG2_DV_BOARD
#define I2C_SYS_CLOCK   150     // 75 MHz, this is the config clock
/*#else
#define I2C_SYS_CLOCK	10	// 5 MHz
#endif*/

#define I2C_SDA_HOLD_TIME   2     // Range: [2, SCL_LCNT -2]

// I2C master TX/RX fifo size
#define I2C_RX_FIFO_SIZE        64
#define I2C_TX_FIFO_SIZE	64

// register definitions of SPI master
#define I2C_REG_CON		0x00
#define I2C_REG_TAR		0x04
#define I2C_REG_SAR		0x08
#define I2C_REG_HS_MADDR	0x0C
#define I2C_REG_DATA_CMD	0x10
#define I2C_REG_SS_SCL_HCNT	0x14
#define I2C_REG_SS_SCL_LCNT	0x18
#define I2C_REG_FS_SCL_HCNT	0x1C
#define I2C_REG_FS_SCL_LCNT	0x20
#define I2C_REG_HS_SCL_HCNT	0x24
#define I2C_REG_HS_SCL_LCNT	0x28
#define I2C_REG_INTR_STAT	0x2C
#define I2C_REG_INTR_MASK	0x30
#define I2C_REG_RINTR_STAT	0x34
#define I2C_REG_RX_TL		0x38
#define I2C_REG_TX_TL		0x3C
#define I2C_REG_CLR_INTR	0x40
#define I2C_REG_CLR_RX_UNDER	0x44
#define I2C_REG_CLR_RX_OVER	0x48
#define I2C_REG_CLR_TX_OVER	0x4C
#define I2C_REG_CLR_RD_REQ	0x50
#define I2C_REG_CLR_TX_ABRT	0x54
#define I2C_REG_CLR_RX_DONE	0x58
#define I2C_REG_CLR_ACTIVITY	0x5C
#define I2C_REG_CLR_STOP_DET	0x60
#define I2C_REG_CLR_START_DET	0x64
#define I2C_REG_CLR_GEN_CALL	0x68
#define I2C_REG_ENABLE		0x6C
#define I2C_REG_STATUS		0x70
#define I2C_REG_TXFLR		0x74
#define I2C_REG_RXFLR		0x78
#define I2C_REG_SDA_HOLD		0x7C  //for SDA_HOLD
#define I2C_REG_TX_ABRT_STAT	0x80
#define I2C_REG_DMA_CR		0x88
#define I2C_REG_DMA_TDLR	0x8C
#define I2C_REG_DMA_RDLR	0x90
#define I2C_REG_SDA_SETUP		0x94  //for SDA_SETUP
#define I2C_REG_COMP_PARAM	0xF4
#define I2C_REG_COMP_VERSION	0xF8
#define I2C_REG_COMP_TYPE	0xFC // reset value: 0x44570140

// Macros of I2C_REG_CON bit offset
#define CON_SLAVE_DISABLE	6
#define CON_RESTART_EN		5
#define CON_10BITADDR		4
#define CON_SPEED		1
#define CON_MASTER_ENABLE	0

// Macros of I2C_REG_TAR bit offset
#define TAR_10BITADDR	12
#define TAR_SPECIAL	11
#define TAR_GC_OR_START	10

// Macros of I2C_REG_INTR_* bit definition
#define INTR_GEN_CALL	0x800
#define INTR_START_DET	0x400
#define INTR_STOP_DET	0x200
#define INTR_ACTIVITY	0x100
#define INTR_RX_DONE	0x080
#define INTR_TX_ABRT	0x040
#define INTR_RD_REQ		0x020
#define INTR_TX_EMPTY	0x010
#define INTR_TX_OVER	0x008
#define INTR_RX_FULL	0x004
#define INTR_RX_OVER	0x002
#define INTR_RX_UNDER	0x001

// Macros of I2C_REG_ENABLE bit definition
#define EN_ENABLE	0x01

// Macros of read-command
#define READ_CMD	0x100

// Macros to interrupt mask flag
#define MASK	0
#define UNMASK	1

// Macros of read/write I2C master registers
#define I2C_RegRead(a, pv)	BFM_HOST_Bus_Read32(base_addr+(a), pv) /*(*(volatile int *)(base_addr+(a)))*/
#define I2C_RegWrite(a, v)	BFM_HOST_Bus_Write32(base_addr+(a), v) /**(volatile int *)(base_addr+(a)) = (v)*/

// I2C master's transfer buffer, shared by TX and RX
static int i2c_master_buffer[I2C_MASTER_NUM][256+4];
// number of command which have been written to TX fifo so far,
// this will equal bytes_of_write_cmd + bytes_of_read_cmd at the end of transaction
static int bytes_written_to_txfifo[I2C_MASTER_NUM];
// number of bytes read out from RX fifo so far,
static int bytes_read_from_rxfifo[I2C_MASTER_NUM];
// number of write-command which need to be written to TX fifo
static int bytes_of_write_cmd[I2C_MASTER_NUM];
// number of read-command which need to be written to TX fifo
static int bytes_of_read_cmd[I2C_MASTER_NUM];
// variable indicating transaction finishing or not
static volatile int transaction_done[I2C_MASTER_NUM];


// I2C master's base-address vector
static const unsigned int I2C_MASTER_BASEADDR[I2C_MASTER_NUM] = {APB_I2C0_BASE, APB_I2C1_BASE, SM_APB_I2C0_BASE, SM_APB_I2C1_BASE};

/*****************************************************
 * set I2C master TX FIFO underflow threshold
 * thrld: 0 - 255
 *
 *****************************************************/
static void set_txfifo_threshold(int master_id, int thrld)
{
	unsigned int base_addr;

	if (thrld<0 || thrld>255) return;

	base_addr = I2C_MASTER_BASEADDR[master_id];

	I2C_RegWrite(I2C_REG_TX_TL, thrld);

	return;
}

/*****************************************************
 * set I2C master RX FIFO overflow threshold
 * thrld: 1 - 256
 *
 *****************************************************/
static void set_rxfifo_threshold(int master_id, int thrld)
{
	unsigned int base_addr;

	if (thrld<1 || thrld>256) return;

	base_addr = I2C_MASTER_BASEADDR[master_id];

	I2C_RegWrite(I2C_REG_RX_TL, thrld-1);

	return;
}

/*********************************************************
 * FUNCTION: init I2C master: set default bus speed
 * PARAMS: master_id - id of I2C master to init
 * RETURN: I2C_OK - succeed
 *         I2C_ERROR - invalid parameter
 ********************************************************/
int diag_i2c_master_init(int master_id)
{
	unsigned int base_addr;
	int i;

	if ((master_id < 0) || (master_id > I2C_MASTER_NUM-1))
	{
		dbg_printf(  "I2C wrong master id\n");
		return (I2C_ERROR);
	}

	base_addr = I2C_MASTER_BASEADDR[master_id];

	// disable I2C module first
	I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);

	// set TX/RX fifo default threshold
	set_txfifo_threshold(master_id, 0);
	set_rxfifo_threshold(master_id, I2C_RX_FIFO_SIZE/2);

        // disable all I2C interrupts.
	I2C_RegWrite(I2C_REG_INTR_MASK, 0);

	// disable slave mode, enable restart function, enable master mode
	I2C_RegRead(I2C_REG_CON, &i);
	i |= (1<<CON_SLAVE_DISABLE)|(1<<CON_RESTART_EN)|(1<<CON_MASTER_ENABLE);
	I2C_RegWrite(I2C_REG_CON, i);

	// set default I2C master speed: 100KHz fast-speed mode
	diag_i2c_master_set_speed(master_id, I2C_STANDARD_SPEED_MODE, I2C_STANDARD_SPEED_100K, I2C_SDA_HOLD_TIME);
    //diag_i2c_master_set_speed(master_id, I2C_FAST_SPEED_MODE, I2C_STANDARD_SPEED_400K, I2C_SDA_HOLD_TIME);

	I2C_RegRead(I2C_REG_INTR_MASK, &i);

	return (I2C_OK);
}

static int addr_type = I2C_7BIT_SLAVE_ADDR;

int diag_i2c_master_set_addr_type(int type)
{
	addr_type = type;
	return 0;
}

/*********************************************************************
 * FUNCTION: set I2C master bus speed
 * PARAMS: master_id - id of I2C master to config
 *         mode - STANDARD(1) or FAST(2)
 *         speed - in unit of KHz
 * RETURN: I2C_OK - succeed
 *         I2C_ERROR - I2C module is enabled or speed not support
 ********************************************************************/
int diag_i2c_master_set_speed(int master_id, int mode, int speed, int hold_time)
{
	unsigned int base_addr;
	int i, sda_hold_max;

	if ((master_id < 0) || (master_id > I2C_MASTER_NUM-1))
	{
		dbg_printf(  "I2C wrong master id\n");
		return (I2C_ERROR);
	}

	base_addr = I2C_MASTER_BASEADDR[master_id];

	// check whether I2C module is disabled
	I2C_RegRead(I2C_REG_ENABLE, &i);
	if (i & EN_ENABLE)
	{
		dbg_printf(  "I2C is busy\n");
		return (I2C_ERROR);
	}

	switch (mode)
	{
		case I2C_STANDARD_SPEED_MODE: // standard-speed
			I2C_RegRead(I2C_REG_CON, &i);
			i &= ~(3<<CON_SPEED);
			i |= (1<<CON_SPEED);
			I2C_RegWrite(I2C_REG_CON, i); /* set master to operate in standard-speed mode */
			if(speed==I2C_STANDARD_SPEED_100K)
			{
				//use the default standard 100KHz speed
				I2C_RegWrite(I2C_REG_SS_SCL_HCNT, 4000*I2C_SYS_CLOCK/1000); /* 4000-ns minimum HIGH-period*/
				I2C_RegWrite(I2C_REG_SS_SCL_LCNT, 4700*I2C_SYS_CLOCK/1000); /* 4700-ns minimum LOW-period*/
			}
			else
			{
				// calculate high/low period of SCL in 1/3(high)-2/3(low) criteria.
				i = 1000000/(speed*3);
				I2C_RegWrite(I2C_REG_SS_SCL_HCNT, i*I2C_SYS_CLOCK/1000);
				i <<= 1;
				I2C_RegWrite(I2C_REG_SS_SCL_LCNT, i*I2C_SYS_CLOCK/1000);
			}
			break;
		case I2C_FAST_SPEED_MODE: // fast-speed
			I2C_RegRead(I2C_REG_CON, &i);
			i &= ~(3<<CON_SPEED);
			i |= (2<<CON_SPEED);
			I2C_RegWrite(I2C_REG_CON, i); /* set master to operate in fast-speed mode */
			if(speed==I2C_STANDARD_SPEED_400K)
			{
				//use the default fast 400KHz speed
				I2C_RegWrite(I2C_REG_FS_SCL_HCNT, 600*I2C_SYS_CLOCK/1000); /* 600-ns minimum HIGH-period*/
				I2C_RegWrite(I2C_REG_FS_SCL_LCNT, 1300*I2C_SYS_CLOCK/1000); /* 1300-ns minimum LOW-period*/
			}
			else
			{
								// calculate high/low period of SCL in 1/3(high)-2/3(low) criteria.
				i = 1000000/(speed*3);
				I2C_RegWrite(I2C_REG_FS_SCL_HCNT, i*I2C_SYS_CLOCK/1000);
				i <<= 1;
				I2C_RegWrite(I2C_REG_FS_SCL_LCNT, i*I2C_SYS_CLOCK/1000);
			}
			break;
		default:
			dbg_printf(  "I2C unsupported speed mode\n");
			return (I2C_ERROR);
	}

	/***SDA SETUP/HOLD time tuning***/
	I2C_RegWrite(I2C_REG_SDA_HOLD, hold_time);
	I2C_RegRead(I2C_REG_SDA_HOLD, &i);

	//set_sda_setup(master_id,value);
	if(speed==I2C_STANDARD_SPEED_100K)
          I2C_RegRead(I2C_REG_SS_SCL_LCNT, &sda_hold_max);
	else if(speed==I2C_STANDARD_SPEED_400K)
          I2C_RegRead(I2C_REG_FS_SCL_LCNT, &sda_hold_max);



	if ((i < 2) || (i > sda_hold_max-2))
	{
		dbg_printf("I2C wrong sda_hold setting.\n");
		return (I2C_ERROR);
	}

	return (I2C_OK);
}

/***********************************************************************
 * FUNCTION: read bytes from slave device
 * PARAMS: master_id - id of I2C master to operate
 *         slave_addr - address of slave device
 *         pwbuf - pointer of write buffer
 *         wnum - number of bytes to write
 *         prbuf - pointer of read buffer
 *         rnum - number of bytes to read
 * RETURN: I2C_OK - succeed
 *         I2C_ERROR - I2C module is enabled, or read failed
 * NOTE: maximum write bytes is 260, maximum read bytes is 256 in a single
 *       transaction
 ***********************************************************************/
int diag_i2c_master_writeread_bytes(int master_id, int slave_addr, unsigned char *pwbuf, int wnum, unsigned char *prbuf, int rnum)
{
	unsigned int base_addr;
	int i, j;
	int timeout=1000000;

	if ((master_id < 0) || (master_id > I2C_MASTER_NUM-1))
	{
        dbg_printf(  "I2C wrong master id\n");
		return (I2C_ERROR);
    }

	if (wnum>260 || rnum>256)
	{
        dbg_printf(  "I2C number of bytes too big\n");
		return (I2C_ERROR);
    }

	if (addr_type!=I2C_7BIT_SLAVE_ADDR && addr_type!=I2C_10BIT_SLAVE_ADDR)
	{
        dbg_printf(  "I2C wrong address type\n");
		return (I2C_ERROR);
    }

	base_addr = I2C_MASTER_BASEADDR[master_id];

	// check whether I2C module is disabled
	I2C_RegRead(I2C_REG_ENABLE, &i);
	if (i & EN_ENABLE)
	{
        dbg_printf(  "I2C is busy\n");
		return (I2C_ERROR);
    }

	I2C_RegRead(I2C_REG_CON, &i);

	if (addr_type==I2C_7BIT_SLAVE_ADDR){ // 7-bit slave address

		// set 7-bit target address, normal transfer mode
		I2C_RegRead(I2C_REG_TAR, &i);
		i &= ~((1<<TAR_10BITADDR) | (1<<TAR_SPECIAL) | 0x3ff);
		i |= slave_addr;
		I2C_RegWrite(I2C_REG_TAR, i);

		// for SM_TW1, the dynamic address change is 0,
		// so we have to set 7-bit address type here
		I2C_RegRead(I2C_REG_CON, &i);
		i &= ~(1<<CON_10BITADDR);
		I2C_RegWrite(I2C_REG_CON, i);

	} else { // 10-bit slave address

		// set 10-bit target address, normal transfer mode
		I2C_RegRead(I2C_REG_TAR, &i);
		i &= ~((1<<TAR_SPECIAL) | 0x3ff);
		i |= ((1<<TAR_10BITADDR) | slave_addr);
		I2C_RegWrite(I2C_REG_TAR, i);

		// for SM_TW1, the dynamic address change is 0,
		// so we have to set 7-bit address type here
		I2C_RegRead(I2C_REG_CON, &i);
		i |= (1<<CON_10BITADDR);
		I2C_RegWrite(I2C_REG_CON, i);
	}

	// initiate transaction status flag
	transaction_done[master_id] = 0;

	// set TX fifo threshold to make sure TXE interrupt will be triggered
	// as soon as we unmask the TXE interupt, we will start transaction by writing CMD_DATA register then.
	set_txfifo_threshold(master_id, I2C_TX_FIFO_SIZE/2);

	// clear TX_ABRT_SOURCE by reading
	I2C_RegRead(I2C_REG_CLR_INTR, &i);

	if ((pwbuf && wnum>0) && (prbuf && rnum>0)){ // write & read transaction
		// set RX fifo threshold
		if (rnum<=I2C_RX_FIFO_SIZE)
			set_rxfifo_threshold(master_id, rnum);
		else
			set_rxfifo_threshold(master_id, I2C_RX_FIFO_SIZE/2);

		// initiate number of write and read commands which are going to be written to TX fifo
		bytes_written_to_txfifo[master_id] = 0;
		bytes_read_from_rxfifo[master_id] = 0;
		bytes_of_write_cmd[master_id] = wnum;
		bytes_of_read_cmd[master_id] = rnum;

		// write-command need to be copy to buffer first,
		// read-command don't need because it can be generated dynamically
		for (i=0; i<wnum; i++)
			i2c_master_buffer[master_id][i] = (int)pwbuf[i];

		// enable I2C master
		I2C_RegWrite(I2C_REG_ENABLE, EN_ENABLE);

		// issue write commands to TX fifo
		for (i=0; i<wnum; i++)
		{
			do{
				I2C_RegRead(I2C_REG_STATUS, &j);
			}while (!(j&0x02));	/* TX fifo is full */

			I2C_RegWrite(I2C_REG_DATA_CMD, i2c_master_buffer[master_id][i]);
		}
		// issue read commands to TX fifo and receive data from RX fifo
		i = 0;
		while (bytes_read_from_rxfifo[master_id]<rnum)
		{
			// if there are received data in RX fifo, read out to buffer
			I2C_RegRead(I2C_REG_STATUS, &j);
			if (j&0x08)	/* RX fifo is not empty */
				I2C_RegRead(I2C_REG_DATA_CMD, &(i2c_master_buffer[master_id][bytes_read_from_rxfifo[master_id]++]));
			// if TX fifo not full, issue remaining read commands to TX fifo
			if (i<rnum){
				I2C_RegRead(I2C_REG_STATUS, &j);
				if (j&0x02){	/* TX fifo is not full */
					I2C_RegWrite(I2C_REG_DATA_CMD, READ_CMD);
					i ++;
				}
			}
			if(!(--timeout)) break;
		}
		transaction_done[master_id] = 1;

	} else if (pwbuf && wnum>0) { // write-only transaction
		// initiate number of write commands which are going to be written to TX fifo
		bytes_written_to_txfifo[master_id] = 0;
		bytes_read_from_rxfifo[master_id] = 0;
		bytes_of_write_cmd[master_id] = wnum;
		bytes_of_read_cmd[master_id] = 0;

		// write-command need to be copy to buffer first,
		for (i=0; i<wnum; i++)
			i2c_master_buffer[master_id][i] = (int)pwbuf[i];

		// enable I2C master
		I2C_RegWrite(I2C_REG_ENABLE, EN_ENABLE);

		// issue write commands to TX fifo
		for (i=0; i<wnum; i++)
		{

#if 0 //original way to check TX fifo when write
			if(!(--timeout)) break;
			I2C_RegRead(I2C_REG_STATUS, &j);
			while (!(j&0x02)) /* TX fifo is full */
			{
				if (!(j&0x01)) /* but I2C master is idle */
				{
					// disable I2C master
					I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);
					/* somehow error happen !!! */
					dbg_printf(  "I2C write failed\n");
					return (I2C_ERROR);
				}
				I2C_RegRead(I2C_REG_STATUS, &j);
			}
#else
			timeout=10000;
			I2C_RegRead(I2C_REG_STATUS, &j);
			while (!(j&0x02)) /* TX fifo is full, so wait, but if it take too long, timeout*/
			{
				if(!(timeout--))
				{
					// disable I2C master
					I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);
					/* somehow error happen !!! */
					dbg_printf(  "(%d)I2C write failed\n", __LINE__);
					return (I2C_ERROR);
				}
				I2C_RegRead(I2C_REG_STATUS, &j);
			}

#endif
			I2C_RegWrite(I2C_REG_DATA_CMD, i2c_master_buffer[master_id][i]);

		}

		//add some delay, otherwise, the next error condition will get hit on fast cpu
		//fifo not empty and master is idle
		{
			volatile int delay=10000;
			while(delay--);
		}

		I2C_RegRead(I2C_REG_STATUS, &j);
		while (!(j&0x04)){ /* TX fifo is not empty */
			if(!(j&0x01)) /* but I2C master is idle */
			{
				// disable I2C master
				I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);
				/* somehow error happen !!! */
				dbg_printf(  "(%d)I2C write failed\n", __LINE__);
				return (I2C_ERROR);
			}
			I2C_RegRead(I2C_REG_STATUS, &j);
		}

		do{
			I2C_RegRead(I2C_REG_STATUS, &i);
		}while (i&0x01); /* wait until transfer finish */

		//for write only transaction, need to check TX_ABRT_SOURCE to know if tx failed
		I2C_RegRead(I2C_REG_TX_ABRT_STAT, &i);
		if(i)
		{
			// disable I2C master
			I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);
			/* somehow error happen !!! */
			dbg_printf(  "I2C tx aborted! I2C_REG_TX_ABRT_STAT = 0x%x\n", i);
			return (I2C_ERROR);
		}


		transaction_done[master_id] = 1;

	} else if (prbuf && rnum>0) { // read-only transaction
		// set RX fifo threshold
		if (rnum<=I2C_RX_FIFO_SIZE)
			set_rxfifo_threshold(master_id, rnum);
		else
			set_rxfifo_threshold(master_id, I2C_RX_FIFO_SIZE/2);
		// initiate number of read commands which are goding to be written to TX fifo
		bytes_written_to_txfifo[master_id] = 0;
		bytes_read_from_rxfifo[master_id] = 0;
		bytes_of_write_cmd[master_id] = 0;
		bytes_of_read_cmd[master_id] = rnum;

		// enable I2C master
		I2C_RegWrite(I2C_REG_ENABLE, EN_ENABLE);

		// issue read commands to TX fifo and receive data from RX fifo
		i = 0;
		while (bytes_read_from_rxfifo[master_id]<rnum)
		{
			// if there are received data in RX fifo, read out to buffer
			I2C_RegRead(I2C_REG_STATUS, &j);
			if (j&0x08)	/* RX fifo is not empty */
				I2C_RegRead(I2C_REG_DATA_CMD, &(i2c_master_buffer[master_id][bytes_read_from_rxfifo[master_id]++]));
			// if TX fifo not full, issue remaining read commands to TX fifo
			if (i<rnum){
				I2C_RegRead(I2C_REG_STATUS, &j);
				if (j&0x02){	/* TX fifo is not full */
					I2C_RegWrite(I2C_REG_DATA_CMD, READ_CMD);
					i ++;
				}
			}
			if(!(--timeout)) break;
		}
		transaction_done[master_id] = 1;
	}

    // if it is a write only transaction, wait until transfer finish
    do{
		I2C_RegRead(I2C_REG_STATUS, &i);
	}while (i&0x01); /* wait until transfer finish */

	// copy received data from buffer to prbuf if there is
	if (bytes_of_read_cmd[master_id] > 0){
		for (i=0; i<bytes_of_read_cmd[master_id]; i++)
			prbuf[i] = (unsigned char)i2c_master_buffer[master_id][i];
	}

	// disable I2C master
	I2C_RegWrite(I2C_REG_ENABLE, ~EN_ENABLE);

	if(!timeout)
	{
		dbg_printf(  "I2C Timed out\n");
		return (I2C_ERROR);
	}

	return (I2C_OK);
}


/***********************************************************************
 * FUNCTION: checks the 16 bit TX_ABRT_SOURCE register and gives the reason
 * PARAMS: master_id - id of I2C master to operate
 *         tx_abort_source - pointer to return the value from the TX_ABRT_SOURCE register
 * RETURN: I2C_OK or I2C_ERROR
 *
 ***********************************************************************/
const char* g_aApbI2C_AbortSrc[] =
{
	"(7-bit addressing) address not ACKed by any slave",
	"(10-bit addressing) 1st address byte not ACKed by any slave",
	"(10-bit addressing) 2nd address byte not ACKed by any slave",
	"address ACKed, data not ACKed by slave",
	"general call not responded by any slave",
	"general call followed by read command from user",
	"master in high speed mode and high speed master code ACKed",
	"master sent a start byte and start byte ACKed",
	"restart disabled while user trying to use master to send data in high speed mode",
	"restart disabled while user trying to send a start byte",
	"restart disabled and master send a read command in 10-bit addressing mode",
	"user attempted to use disabled master",
	"lost arbitration",
	"slave received read command while data exists in Tx FIFO",
	"slave lost bus while trasmitting data to a remote master",
	"slave requesting data to Tx and user wrote read command into Tx FIFO"
};

int diag_i2c_check_error(int master_id, unsigned int *tx_abort_source)
{
	unsigned int base_addr;
	unsigned int read, i;

	if ((master_id < 0) || (master_id > I2C_MASTER_NUM-1))
	{
        dbg_printf(  "I2C wrong master id\n");
		return (I2C_ERROR);
    }

	base_addr = I2C_MASTER_BASEADDR[master_id];

	I2C_RegRead(I2C_REG_TX_ABRT_STAT, &read);;	// check Tx Aborad Source register
	//dbg_printf(  "Tx Abort Source 0x%04X\n", read);
	*tx_abort_source = read;
	for (i = 0; i < 16; i++)
	{
		if (read & 1)
			dbg_printf(  "%s\n", g_aApbI2C_AbortSrc[i]);
		read /= 2;
		if ((read & 0xFFFF) == 0)
			break;
	}

	return (I2C_OK);
}



#define I2C_MAX_BUF_SIZE 128

static unsigned char rbuf[I2C_MAX_BUF_SIZE];
//__align(4) static unsigned char wbuf[I2C_MAX_BUF_SIZE];
static const unsigned char* wbuf= (unsigned char*)0xF7A47F00;




void diag_i2c_master_read_bytes(unsigned int data1, unsigned int data2, unsigned int data3)
{
	unsigned int iResult,data4;

			data2 = 0;			// slave address
			data3 = 1;			// number of bytes
			/* Parse command arguments. */

			if (data1 >= I2C_MAX) //GPIO simulated I2C
				//iResult = diag_i2c_gpio_master_writeread_bytes(data1-I2C_MAX, data2, I2C_7BIT_SLAVE_ADDR, 0, 0, rbuf, data3);
				dbg_printf("I2C GPIO master is not enabled\n");
			else
				iResult = diag_i2c_master_writeread_bytes(data1, data2, 0, 0, rbuf, data3);
			if (iResult)
			{
				if (data1 >= I2C_MAX)
				//	iResult |= diag_i2c_gpio_check_error(data1 - I2C_MAX);
					dbg_printf("I2C GPIO master is not enabled\n");
				else
				{
					iResult |= diag_i2c_check_error(data1, &data4);
					dbg_printf(" error code 0x%08x\n", data4);
				}
				dbg_printf(" Failed\n");
			}
			else
			{
				unsigned int i;
				dbg_printf("Read Data: ");
				for (i = 0; i < data3; i++)
					dbg_printf("0x%X ", rbuf[i]);
				dbg_printf("\n Passed\n");
			}
}

void diag_i2c_master_write_bytes(unsigned int data1, unsigned int data2, unsigned int data3)
{
	unsigned int iResult,data4;
			//clear the wbuf first, if number of bytes is
			//less than the cmd line supplied bytes, the extra bytes will still
			//be copied int wbuf but not sent to I2C. If number of bytes is
			//larger than the cmd line supplied bytes, extra zeros will be sent
/*			memset(wbuf, 0, I2C_MAX_BUF_SIZE);
			data4 = 0;	//counts how many dwords are collected
			while (1)
			{
				skipSpace( &pCmd );
				if ( *pCmd != '\0' )
					pCmd = getHexToken( pCmd, (unsigned int *)(wbuf+data4*4));
				else
					break;
				data4++;
			}*/
			if (data1 >= I2C_MAX)
				//iResult = diag_i2c_gpio_master_writeread_bytes(data1-I2C_MAX, data2, I2C_7BIT_SLAVE_ADDR, wbuf, data3, 0, 0);
				dbg_printf("I2C GPIO master is not enabled\n");
			else
				iResult = diag_i2c_master_writeread_bytes(data1, data2, (unsigned char *)wbuf, data3, 0, 0);
			if (iResult)
			{
				if (data1 >= I2C_MAX)
				{
					//iResult |= diag_i2c_gpio_check_error(data1 - I2C_MAX);
					dbg_printf("I2C GPIO master is not enabled\n");
				}
				else
				{
					iResult |= diag_i2c_check_error(data1, &data4);
					dbg_printf(" error code 0x%08x\n", data4);
				}
				dbg_printf(" Failed\n");
			}
			else
			{
				unsigned int i;
				dbg_printf( "Write Data: ");
				for (i = 0; i < data3; i++)
				{
					dbg_printf( "0x%x ", wbuf[i]);
				}
				dbg_printf("\n Passed\n");
			}
}

void diag_i2c_writeread_bytes(unsigned int data1,unsigned int  data2,unsigned int  data3,unsigned int  data4)
{

	unsigned int iResult,data5 = 0;	//counts how many dwords are collected
			/*while (1)
			{
				skipSpace( &pCmd );
				if ( *pCmd != '\0' )
					pCmd = getHexToken( pCmd, (unsigned int *)(wbuf+data5*4));
				else
					break;
				data5++;
			}*/
			if (data1 >= I2C_MAX)
				//iResult = diag_i2c_gpio_master_writeread_bytes(data1-I2C_MAX, data2, I2C_7BIT_SLAVE_ADDR, wbuf, data3, rbuf, data4);
				dbg_printf("I2C GPIO master is not enabled\n");
			else
				iResult = diag_i2c_master_writeread_bytes(data1, data2, (unsigned char *)wbuf, data3, rbuf, data4);
			if (iResult)
			{
				if (data1 >= I2C_MAX)
					//iResult |= diag_i2c_gpio_check_error(data1 - I2C_MAX);
					dbg_printf("I2C GPIO master is not enabled\n");
				else
				{
					iResult |= diag_i2c_check_error(data1, &data5);
					dbg_printf(" error code 0x%08x\n", data5);
				}
				dbg_printf(" Failed\n");
			}
			else
			{
				unsigned int i;
				dbg_printf( "Write Data: ");
				for (i = 0; i < data3; i++)
					dbg_printf( "0x%X ", wbuf[i]);
				dbg_printf( "\nRead Data: ");
				for(i = 0; i < data4; i++)
					dbg_printf( "0x%X ", rbuf[i]);
				dbg_printf("\n Passed\n");
			}
}



//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////SPI API/////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////

// polling mode vs. interrupt mode
#define POLLING_MODE

// number of SPI masters in system
//#if defined(BERLIN)
#if 1
#define SPI_MASTER_NUM	2
#else
#define SPI_MASTER_NUM	3
#endif

// SSI system clock
// SPI output clock can be calculated as: SPI_SYS_CLOCK/SPI_REG_BAUD
//#if defined(BERLIN)
#if 1
#if PLATFORM==FPGA
#define SPI_SYS_CLOCK	1500 // 1.5MHz
#define SPI_SM_CLOCK	9000 // 9MHz
#else
#define SPI_SYS_CLOCK   75000 // 75MHz, it uses config clock
#define SPI_SM_CLOCK    25000 // 25MHz
#endif
#else
#if defined(SOC_ES1)
#define SPI_SYS_CLOCK   100000 // 100 MHz
#else
#define SPI_SYS_CLOCK	5000 // 5MHz
#endif
#endif

// SPI master TX/RX FIFO size
#define SPI_TX_FIFO_SIZE	64
#define SPI_RX_FIFO_SIZE	64

// register definitions of SPI master
#define SPI_REG_CTRL0	0x00
#define SPI_REG_CTRL1	0x04
#define SPI_REG_SSIENR	0x08
#define SPI_REG_MWCR	0x0C
#define SPI_REG_SER	0x10
#define SPI_REG_BAUD	0x14
#define SPI_REG_TXFTLR	0x18
#define SPI_REG_RXFTLR	0x1C
#define SPI_REG_TXFLR	0x20
#define SPI_REG_RXFLR	0x24
#define SPI_REG_SR	0x28
#define SPI_REG_IMR	0x2C
#define SPI_REG_ISR	0x30
#define SPI_REG_RISR	0x34
#define SPI_REG_TXOICR	0x38
#define SPI_REG_RXOICR	0x3C
#define SPI_REG_RXUICR	0x40
#define SPI_REG_MSTICR	0x44
#define SPI_REG_ICR	0x48
#define SPI_REG_DMACR	0x4C
#define SPI_REG_DMATDLR	0x50
#define SPI_REG_DMARDLR	0x54
#define SPI_REG_IDR	0x58
#define SPI_REG_DR	0x60

// interrupt masks
#define IMR_MASK_RXF	0x10
#define IMR_MASK_RXO	0x08
#define IMR_MASK_RXU	0x04
#define IMR_MASK_TXO	0x02
#define IMR_MASK_TXE	0x01

// interrupt status
#define ISR_STAT_RXF	0x10
#define ISR_STAT_RXO	0x08
#define ISR_STAT_RXU	0x04
#define ISR_STAT_TXO	0x02
#define ISR_STAT_TXE	0x01

// error code definitions
#define SSI_OK           0
#define SSI_EBADPARAM   -1      // bad input parameter
#define SSI_EBUSRW      -2      // bus read/write error
#define SSI_EBUSY       -3      // SSI master is busy

enum SPI_INSTANCES
{
	SPI_0,
	SPI_1,
	SPI_MAX
};

#define IOMAPPER_SPI_MASTER	0	// master id = 0
#define IOMAPPER_SPI_DEVID	0	// CSn=0

#ifndef MAX
#define MAX(a,b) (((a)>(b))?(a):(b))
#define MIN(a,b) (((a)>(b))?(b):(a))
#endif



// Macros of read/write SPI master registers
#define SPI_RegRead(a, pv)	BFM_HOST_Bus_Read32(base_addr+(a), pv) /*(*(volatile int *)(base_addr+(a)))*/
#define SPI_RegWrite(a, v)	BFM_HOST_Bus_Write32(base_addr+(a), v) /**(volatile int *)(base_addr+(a)) = (v)*/

// SPI master's transfer buffer, shared by TX and RX.
// spi_master_buffer[][0] is the number of valid TX/RX frames in buffer
int spi_master_buffer[SPI_MASTER_NUM][256+5];
// number of receive frames
int spi_master_rx_number[SPI_MASTER_NUM];
// transmit done flag
volatile int spi_master_tx_done[SPI_MASTER_NUM];
// receive done flag
volatile int spi_master_rx_done[SPI_MASTER_NUM];

// SPI master's base-address vector
//#if defined(BERLIN)
#if 1
// We actually have only two SPI, but the driver always assume that
// SPI_BUS 0 = APB_SSI_INST0
// SPI_BUS 1 = APB_SSI_INST1
// SPI_BUS 2 = APB_SSI_INST2
const unsigned int SPI_MASTER_BASEADDR[SPI_MASTER_NUM] = {APB_SSI_INST1_BASE, APB_SSI_INST2_BASE};
#else
const unsigned int SPI_MASTER_BASEADDR[SPI_MASTER_NUM] = {APB_SSI_INST0_BASE, APB_SSI_INST1_BASE, APB_SSI_INST2_BASE};
#endif

// Macros for pca995X LED driver
/* LED mode registers (read/write) */
#define PCA995X_MODE_REGISTER_1 0x00
#define PCA995X_MODE_REGISTER_2 0x01

#define PCA995X_PWM_BASE_INDEX      0
#define PCA995X_IREFALL_INDEX       1
#define PCA995X_LEDOUT_COUNT_INDEX  2
#define PCA995X_MAX_NUM_LEDS_INDEX  3
#define PCA995X_INFO_LENGTH (PCA995X_MAX_NUM_LEDS_INDEX+1)

const unsigned char kLedInfoMap[LED_MODEL_CNT][PCA995X_INFO_LENGTH] = {
	{0x0A, 0x40, 6, 24},
	{0x08, 0x45, 4, 16},
};

/* If auto-increment is enabled, the MSb(bit) needs to be set to 1 in register
 * address according to datasheet.
 * In this driver, auto-increment is enabled in pca9956_led_init function.
 */
#define PCA995X_AUTO_INCREMENT_ADDRESS_MASK 0x80

/*************************************************
 * set SSI protocol .
 * protocol = 0: Motorola SPI
 *            1: TI SSP
 *            2: National Semiconductor Microwire
 *
 *************************************************/
static void set_ssi_protocol(int master_id, int protocol)
{
	unsigned int base_addr;
	int value;

	if (protocol<0 || protocol>2) return;

	base_addr = SPI_MASTER_BASEADDR[master_id];

	SPI_RegRead(SPI_REG_CTRL0, &value);
	value &= 0xffcf;
	value |= (protocol<<4);
	SPI_RegWrite(SPI_REG_CTRL0, value);

	return;
}

/*******************************************************************
 * FUNCTION: Set SPI master clock mode.
 * PARAMETERS: master_id - ID of SSI master to config
 *             mode - 0: polarity 0 & phase 0
 *                    1: polarity 0 & phase 1
 *                    2: polarity 1 & phase 0
 *                    3: polarity 1 & phase 1
 * RETURN: >=0 - master clock mode
 *         SSI_EBADPARAM - invalid parameters
 *         SSI_EBUSY - master is busy
 *******************************************************************/
int diag_spi_master_set_clock_mode(int master_id, int mode)
{
	unsigned int base_addr;
	int value;

	if (master_id<0 || master_id>SPI_MASTER_NUM-1) return (SSI_EBADPARAM);

	if (mode<0 || mode>3) return(SSI_EBADPARAM);

	base_addr = SPI_MASTER_BASEADDR[master_id];

	// check status first
	SPI_RegRead(SPI_REG_SR, &value);
	if (value&0x01) return (SSI_EBUSY);

	// config SPI master clock mode
	SPI_RegRead(SPI_REG_CTRL0, &value);
	value &= 0xff3f;
	value |= (mode<<6);
	SPI_RegWrite(SPI_REG_CTRL0, value);

	//return (mode);
	return (SSI_OK);
}



/*******************************************************************
 * FUNCTION: Initialize SSI master.
 *           More than one SSI masters could exist in system.
 * PARAMETERS: master_id - ID of SSI master to initialize
 * RETURN: SSI_OK - succeed
 *         SSI_EBADPARAM - invalid parameters
 *******************************************************************/
int diag_spi_master_init(int master_id)
{
	unsigned int base_addr;

	if (master_id<0 || master_id>SPI_MASTER_NUM-1) return (SSI_EBADPARAM);

	base_addr = SPI_MASTER_BASEADDR[master_id];

	// disable SPI
	SPI_RegWrite(SPI_REG_SSIENR, 0);

	// disable all interrupts
	SPI_RegWrite(SPI_REG_IMR, 0);

	// configure SSI as a SPI master device
	set_ssi_protocol(master_id, 0); // SPI protocol
	set_transfer_mode(master_id, 0); // duplex transfer mode

	// global variables for SPI master
	spi_master_rx_number[master_id] = 0;
	spi_master_tx_done[master_id] = 0;
	spi_master_rx_done[master_id] = 0;

	return (SSI_OK);
}

/*************************************************
 * set transfer mode.
 * mode = 0: full-duplex transfer mode
 *        1: transmit only
 *        2: receive only
 *        3: half-duplex tranfer mode
 *
 *************************************************/
void set_transfer_mode(int master_id, int mode)
{
	unsigned int base_addr;
	int value;

	if (mode<0 || mode>3) return;

	base_addr = SPI_MASTER_BASEADDR[master_id];

	SPI_RegRead(SPI_REG_CTRL0, &value);
	value &= 0xfcff;
	value |= (mode<<8);
	SPI_RegWrite(SPI_REG_CTRL0, value);

	return;
}

/*****************************************************
 * select slave device
 * flag: 1 - select
 *       0 - de-select
 *****************************************************/
static void select_slave_device(int master_id, int dev_id, int flag)
{
	unsigned int base_addr;

	base_addr = SPI_MASTER_BASEADDR[master_id];

	if (flag)
		SPI_RegWrite(SPI_REG_SER, 1<<dev_id);
	else
		SPI_RegWrite(SPI_REG_SER, 0);

	return;
}


/*******************************************************************
 * FUNCTION: Set SPI master speed.
 * PARAMETERS: master_id - ID of SSI master to config
 *             speed - master speed in unit of KHz
 * RETURN: >0 - master speed
 *         SSI_EBADPARAM - invalid parameters
 *         SSI_EBUSY - master is busy
 *******************************************************************/
int diag_spi_master_set_speed(int master_id, unsigned int speed)
{
	unsigned int base_addr;
	int value;

	if (master_id<0 || master_id>SPI_MASTER_NUM-1) return (SSI_EBADPARAM);

	if (speed<=0) return(SSI_EBADPARAM);

	base_addr = SPI_MASTER_BASEADDR[master_id];

	// check status first
	SPI_RegRead(SPI_REG_SR, &value);
	if (value&0x01) return (SSI_EBUSY);
//#if defined(BERLIN)
#if 1
	{
		unsigned base_clk;
		if (base_addr == APB_SSI_INST1_BASE)
			base_clk = SPI_SYS_CLOCK;
		else if (base_addr == SM_APB_SPI_BASE)
			base_clk = SPI_SM_CLOCK;
		// config SPI master speed
		if (speed>base_clk) value = 1;
		else value = (base_clk+speed-1)/speed;

		SPI_RegWrite(SPI_REG_BAUD, value);

		//dbg_printf("SPI speed set to %fkHz\n", (float)base_clk/(value&0xfffe));
		//return (base_clk/value);
		return (SSI_OK);
	}
#else
	// config SPI master speed
	if (speed>SPI_SYS_CLOCK) value = 1;
	else value = (SPI_SYS_CLOCK+speed-1)/speed;

	SPI_RegWrite(SPI_REG_BAUD, value);

	return (SPI_SYS_CLOCK/value);
#endif
}

/*******************************************************************
 * FUNCTION: half-duplex read/write frames.
 * PARAMETERS: master_id - ID of SSI master
 *             pwbuf - pointer to transmit data buffer
 *             wnum - number of frames to transmit
 *             prbuf - pointer to receive data buffer
 *             rnum - number of frames to receive
 *			   eeprom - flag when both read and write buffers have data
 *                           if 1, SPI is set to eeprom mode, first tx then rx
 *                           if 0, SPI tx and rx happens at the same time
 * RETURN: SSI_OK - transfer succeed
 *         SSI_EBADPARAM - invalid parameters
 *         SSI_EBUSY - master is busy
 *         SSI_EBUSRW - master read/write fail
 * NOTE: the maximum value of wnum is 256+4,
 *       while the maximum value of rnum is 256
 *******************************************************************/
int diag_spi_master_writeread_frames(int master_id, int dev_id, int *pwbuf, int wnum, int *prbuf, int rnum, int eeprom)
{
	unsigned int base_addr;
	int value;
#ifndef POLLING_MODE // interrupt mode
	int irqs;
#endif
	int i, j;

	if (master_id<0 || master_id>SPI_MASTER_NUM-1) return (SSI_EBADPARAM);
	if (wnum>260 || rnum>256) return (SSI_EBADPARAM);

	base_addr = SPI_MASTER_BASEADDR[master_id];

	// check status first
	SPI_RegRead(SPI_REG_SR, &value);
	if (value&0x01) return (SSI_EBUSY);



	// setup transfer
	if ((pwbuf && wnum>0) && (prbuf && rnum>0)){ // duplex write, read transfer
		// set transfer mode to half-duplex
		if(eeprom)
			set_transfer_mode(master_id, 3); //eeprom mode
		else
			set_transfer_mode(master_id, 0); //full-duplex mode

		// set number of read frames
		SPI_RegWrite(SPI_REG_CTRL1, rnum-1);
		spi_master_rx_number[master_id] = rnum;

		// set TX fifo threshold
		if (wnum>SPI_TX_FIFO_SIZE)
			set_txfifo_threshold(master_id, SPI_TX_FIFO_SIZE/2);
		else
			set_txfifo_threshold(master_id, 0);

		// set RX fifo threshold
		if (rnum>SPI_RX_FIFO_SIZE)
			set_rxfifo_threshold(master_id, SPI_RX_FIFO_SIZE/2);
		else
			set_rxfifo_threshold(master_id, rnum);

		// enable SPI
		SPI_RegWrite(SPI_REG_SSIENR, 1);

		// write TX FIFO
		for (i=0; i<MIN(wnum,SPI_TX_FIFO_SIZE); i++)
			SPI_RegWrite(SPI_REG_DR, pwbuf[i]);

		// write the remaining TX frames into master TX/RX buffer
		if (wnum>i){
			spi_master_buffer[master_id][0] = wnum - i;
			j = wnum-i;
			do{ spi_master_buffer[master_id][j--] = pwbuf[i++];
			} while (i<wnum);
		} else spi_master_buffer[master_id][0] = 0;

		// initiate tx_done and rx_done flags
		spi_master_tx_done[master_id] = 0;
		spi_master_rx_done[master_id] = 0;

#ifndef POLLING_MODE // interrupt mode
		// if number of write frames is larger than TX fifo size,
		// unmask TX empty interrupt
		irqs = 0;
		if (wnum>SPI_TX_FIFO_SIZE)
			irqs |= IMR_MASK_TXE;
		// if number of read frames is larger than RX fifo size,
		// unmask RX full interrupt
		if (rnum>SPI_RX_FIFO_SIZE)
			irqs |= IMR_MASK_RXF;
		SPI_RegWrite(SPI_REG_IMR, irqs);
#endif

		// select slave device to start transfer
		select_slave_device(master_id, dev_id, 1);

#ifndef POLLING_MODE // interrupt mode
		// wait until RX finish
		if (irqs & IMR_MASK_RXF) { /* RX interrupt wait required */

			while (!spi_master_rx_done[master_id]);

		} else { /* no RX interrupt wait required */
		    do{
			    SPI_RegRead(SPI_REG_SR, &i);
		    } while (!(i&0x08)); /* RX fifo is empty */

			do {
				SPI_RegRead(SPI_REG_SR, &i);
			} while (i&0x01); /* RX transaction is in process */

			/* read data from RX fifo */
            SPI_RegRead(SPI_REG_RXFLR, &i);
            for (j=0; j<i; j++) {
		        SPI_RegRead(SPI_REG_DR, &(spi_master_buffer[master_id][j+1]));
			}
			spi_master_buffer[master_id][0] = i;
		}
#else // polling mode

        i = 0;
        while (i<rnum) {
		    do{
			    SPI_RegRead(SPI_REG_SR, &j);
		    } while (!(j&0x08));

		    SPI_RegRead(SPI_REG_SR, &j);
		    if( j&0x10 ) break;

            SPI_RegRead(SPI_REG_RXFLR, &j);
            while (j--){
		        SPI_RegRead(SPI_REG_DR, &(spi_master_buffer[master_id][i+1]));
                i ++;
            }
	    }
	    spi_master_buffer[master_id][0] = i;

#endif
		// copy out the received frames in buffer
		if (spi_master_buffer[master_id][0]!=rnum){
			// SPI I/O error!!!
			// de-select slave device
			select_slave_device(master_id, dev_id, 0);
			// disable SPI
			SPI_RegWrite(SPI_REG_SSIENR, 0);

			return (SSI_EBUSRW);
		}
		for (i=0; i<rnum; i++)
			prbuf[i] = spi_master_buffer[master_id][i+1];

	} else if (pwbuf && wnum>0){ // write-only transfer
		// set transfer mode to transmit-only
		set_transfer_mode(master_id, 1);

		// set TX fifo threshold
		if (wnum>SPI_TX_FIFO_SIZE)
			set_txfifo_threshold(master_id, SPI_TX_FIFO_SIZE/2);
		else
			set_txfifo_threshold(master_id, 0);

		// enable SPI
		SPI_RegWrite(SPI_REG_SSIENR, 1);

		// write TX FIFO
		for (i=0; i<MIN(wnum,SPI_TX_FIFO_SIZE); i++)
			SPI_RegWrite(SPI_REG_DR, pwbuf[i]);

		// write the remaining TX frames into master TX/RX buffer
		if (wnum>i){
			spi_master_buffer[master_id][0] = wnum - i;
			j = wnum-i;
			do{ spi_master_buffer[master_id][j--] = pwbuf[i++];
			} while (i<wnum);
		} else spi_master_buffer[master_id][0] = 0;

		// initiate tx_done flags
		spi_master_tx_done[master_id] = 0;

#ifndef POLLING_MODE // interrupt mode
		// if number of write frames is larger than TX fifo size,
		// unmask TX empty interrupt
		irqs = 0;
		if (wnum>SPI_TX_FIFO_SIZE)
			irqs |= IMR_MASK_TXE;
		SPI_RegWrite(SPI_REG_IMR, irqs);
#endif

		// select slave device to start transfer
		select_slave_device(master_id, dev_id, 1);

#ifndef POLLING_MODE // interrupt mode
		// wait until TX finish
		if (irqs & IMR_MASK_TXE) { /* TX interrupt wait required */

			while (!spi_master_tx_done[master_id]);

		} else { /* no TX interrupt wait required */

			/* wait until TX fifo is empty */
			do {
				SPI_RegRead(SPI_REG_SR, &j);
			} while (!(j&0x04)); /* TX fifo is not empty */

		}

		// wait until the shift register is flushed
		do {
			SPI_RegRead(SPI_REG_SR, &i);
		} while (i&0x01);

#else // polling mode
	    i = spi_master_buffer[master_id][0];
	    while (i>0) {
		    do {
			    SPI_RegRead(SPI_REG_SR, &j);
		    } while (!(j&0x02)); /* TX fifo is full */

		    SPI_RegRead(SPI_REG_SR, &j);
		    if( j&0x04 ) break; /* TX fifo is empty */

		    SPI_RegRead(SPI_REG_TXFLR, &j);
		    j = MIN(SPI_TX_FIFO_SIZE-j, i);
		    while (j-->0) {
		        SPI_RegWrite(SPI_REG_DR, spi_master_buffer[master_id][i--]);
		    }
	    }
	    spi_master_buffer[master_id][0] = i;

	    do {
		    SPI_RegRead(SPI_REG_SR, &j);
	    } while (!(j&0x04)); /* TX fifo is not empty */

	    do {
		    SPI_RegRead(SPI_REG_SR, &j);
	    } while (j&0x01); /* SSI is transfering */

		if (spi_master_buffer[master_id][0]!=0){
			// SPI I/O error!!!
			// de-select slave device
			select_slave_device(master_id, dev_id, 0);
			// disable SPI
			SPI_RegWrite(SPI_REG_SSIENR, 0);

			return (SSI_EBUSRW);
		}
#endif

	} else if (prbuf && rnum>0){ // read-only transfer
		// set transfer mode to receive-only
		set_transfer_mode(master_id, 2);

		// set number of read frames
		SPI_RegWrite(SPI_REG_CTRL1, rnum-1);
		spi_master_rx_number[master_id] = rnum;

		// set RX fifo threshold
		if (rnum>SPI_RX_FIFO_SIZE)
			set_rxfifo_threshold(master_id, SPI_RX_FIFO_SIZE/2);
		else
			set_rxfifo_threshold(master_id, rnum);

		// enable transfer
		SPI_RegWrite(SPI_REG_SSIENR, 1);

		// reset RX buffer
		spi_master_buffer[master_id][0] = 0;

		// initiate rx_done flags
		spi_master_rx_done[master_id] = 0;

		// write TX FIFO to start transfer
		SPI_RegWrite(SPI_REG_DR, 0);

#ifndef POLLING_MODE // interrupt mode
		// if number of read frames is larger than RX fifo size,
		// unmask RX full interrupt
		irqs = 0;
		if (rnum>SPI_RX_FIFO_SIZE)
			irqs |= IMR_MASK_RXF;
		SPI_RegWrite(SPI_REG_IMR, irqs);
#endif

		// select slave device to start transfer
		select_slave_device(master_id, dev_id, 1);

#ifndef POLLING_MODE // interrupt mode
		// wait until RX finish
		if (irqs & IMR_MASK_RXF) { /* RX interrupt wait required */

			while (!spi_master_rx_done[master_id]);

		} else { /* no RX interrupt wait required */
		    do{
			    SPI_RegRead(SPI_REG_SR, &i);
		    } while (!(i&0x08)); /* RX fifo is empty */

			do {
				SPI_RegRead(SPI_REG_SR, &i);
			} while (i&0x01); /* RX transaction is in process */

			/* read data from RX fifo */
            SPI_RegRead(SPI_REG_RXFLR, &i);
            for (j=0; j<i; j++) {
		        SPI_RegRead(SPI_REG_DR, &(spi_master_buffer[master_id][j+1]));
			}
			spi_master_buffer[master_id][0] = i;
		}
#else // polling mode
	    for (i=0; i<rnum; i++) {
		    do {
			    SPI_RegRead(SPI_REG_SR, &j);
		    } while (!(j&0x08));

		    SPI_RegRead(SPI_REG_SR, &j);
		    if( j&0x10 ) break;

		    SPI_RegRead(SPI_REG_DR, &(spi_master_buffer[master_id][i+1]));
	    }
	    spi_master_buffer[master_id][0] = i;
#endif

		// copy out the received frames in buffer
		if (spi_master_buffer[master_id][0]!=rnum){
			// SPI I/O error!!!
			// de-select slave device
			select_slave_device(master_id, dev_id, 0);
			// disable SPI
			SPI_RegWrite(SPI_REG_SSIENR, 0);

			return (SSI_EBUSRW);
		}
		for (i=0; i<rnum; i++)
			prbuf[i] = spi_master_buffer[master_id][i+1];
	}

	// de-select slave device
	select_slave_device(master_id, dev_id, 0);
	// disable SPI
	SPI_RegWrite(SPI_REG_SSIENR, 0);

	return (SSI_OK);
}


//////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////VCORE API/////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////

unsigned int	diag_spi_ADC_read(unsigned int channel,unsigned int mode, unsigned int master_id)
{

	unsigned int base_addr;
	int value;
	int i, j,rnum=3;
	unsigned int write_buf[3]=
	{
		0,0,0
	};
	unsigned int read_buf[3]=
	{
		0,0,0
	};

	if((channel&0x1)==0)
		channel>>=1;
	else
		channel=(channel>>1)+4;
	if(mode)
		write_buf[0]=0x87|((channel&0x7)<<4);
	else
		write_buf[0]=0x83|((channel&0x7)<<4);

//	write_buf[1]=(write_buf[0]&0x1)<<7;
//	write_buf[0]>>=1;

	base_addr = SPI_MASTER_BASEADDR[master_id];

	//set_ssi_protocol(master_id, 0);
	// check status first
	SPI_RegRead(SPI_REG_SR, &value);
	if (value&0x01) return (SSI_EBUSY);

	//set_transfer_mode(master_id, 0); //full-duplex mode
	diag_spi_master_set_clock_mode(master_id,3);
		// set number of read frames
	SPI_RegWrite(SPI_REG_CTRL1, rnum-1);
	spi_master_rx_number[master_id] = rnum;

		// set TX fifo threshold
	/*	if (wnum>SPI_TX_FIFO_SIZE)
			set_txfifo_threshold(master_id, SPI_TX_FIFO_SIZE/2);
		else*/
	set_txfifo_threshold(master_id, 0);

		// set RX fifo threshold
	if (rnum>SPI_RX_FIFO_SIZE)
		set_rxfifo_threshold(master_id, SPI_RX_FIFO_SIZE/2);
	else
		set_rxfifo_threshold(master_id, rnum);

		// enable SPI
	SPI_RegWrite(SPI_REG_SSIENR, 1);

		// write TX FIFO
	for (i=0; i<3; i++)
		SPI_RegWrite(SPI_REG_DR, write_buf[i]);

		// write the remaining TX frames into master TX/RX buffer
/*	if (wnum>i){
		spi_master_buffer[master_id][0] = wnum - i;
		j = wnum-i;
		do{ spi_master_buffer[master_id][j--] = pwbuf[i++];
		} while (i<wnum);
	} else*/
	spi_master_buffer[master_id][0] = 0;

		// initiate tx_done and rx_done flags
	spi_master_tx_done[master_id] = 0;
	spi_master_rx_done[master_id] = 0;


	// select slave device to start transfer
	select_slave_device(master_id, 0, 1);//slave id =0

    i = 0;
    while (i<rnum) {
		do{
			SPI_RegRead(SPI_REG_SR, &j);
		} while (!(j&0x0A));
		//} while (!(j&0x02));
	SPI_RegRead(SPI_REG_SR, &j);
	if( j&0x10 ) break;

            SPI_RegRead(SPI_REG_RXFLR, &j);
            while (j--){
		        SPI_RegRead(SPI_REG_DR, &(spi_master_buffer[master_id][i+1]));
                i ++;
            }
	}
	spi_master_buffer[master_id][0] = i;


	// copy out the received frames in buffer
	if (spi_master_buffer[master_id][0]!=rnum){
			// SPI I/O error!!!
			// de-select slave device
	select_slave_device(master_id, 0, 0);//slave id =0
			// disable SPI
	SPI_RegWrite(SPI_REG_SSIENR, 0);

	return (SSI_EBUSRW);
	}
	for (i=0; i<rnum; i++)
	read_buf[i] = spi_master_buffer[master_id][i+1];


	// de-select slave device
	select_slave_device(master_id, 2, 0);
	// disable SPI
	SPI_RegWrite(SPI_REG_SSIENR, 0);

	value=((read_buf[1]&0x7F)<<5)|((read_buf[2]&0xF8)>>3);
	dbg_printf("12 bit ADC read back value is 0x%x (%d) = %f V\n",value,value,(float)value/4095*2.048);

	gpioSet(5,0x10,0x10);
	//pinmux_write(1,0,1);
	diag_spi_master_set_clock_mode(master_id,0);
	return (value);
}


int read_adc( int chan_num)
{
#if	PEK_NOT_DONGLE == 1

	int data2;
	unsigned char wbuf1[2]=
	{
		0x03,0x70
	};
	unsigned char wbuf2[2]=
	{
		0x01,0x86
	};

	gpioSet(0, 0, 0); // GPIO 7 -> 0
	gpioSet(1,0x200,0x200); // GPIO 41 ->1
	//pinmux_write(9,3);
	// Select MUX to ADC SPI Chip Select
	//diag_i2c_master_init(0);
	//diag_i2c_master_writeread_bytes( 0, 0x38, wbuf1, 2, 0, 0);
	//diag_i2c_master_writeread_bytes( 0, 0x38, wbuf2, 2, 0, 0);

	// Init SPI and Speed
	diag_spi_master_init(0);
	diag_spi_master_set_speed(0,78);
    data2 = diag_spi_ADC_read(chan_num,1,0);

    dbg_printf("Read channel #%d : 0x%04x\n", chan_num, data2);

	//change I2C expander to default
    //wbuf1[0]=0x03;
//	wbuf1[1]=0xF8;

    //wbuf2[0]=0x01;
//	wbuf2[1]=0x06;


	//pinmux_write(9,3);
	// Select MUX to ADC SPI Chip Select
	//diag_i2c_master_init(0);
//	diag_i2c_master_writeread_bytes( 0, 0x38, wbuf1, 2, 0, 0);
	//diag_i2c_master_writeread_bytes( 0, 0x38, wbuf2, 2, 0, 0);

//	reset_perif();
    return data2;
#else
	(void)chan_num;
	dbg_printf("Only valid on PEK boards\n");
	return 0;
#endif

}



void reset_perif(void)
{

	unsigned int i = 0;
	BFM_HOST_Bus_Write32(MEMMAP_CHIP_CTRL_REG_BASE+RA_Gbl_ResetTrigger,MSK32Gbl_ResetTrigger_perifSyncReset);
	for(i = 0; i< 0x1000; i++);
	BFM_HOST_Bus_Write32(MEMMAP_CHIP_CTRL_REG_BASE+RA_Gbl_ResetTrigger,0);
	dbg_printf("Perif is reset\n");

}

int vcore_set_voltage(int code)
{
#if	PEK_NOT_DONGLE == 1
	unsigned char wbuf1[2]=
	{
		0x03,0x70
	};
	unsigned char wbuf2[2]=
	{
		0x01,
		0x0E,
	};

	int spi_wbuf[3];

	gpioSet(0,0x80,0x80); // GPIO SET 7 -> 1
	gpioSet(1,0x400,0x400); // GPIO SET 42,41 -> (1,0)
	spi_wbuf[0] = 0xb0;
	spi_wbuf[1] = (code>>8) & 0x0ff;
	spi_wbuf[2] = code & 0x0ff;
//	pinmux_write(9,3);
		// Select MUX to PMIC SPI Chip Select
//	diag_i2c_master_init(0);
//	diag_i2c_master_writeread_bytes( 0, 0x38, wbuf1, 2, 0, 0);
//	diag_i2c_master_writeread_bytes( 0, 0x38, wbuf2, 2, 0, 0);
//	pinmux_write(8,0);
//	pinmux_write(9,1);
//	pinmux_write(10,0);
//	pinmux_write(11,0);
	diag_spi_master_init(0);
	diag_spi_master_set_speed(0,1000);
	diag_spi_master_writeread_frames(0,0,spi_wbuf,3,0,0,0);

	//change I2C expander to default
  //  wbuf1[0]=0x03;
	//wbuf1[1]=0xF8;

    //wbuf2[0]=0x01;
	//wbuf2[1]=0x06;


	//pinmux_write(9,3);
	// Select MUX to ADC SPI Chip Select
	//diag_i2c_master_init(0);
	//diag_i2c_master_writeread_bytes( 0, 0x38, wbuf1, 2, 0, 0);
	//diag_i2c_master_writeread_bytes( 0, 0x38, wbuf2, 2, 0, 0);

	//reset_perif();
	return 0;
#else
	(void)code;
	dbg_printf("Only valid on PEK boards\n");
	return 0;
#endif
}

#define DELAY_VCORE 0x45000//400MHz CPU, cache is not enabled

int vcore_set(int mv, int step10, int ref1050)
{
#if	PEK_NOT_DONGLE == 1
	int cur_v;
	int i;
	int last_step;

	mv = mv*2;
	// Pot case
	// Step are 8 per 10mv
	// 1115 mv = 0x2E0
	step10 = 0x08;
	ref1050 = 0x2E0;

	vcore_set_voltage( ref1050 );
	for(i=0;i<DELAY_VCORE;i++);
	cur_v = read_adc(7);
	dbg_printf("Starting voltage is %d mv\n", cur_v/2);
	if ((cur_v>(1125*2))||(cur_v<(1105*2)))
	{
		dbg_printf("Out of bound starting voltage!\n");
		reset_perif();
		return 1;
	}

	if (cur_v > mv) // current voltage is higher than target, go down by steps.
	{
		while ((cur_v-mv)>20)
		{
			ref1050 += step10;
			vcore_set_voltage( ref1050 );
			for(i=0;i<DELAY_VCORE;i++);
			cur_v = read_adc(7);
		}
		last_step = ((cur_v-mv)*step10)/20;

		ref1050 += last_step;
		vcore_set_voltage( ref1050 );
		for(i=0;i<DELAY_VCORE;i++);
		cur_v = read_adc(7);
	}
	else  // current voltage is lower than target. go up by steps.
	{
		while ((mv-cur_v)>20)
		{
			ref1050 -= step10;
			vcore_set_voltage( ref1050 );
			for(i=0;i<DELAY_VCORE;i++);
			cur_v = read_adc(7);
		}
		last_step = ((mv-cur_v)*step10)/20;

		ref1050 -= last_step;
		vcore_set_voltage( ref1050 );
		for(i=0;i<DELAY_VCORE;i++);
		cur_v = read_adc(7);

	}
	dbg_printf("Rough voltage is %d mv\n", cur_v/2);

	last_step = ((mv-cur_v)*step10)/20;

	ref1050 -= last_step;
	vcore_set_voltage( ref1050 );
	for(i=0;i<DELAY_VCORE;i++);
	cur_v = read_adc(7);

	dbg_printf("Fine voltage is %d mv\n", cur_v/2);

	if ((cur_v>(mv+10))||(cur_v<(mv-10)))
	{
		dbg_printf("Ending Voltage out of bound!\n");
		return 1;
	}
	reset_perif();
	dbg_printf("\nPerif has been reset.SPI handler is reset\nFor DIAG boot Please use:\n");
	dbg_printf(" Turnon DDR power 'gpiowrite 2 0x40 0x40'\n Use 'mempll [frequency]'\nThen 'bootdiag'\n");
	return 0;

#else
	(void)ref1050;
	(void)mv;
	(void)step10;
	dbg_printf("Only valid on PEK boards\n");
	return 0;
#endif

}



// VIndex: [0-0.90,1-0.925,2-0.95(default),3-0.975,4-1.00,5-1.025,6-1.050, 14-1.25V]
int diag_i2c_vcore_control(int master_id, int volt_index)
{
#if	1 //PMIC_I2C_INTERFACE
	static int FPWM_mode = 0;
	unsigned char buff[2];
	unsigned char read = 0xff;
	unsigned char slaveAddr = 0x19; // PG868 slave address is 0x19
	int ret;

	if(FPWM_mode == 0)
	{
		buff[0] = 0x02;  // PG867 buck2 target voltage 2 (active) register

		diag_i2c_master_init(master_id);
		diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);
		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
		if (ret != 0)
		{
			dbg_printf(" i2c read fail\n");
			return ret;
		}

		buff[1] = (read&0xFC)| 0x3;

		// write
		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 2, 0, 0);
		if (ret != 0)
		{
			dbg_printf(" i2c write fail\n");
			return ret;
		}

		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
		if (ret != 0)
		{
			dbg_printf(" i2c read fail\n");
			return ret;
		}

		if (buff[1] != read)
		{
			dbg_printf(" PWM mode control fail, read:0x%02x != write:0x%02x\n", read, buff[1]);
			return 1;
		}
		dbg_printf(" PWM mode switch success!\n");

		FPWM_mode = 1;
	}



	buff[0] = 0x24;  // PG868 buck1 target voltage 2 (active) register
	buff[1] = 0x7 + volt_index; // 0x7 is 0.90v. each step is 0.025
	ret = diag_i2c_master_init(master_id);
	ret = diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);

	// write
	ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 2, 0, 0);
	if (ret != 0)
	{
		dbg_printf( " i2c write fail\n");
		return ret;
	}

	ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
	if (ret != 0)
	{
		dbg_printf( " i2c read fail\n");
		return ret;
	}
	if (buff[1] != read)
	{
		dbg_printf(  " i2c vcore control fail, read:0x%02x != write:0x%02x\n", read, buff[1]);
		return 1;
	}
	dbg_printf(  " vcore is %04d mv\n", 900 +25*volt_index);

	return 0;

#else

	return 0;

#endif
}

// VIndex: [0-1.20,1:1.225,2:1.25,3:1.275,4:1.3,5-1.325,6:1.350(default),,, 12:1.5v]
int diag_i2c_vddr_control(int master_id, int volt_index)
{
#if	1 //PMIC_I2C_INTERFACE
	static int FPWM_mode = 0;
	unsigned char buff[2];
	unsigned char read = 0xff;
	unsigned char slaveAddr = 0x19; // PG868 slave address is 0x19
	int ret;
	if(FPWM_mode == 0)
	{
		buff[0] = 0x02;  // PG867 buck2 target voltage 2 (active) register

		diag_i2c_master_init(master_id);
		diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);
		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
		if (ret != 0)
		{
			dbg_printf(" i2c read fail\n");
			return ret;
		}

		buff[1] = (read&0xFC)| 0x3;

		// write
		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 2, 0, 0);
		if (ret != 0)
		{
			dbg_printf(" i2c write fail\n");
			return ret;
		}

		ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
		if (ret != 0)
		{
			dbg_printf(" i2c read fail\n");
			return ret;
		}

		if (buff[1] != read)
		{
			dbg_printf(" PWM mode control fail, read:0x%02x != write:0x%02x\n", read, buff[1]);
			return 1;
		}
		dbg_printf(" PWM mode switch success!\n");

		FPWM_mode = 1;
	}

	buff[0] = 0x13;  // PG867 buck2 target voltage 2 (active) register
	buff[1] = 0x18 + volt_index; //0x18 is 1.20v. each step is 0.025
	diag_i2c_master_init(master_id);
	diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);
	// write
	ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 2, 0, 0);
	if (ret != 0)
	{
		dbg_printf(" i2c write fail\n");
		return ret;
	}

	ret = diag_i2c_master_writeread_bytes(master_id, slaveAddr, buff, 1, &read, 1);
	if (ret != 0)
	{
		dbg_printf(" i2c read fail\n");
		return ret;
	}

	if (buff[1] != read)
	{
		dbg_printf( " i2c vddr control fail, read:0x%02x != write:0x%02x\n", read, buff[1]);
		return 1;
	}
	dbg_printf( " [I2C control]vddr is %04d mv\n", 1200+25*volt_index);

	return 0;
#else
	return 0;
#endif
}

static int diag_i2c_check_regulator(int master_id, unsigned char addr, unsigned char command, unsigned char *val)
{
	unsigned char buff = command;
	unsigned char read = 0xFF;
	int ret;

	ret = diag_i2c_master_init(master_id);
	if (ret == I2C_ERROR) {
		dbg_printf(" failed to init master\n");
		return ret;
	}

	ret = diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);
	if (ret == I2C_ERROR) {
		dbg_printf(" failed to set addr type\n");
		return ret;
	}

	ret = diag_i2c_master_writeread_bytes(master_id, addr, &buff, 1, &read, 1);
	if (ret == I2C_ERROR) {
		dbg_printf(" i2c read fail\n");
		return ret;
	}

	if (val) {
		*val = read;
	}

	return 0;
}

int sy21202_val = 0;
int diag_i2c_check_sy21202(int master_id)
{
	int ret;
	unsigned char val = 0xFF;
	unsigned char voltage;
	const unsigned char addr = 0x66;
	const unsigned char command = 0x00;

	ret = diag_i2c_check_regulator(master_id, addr, command, &val);
	if (ret != 0) {
		dbg_printf("diag_i2c_check_sy21202 ret: %d\n", ret);
		return 1;
	}
	dbg_printf("diag_i2c_check_sy21202 val: %d\n", val);

	sy21202_val = val;
	// validate BUCK_EN and default MODE
	if ((val & (1 << 7)) && !(val & (1 << 6))) {
		// Check voltage is within the expected range
		// Range from kernel/arch/arm/march-berlin/opp.c
		// 0x15 -> 1025 mV to 0x23 -> 1200 mV
		voltage = (val & 0x3F);
		if (voltage >= 0x15 && voltage <= 0x23) {
			return 0;
		}
	}

	return 1;
}

int pg86x_val = 0;
int diag_i2c_check_pg86x(int master_id)
{
	int ret;
	unsigned char val = 0xFF;
	const unsigned char addr = 0x19;
	const unsigned char command = 0x24;

	ret = diag_i2c_check_regulator(master_id, addr, command, &val);
	if (ret != 0) {
		dbg_printf("diag_i2c_check_pg86x ret: %d\n", ret);
		return 1;
	}
	dbg_printf("diag_i2c_check_pg86x val: %d\n", val);

	pg86x_val = val;

	// Validate that the voltage is within the expected range
	// Range from kernel/arch/arm/march-berlin/opp.c
	// 0x0C -> 1025 mV to 0x13 -> 1200 mV
	if (pg86x_val >= 0x0C && pg86x_val <= 0x13) {
		return 0;
	}

	return 1;
}

// Initializes the PCA9956 LED driver at I2C address |slave_addr|.
int diag_i2c_led_init(int master_id, unsigned char slave_addr, enum led_chip_model_t model) {
	unsigned char buff[2];
	int ret;

	diag_i2c_master_init(master_id);
	diag_i2c_master_set_addr_type(I2C_7BIT_SLAVE_ADDR);

	/* Initialize NXP LED driver */
	/* Set all IREF values */
	buff[0] = kLedInfoMap[model][PCA995X_IREFALL_INDEX];
	buff[1] = 0xFF;
	ret = diag_i2c_master_writeread_bytes(master_id, slave_addr, buff, 2, 0, 0);
	if (ret != 0) {
		dbg_printf("%s: failed to set all IREF values.\n", __func__);
		return ret;
	}

	/* Set Mode1 register to enable I2C auto-increment for MODE1 to
	 * IREF23 control registers (00h to 39h).
	 */
	buff[0] = PCA995X_MODE_REGISTER_1;
	buff[1] = 0x40;
	ret = diag_i2c_master_writeread_bytes(master_id, slave_addr, buff, 2, 0, 0);
	if (ret != 0) {
		dbg_printf("%s: failed to set Mode 1 registers.\n", __func__);
		return ret;
	}

	size_t ledout_count = kLedInfoMap[model][PCA995X_LEDOUT_COUNT_INDEX];
	unsigned char val[ledout_count + 3];
	size_t i;

	val[0] = PCA995X_MODE_REGISTER_2 | PCA995X_AUTO_INCREMENT_ADDRESS_MASK;
	val[1] = 0x05;
	for (i = 2; i < ledout_count+2; ++i) {
		val[i] = 0xFF;
	}
	val[ledout_count+2] = 0x08;
	ret = diag_i2c_master_writeread_bytes(master_id, slave_addr, val, sizeof(val),
	                                      0, 0);
	if (ret < 0) {
		dbg_printf("%s: failed on initilization LED.", __func__);
		return ret;
	}

	return 0;
}

// Sets the first |size| LEDs of PCA9956 with address |slave_addr| to |frame|.
int diag_i2c_led_set_frame(int master_id, unsigned char slave_addr,
                           unsigned char *frame, size_t size, enum led_chip_model_t model) {
	int ret;
	size_t max_num_leds = kLedInfoMap[model][PCA995X_MAX_NUM_LEDS_INDEX];
	if (size > max_num_leds) {
		dbg_printf("%s: Too many LEDs specified: %d", __func__, size);
		size = max_num_leds;
	}

	// Build buffer for writeread_bytes
	unsigned char val[max_num_leds + 1];
	val[0] = kLedInfoMap[model][PCA995X_PWM_BASE_INDEX] | PCA995X_AUTO_INCREMENT_ADDRESS_MASK;
	size_t i;
	for (i = 0; i < size; ++i) {
		val[i + 1] = frame[i];
	}
	for (i = size; i < max_num_leds; ++i) {
		val[i + 1] = 0;
	}

	ret = diag_i2c_master_writeread_bytes(master_id, slave_addr, val, sizeof(val),
	                                      0, 0);
	if (ret < 0) {
		dbg_printf("%s: failed to set LEDs.", __func__);
		return ret;
	}

	return 0;
}

////////////////////////////////////////////////////////////
/*
#define OTP_BindInfo1H		(MEMMAP_TSI_REG_BASE + 0x7DC)
void parsePVCOMPCmd( unsigned char Index )
{
    unsigned int data,Leakage;
	unsigned int lkdID[9]={196, 248, 296, 348, 396, 448, 496, 548, 596};
    unsigned int *ptr = &lkdID;

    //Chip Leakage INFO
    BFM_HOST_Bus_Read32(OTP_BindInfo1H, &data);
    Leakage = ((data>>8)&0xFF) * 4;   //  4mv/step in BG2_CD project.
    dbg_printf(" The Chipset leakage is %d(mA).\n",Leakage);


   if (Index == 2)
   {
		//diag_i2c_pwm_mode(0,0,1);
       if (Leakage == 0)
           {
            diag_i2c_vcore_control(0, 8);  // 1.1v.
            dbg_printf(" Warning message: Leakage ID is zero! Set vcore to 1.1v. \n");
           }

       else
           {
			  if (0 < Leakage && Leakage <= *ptr)
			         ,   (0, 10);  //1.150v.
			  else if (*ptr < Leakage && Leakage <= *(ptr+1))
			      diag_i2c_vcore_control(0, 9);  // 1.125v.
			  else if (*(ptr+1) < Leakage && Leakage <= *(ptr+2))
			      diag_i2c_vcore_control(0, 8);  // 1.100v.
			  else if (*(ptr+2) < Leakage && Leakage <= *(ptr+3))
			      diag_i2c_vcore_control(0, 7);  // 1.075v.
			  else if (*(ptr+3) < Leakage && Leakage <= *(ptr+4))
			      diag_i2c_vcore_control(0, 6);  // 1.050v.
			  else if (*(ptr+4) < Leakage && Leakage <= *(ptr+5))
			      diag_i2c_vcore_control(0, 5);  // 1.025v.
			  else if (*(ptr+5) < Leakage && Leakage <= *(ptr+6))
			      diag_i2c_vcore_control(0, 4);  // 1.000v.
			  else if (*(ptr+6) < Leakage && Leakage <= *(ptr+7))
			      diag_i2c_vcore_control(0, 3);  // 0.975v.
			  else if (*(ptr+7) < Leakage && Leakage <= *(ptr+8))
			      diag_i2c_vcore_control(0, 2);  // 0.950v.
			  else
			      dbg_printf(" Warning message: Leakage ID is out of range: (0-596mA), Keep harware default vcore.\n");
           }
   }
   else
   {
            dbg_printf(" PEK boards with PVCOMP table -3%\n");
	    if (Leakage == 0)
		{
            vcore_set(1100, 0x08, 0x2E0);  // 1.1v.
            dbg_printf(" Warning message: Leakage ID is zero! Set vcore to 1.1v. \n");
		}

		else
		{
			if (0 < Leakage && Leakage <= *ptr)
				vcore_set(1115, 0x08, 0x2E0);    //1.150v.
			else if (*ptr < Leakage && Leakage <= *(ptr+1))
				vcore_set(1091, 0x08, 0x2E0);  // 1.125v.
			else if (*(ptr+1) < Leakage && Leakage <= *(ptr+2))
				vcore_set(1067, 0x08, 0x2E0);   // 1.100v.
			else if (*(ptr+2) < Leakage && Leakage <= *(ptr+3))
				vcore_set(1042, 0x08, 0x2E0);   // 1.075v.
			else if (*(ptr+3) < Leakage && Leakage <= *(ptr+4))
				vcore_set(1018, 0x08, 0x2E0);   // 1.050v.
			else if (*(ptr+4) < Leakage && Leakage <= *(ptr+5))
				vcore_set(994, 0x08, 0x2E0);   // 1.025v.
			else if (*(ptr+5) < Leakage && Leakage <= *(ptr+6))
				vcore_set(970, 0x08, 0x2E0);   // 1.000v.
			else if (*(ptr+6) < Leakage && Leakage <= *(ptr+7))
				vcore_set(945, 0x08, 0x2E0);   // 0.975v.
			else if (*(ptr+7) < Leakage && Leakage <= *(ptr+8))
				vcore_set(921, 0x08, 0x2E0);   // 0.950v.
			else
			      dbg_printf(" Warning message: Leakage ID is out of range: (0-596mA), Keep harware default vcore.\n");
          }
   }
}

*/

#if 0
#define LA_TRIGGER_ENABLE 0

/**********************************************************************
 *
 * Filename:    memtest.c
 *
 * Description: General-purpose memory testing functions.
 *
 * Notes:       This software can be easily ported to systems with
 *              different data bus widths by redefining 'datum'.
 *
 *
 * Copyright (c) 1998 by Michael Barr.  This software is placed into
 * the public domain and may be used for any purpose.  However, this
 * notice must not be changed or removed and no warranty is either
 * expressed or implied by its publication or distribution.
 **********************************************************************/

#define PATTERN_STARTING_VALUE 9
#define PATTERN_INCREMENT 7
#define DRAM_START_ADDR 0
#define AT_SUCCESS		0
#define UINT32          unsigned int
#define UINT8			unsigned char
/**********************************************************************
 *
 * Function:    memTestDevice()
 *
 * Description: Test the integrity of a physical memory device by
 *              performing an increment/decrement test over the
 *              entire region.  In the process every storage bit
 *              in the device is tested as a zero and a one.  The
 *              base address and the size of the region are
 *              selected by the caller.
 *
 * Notes:
 *
 * Returns:     NULL if the test succeeds.
 *
 *              A non-zero result is the first address at which an
 *              incorrect value was read back.  By examining the
 *              contents of memory, it may be possible to gather
 *              additional information about the problem.
 *
 **********************************************************************/
#if 0

int memTestDevice(volatile datum * baseAddress, unsigned long nBytes)
{
    unsigned long offset;
    unsigned long nWords = nBytes / sizeof(datum);

    datum pattern;
    datum antipattern;

	hexout("          :  mtd ", (void*) baseAddress, "");
	hexout(" - ", (void*) &baseAddress[nWords], "\r");

    /*
     * Fill memory with a known pattern.
     */
    for (pattern = initial_pattern, offset = 0;
	 offset < nWords; pattern++, offset++) {
	    if (0 == offset % (1024*128)) {
		    hexout(0, (void*) &baseAddress[offset], "\r");
	    }
	    baseAddress[offset] = pattern;
    }

    /*
     * Check each location and invert it for the second pass.
     */
    for (pattern = initial_pattern, offset = 0;
	 offset < nWords; pattern++, offset++) {
	    if (0 == offset % (1024*128)) {
		    hexout(0, (void*) &baseAddress[offset], "\r");
	    }
	    if (baseAddress[offset] != pattern) {
			failure(">0 ",
				(void*) &baseAddress[offset],
				(void*)  baseAddress[offset],
				(void*) pattern);

            return(1);
	    }

	    antipattern = ~pattern;
	    baseAddress[offset] = antipattern;
    }

    /*
     * Check each location for the inverted pattern and do not zero it,
     * despite what the previous comment said.
     */
    for (pattern = initial_pattern, offset = 0;
	 offset < nWords; pattern++, offset++) {
	    if (0 == offset % (1024*128)) {
		    hexout(0, (void*) &baseAddress[offset], "\r");
	    }
	    antipattern = ~pattern;
	    if (baseAddress[offset] != antipattern) {
		    failure(">1 ",
			    (void *)&baseAddress[offset],
			    (void *) baseAddress[offset],
			    (void *) ~pattern);
            return(1);
	    }
    }
    return(0);
}

#endif

#if LA_TRIGGER_ENABLE
#define LA_TRIGGER	  BFM_HOST_Bus_Write32(0x1800E000, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E004, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E008, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E00c, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E010, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E014, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E018, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800E01c, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F000, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F004, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F008, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F00c, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F010, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F014, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F018, 0xabcdabcd); \
					  BFM_HOST_Bus_Write32(0x1800F01c, 0xabcdabcd);
#else
#define LA_TRIGGER
#endif




/**********************************************************************
 *
 * Function:    memTestDataMask()
 *
 * Description: Test the data masks by writing a byte pattern and
 *		reading it back.  See pages 35-37 of Beth Leonard's
 *		November 2 Limbo notebook for more details.
 *
 * Notes:       StartAddress must be divisible by 8.
 * Author:	Written by Beth Leonard, November 20, 2000.
 *
 * Returns:     0 if the test succeeds.
 *              A non-zero result is the first bit that failed, 1-8.
 *
 **********************************************************************/

int memTestDataMask(volatile UINT32 * startAddress, UINT32 totalBytes, register UINT32 emptyPatttern, UINT8 inc)
{
	char pattern[4];
	char antipattern[4];
	UINT32 i = 0;
	volatile char * byteAddress = (char *) startAddress;

	pattern [3] = 0xEE;	antipattern[3] = 0x11;
	pattern [2] = 0xEE;	antipattern[2] = 0x11;
	pattern [1] = 0xAD;	antipattern[1] = 0x52;
	pattern [0] = 0xDE;	antipattern[0] = 0x21;

     //minPrintf("\r\nmemTestDataMask: ");
	/* Check bottom half for DM stuck at 1 */
	for( i=0; i< 4; i++)
	{
		byteAddress[i] = pattern[i];
		if(byteAddress[i] != pattern[i])
        {
            //reportPatternFailure(((UINT32)&startAddress[i]), byteAddress[i], pattern[i], true);
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddress+((unsigned int)i)), byteAddress[i],pattern[i]);

			return 1;
		}
		byteAddress[i] = antipattern[i];
		if(byteAddress[i] != antipattern[i])
		{
              //reportPatternFailure(((UINT32)&startAddress[i]), byteAddress[i], pattern[i], true);
            dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddress+((unsigned int)i)), byteAddress[i],antipattern[i]);

			return 1;
		}
	}

	/* Check top half for stuck at 1, bottom half for stuck at 0 */
	for(i=0; i<4; i++)
	{
		byteAddress[i+4] = pattern [i];
		if( byteAddress[i] != antipattern[i] )
		{
               //reportPatternFailure(((UINT32)&startAddress[i]), byteAddress[i], antipattern[i], true);
            dbg_printf( " Fail_c: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddress+((unsigned int)i)), byteAddress[i],antipattern[i]);

			return 1;
		}
		if( byteAddress[i+4] != pattern[i] )
		{
            //reportPatternFailure(((UINT32)&startAddress[i+4]), byteAddress[i+4], pattern[i], true);
			return 1;
		}
		byteAddress[i+4] = antipattern[i];
		if(byteAddress[i+4] != antipattern[i] )
		{
            dbg_printf( " Fail_d: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddress+((unsigned int)i)+4), byteAddress[i+4],antipattern[i]);

            //reportPatternFailure(((UINT32)&startAddress[i+4]), byteAddress[i+4], antipattern[i], true);
			return 1;
		}
	}

	/* Check top half for stuck at 0. */
	for(i=0; i<4; i++)
	{
		byteAddress[i] = pattern[i];
		if(byteAddress[i+4] != antipattern[i])
		{
            dbg_printf( " Fail_e: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddress+((unsigned int)i)+4), byteAddress[i+4],antipattern[i]);

            //reportPatternFailure(((UINT32)&startAddress[i+4]), byteAddress[i+4], antipattern[i], true);
			return 1;
		}
	}

    //minPrintf(TEXT_HW_MEM_TEST,"DataMask");
	return AT_SUCCESS;
}

/**********************************************************************
 *
 * Function:    memTestDataBus()
 *
 * Description: Test the data bus wiring in a memory region by
 *              performing a walking 1's test at a fixed address
 *              within that region.  The address (and hence the
 *              memory region) is selected by the caller.
 *
 * Notes:
 *
 * Returns:     0 if the test succeeds.
 *              A non-zero result is the first pattern that failed.
 *
 **********************************************************************/
int memTestDataBus(volatile UINT32 * address, UINT32 totalBytes, register UINT32 emptyPatttern, UINT8 inc)
{
    UINT32 pattern;

    //minPrintf("\r\nmemTestDataBus: ");
    /*
     * Perform a walking 1's test at the given address.
     */

    for (pattern = 1; pattern != 0; pattern <<= 1)
    {
        /*
         * Write the test pattern to both halves of the 64 bit word.
         */
        address[0] = pattern;
        address[1] = pattern;

        /*
         * Read it back (immediately is okay for this test).
         */
        if (address[0] != pattern)
        {
               dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)address), address[0],pattern);

            //reportPatternFailure((UINT32)&address[0], address[0], pattern, false);
            return (1);
        }
        if (address[1] != pattern)
        {
              dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)address+4), address[1],pattern);

            //reportPatternFailure((UINT32)&address[1], address[1], pattern, false);
            return (1);
        }
    }

    //minPrintf(TEXT_HW_MEM_TEST,"DataBus");
    return (AT_SUCCESS);

}   /* memTestDataBus() */


/**********************************************************************
 *
 * Function:    memTestAddressBus()
 *
 * Description: Test the address bus wiring in a memory region by
 *              performing a walking 1's test on the relevant bits
 *              of the address and checking for aliasing. This test
 *              will find single-bit address failures such as stuck
 *              -high, stuck-low, and shorted pins.  The base address
 *              and size of the region are selected by the caller.
 *
 * Notes:       For best results, the selected base address should
 *              have enough LSB 0's to guarantee single address bit
 *              changes.  For example, to test a 64-Kbyte region,
 *              select a base address on a 64-Kbyte boundary.  Also,
 *              select the region size as a power-of-two--if at all
 *              possible.
 *
 * Returns:     NULL if the test succeeds.
 *              A non-zero result is the first address at which an
 *              aliasing problem was uncovered.  By examining the
 *              contents of memory, it may be possible to gather
 *              additional information about the problem.
 *
 * Refer http://www.netrino.com/Articles/MemoryTesting/index.html
 *
 **********************************************************************/
int memTestAddressBus(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{

    //SW  -fixes a bug in the addressMask value
    // Refer http://www.netrino.com/Articles/MemoryTesting/index.html
    // Datum size is assumed to be 4 bytes for our case
    unsigned long addressMask = (nBytes >> 2 ) -1;
    //SW
    unsigned long offset;
    unsigned long testOffset;

    UINT32 pattern     = 0xAAAAAAAA;
    UINT32 antipattern = 0x55555555;


    //minPrintf("\r\nmemTestAddressBus: ");
    /*
     * Write the default pattern at each of the power-of-two offsets.
     */
    for (offset = sizeof(UINT32); (offset & addressMask) != 0; offset <<= 1)
    {
        baseAddress[offset] = pattern;
    }

    /*
     * Check for address bits stuck high.
     */
    testOffset = 0;
    baseAddress[testOffset] = antipattern;


    for (offset = sizeof(UINT32); (offset & addressMask) != 0; offset <<= 1)
    {
        if (baseAddress[offset] != pattern)
        {
            /*
            failure(">0 ",
                 (void*) &baseAddress[offset],
                 (void*)  baseAddress[offset],
                 (void*) pattern);
            */
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

              //reportPatternFailure((UINT32)&baseAddress[offset], baseAddress[offset], pattern, false);
              return 1;
        }
    }


    baseAddress[testOffset] = pattern;

    /*
     * Check for address bits stuck low or shorted.
     */
    for (testOffset = sizeof(UINT32); (testOffset & addressMask) != 0;
         testOffset <<= 1)
    {
        baseAddress[testOffset] = antipattern;

        for (offset = sizeof(UINT32); (offset & addressMask) != 0; offset <<= 1)
        {
            if ((baseAddress[offset] != pattern) && (offset != testOffset))
            {
                /*
                failure(">0 ",
                     (void*) &baseAddress[offset],
                     (void*)  baseAddress[offset],
                     (void*) pattern);
                */
                dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

                //reportPatternFailure((UINT32)&baseAddress[offset], baseAddress[offset], pattern, false);
                return 1;
            }
        }

        baseAddress[testOffset] = pattern;
    }


    //minPrintf(TEXT_HW_MEM_TEST,"AddressBus");
    return (AT_SUCCESS);

}   /* memTestAddressBus() */
/**********************************************************************
 *
 * Function:    memTestIncPA()
 *
 * Description: Increment Address and Pattern by a word each loop.
 *
 **********************************************************************/
int  memTestIncPA(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   register UINT32 dataRead;
   register UINT32 pattern;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestIncPA: ");
   // Fill memory with a known pattern.
   for(pattern = 1, offset = 0; offset < nLongWords; pattern++, offset+=inc)
   {
       baseAddress[offset] = pattern;
   }


   // Check each location
   for(pattern = 1, offset = 0; offset < nLongWords; pattern++, offset+=inc)
   {
       dataRead = baseAddress[offset];

       if(dataRead != pattern)
       {
           LA_TRIGGER
           dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);
           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
           return(1);
       }
       baseAddress[offset] = ~pattern;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"IncPA");
   return(AT_SUCCESS);
}

/**********************************************************************
 *
 * Function:    memTestIncBytePA()
 *
 * Description: Increment Address and Pattern by a word each loop
 *              All 4 bytes are incremented by one each loop
 *              after a lower byte incs to 0xff, its upper byte will
 *              be increased by 2
 *
 **********************************************************************/
int  memTestIncBytePA(volatile UINT32 * baseAddress, UINT32 nBytes, int inc)
{
   register UINT32 dataRead;
   register UINT32 pattern;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestIncPA: ");
   // Fill memory with a known pattern.
   for(pattern = 0x004080c0, offset = 0; offset < nLongWords; pattern+=0x01010101, offset+=inc)
   {
       baseAddress[offset] = pattern;
   }


   // Check each location
   for(pattern = 0x004080c0, offset = 0; offset < nLongWords; pattern+=0x01010101, offset+=inc)
   {
       dataRead = baseAddress[offset];

       if(dataRead != pattern)
       {
           LA_TRIGGER
           dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X, CAS 0X%x \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern, (offset&0x3ff));

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
           return(1);
       }
       //baseAddress[offset] = ~pattern;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"IncPA");
   return(AT_SUCCESS);
}

int memTestReadCheck(volatile char * baseAddress, UINT32 nBytes, int inc, int count)
{
   register char dataRead;
   register char pattern;
   UINT32 offset;

   offset=0;

   for(;;)
   {
          offset += inc;
          if(offset>=nBytes) offset=0;

          dataRead = baseAddress[offset];

   }

   return 0;

}

//Write one dword and read back
int  memTestIncByteWriteRead(volatile UINT32 * baseAddress, UINT32 nBytes, int inc)
{
   register UINT32 dataRead;
   register UINT32 pattern;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestIncPA: ");
   // Fill memory with a known pattern.
   for(pattern = 0x004080c0, offset = 0; offset < nLongWords; pattern+=0x01010101, offset+=32)
   {
       baseAddress[offset] = pattern;

       //if(!(offset % 16))
       {

           dataRead = baseAddress[offset];

	   if(dataRead != pattern)
                {
			LA_TRIGGER
			dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

			return(1);
		}
       }
   }

   return(AT_SUCCESS);
}


/**********************************************************************
 *
 * Function:    memTestInvIncPA()
 *
 * Description: Increment Address and Pattern by a word each loop.  invert
 *              pattern before each read/write.
 *
 **********************************************************************/
int  memTestInvIncPA(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 pattern, UINT8 inc)
{
   register UINT32 dataRead;
   register UINT32 antipattern;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestInvIncPA: ");
   for(pattern = 1, offset = 0; offset < nLongWords; pattern++, offset+=inc)
   {
       baseAddress[offset] = ~pattern;
   }


   // Check each location for the inverted pattern and set it to 0xAAAAAAAA
   for(pattern = 1, offset = 0; offset < nLongWords; pattern++, offset+=inc)
   {
       antipattern = ~pattern;

       dataRead = baseAddress[offset];

       if(dataRead != antipattern)
       {
           /*
           failure(">0 ",
		(void*) &baseAddress[offset],
		(void*)  baseAddress[offset],
		(void*) antipattern);
		   */
		   LA_TRIGGER
		   dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), dataRead, antipattern);
           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, antipattern, false);
           return(1);
       }
       baseAddress[offset] = pattern;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"InvIncPA");
   return(AT_SUCCESS);
}


/**********************************************************************
 *
 * Function:    memTestGeneric()
 *
 * Description: Writes a constant pattern throughout nBytes of memory
 *              beginning at baseAddress.  Verifies each memory location.
 *              and then writes ~pattern to that location.
 *
 **********************************************************************/
int memTestGeneric(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 pattern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestGeneric: ");
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       baseAddress[offset] = pattern;
   }

   // Check each location to see that it is pattern
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       dataRead = baseAddress[offset];

       if(dataRead != pattern)
       {
           /*
           failure(">0 ",
		(void*) &baseAddress[offset],
		(void*)  baseAddress[offset],
		(void*) pattern);
		   */
		   dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
           return(1);
       }
       baseAddress[offset] = ~dataRead;
   }


   //minPrintf("memTest%08X passed\r\n", pattern);
   return(AT_SUCCESS);
}

int memTestWrite16Read(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 pattern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 offset, i;
   UINT32 nLongWords = (nBytes / 4);

   // Check each location to see that it is pattern
   for(offset = 0; offset < nLongWords; offset+=16)
   {
       dataRead = baseAddress[offset];

       if(dataRead != pattern)
       {
           /*
           failure(">0 ",
		   (void*) &baseAddress[offset],
		   (void*)  baseAddress[offset],
		   (void*) pattern);
		   */
		   dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
           return(1);
       }
   }


   //minPrintf("memTest%08X passed\r\n", pattern);
   return(AT_SUCCESS);
}

int memTestWriteRead(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 pattern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);

   // Check each location to see that it is pattern
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       dataRead = baseAddress[offset];

       if(dataRead != pattern)
       {
           /*
           failure(">0 ",
		   (void*) &baseAddress[offset],
		   (void*)  baseAddress[offset],
		   (void*) pattern);
		   */
		   dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
           return(1);
       }
   }


   //minPrintf("memTest%08X passed\r\n", pattern);
   return(AT_SUCCESS);
}


/**********************************************************************
 *
 * Function:    memTestGeneric()
 *
 * Description: Writes a constant pattern throughout nBytes of memory
 *              beginning at baseAddress.  Verifies each memory location.
 *              and then writes ~pattern to that location.
 *
 **********************************************************************/
int memTestReadAll(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 pattern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 offset,offset2;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestReadAll: ");
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       baseAddress[offset] = ~pattern;
   }

   // Check each location to see that it is pattern
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       baseAddress[offset] = pattern;

       for(offset2 = 0; offset2 < nLongWords; offset2+=inc)
       {
           dataRead = baseAddress[offset2];


           if(dataRead != ~pattern)
           {
               if(offset==offset2)
               {
                   continue;
               }
               /*
               failure(">0 ",
                    (void*) &baseAddress[offset],
                    (void*)  baseAddress[offset],
                    (void*) ~pattern);
               */
               dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],~pattern);

               //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
               return(1);
           }
       }

       baseAddress[offset] = ~pattern;
   }


   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       baseAddress[offset] = pattern;
   }

   // Check each location to see that it is pattern
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       baseAddress[offset] = ~pattern;

       for(offset2 = 0; offset2 < nLongWords; offset2+=inc)
       {
           dataRead = baseAddress[offset2];


           if(dataRead != pattern)
           {
               if(offset==offset2)
               {
                   continue;
               }
               /*
               failure(">0 ",
                    (void*) &baseAddress[offset],
                    (void*)  baseAddress[offset],
                    (void*) pattern);
               */
               dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],pattern);

               //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, pattern, false);
               return(1);
           }
       }

       baseAddress[offset] = pattern;
   }


   //minPrintf("memTest%08X passed\r\n", pattern);
   return(AT_SUCCESS);
}

/**********************************************************************
 *
 * Function:    memTest257()
 *
 * Description: Writes pattern, increments the address, adds PATTERN_INCREMENT
 *              to the pattern and repeats 257 times.  This process is then
 *              repeated until the end of memory is reached.
 *
 **********************************************************************/
int  memTest257(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   UINT32 offset, arrayCounter;

   unsigned char byteVal;
   unsigned char *byteAddr = (unsigned char *) baseAddress;
   unsigned char expectedByte;



   //minPrintf("\r\nmemTest257: ");
   for(offset = 0; offset < (nBytes - 257); offset = offset + 257)
   {
       for(arrayCounter = 0; arrayCounter < 257; arrayCounter+=inc)
       {
           byteAddr[offset + arrayCounter] = (unsigned char)(PATTERN_STARTING_VALUE + (PATTERN_INCREMENT * arrayCounter));
       }
   }


   for(offset = 0; offset < (nBytes - 257); offset = offset + 257)
   {
       for(arrayCounter = 0; arrayCounter < 257; arrayCounter+=inc)
       {
           expectedByte = (unsigned char)(PATTERN_STARTING_VALUE + (PATTERN_INCREMENT * arrayCounter));
           byteVal = byteAddr[offset + arrayCounter];

           if(byteVal != expectedByte)
           {
               /*
               failure2(">0 ",
                    (void*) &byteAddr[offset+arrayCounter],
                    (void*) (unsigned int) byteVal,
                    (void*) (unsigned int) expectedByte);
               */
               LA_TRIGGER
               dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddr+((unsigned int)offset+arrayCounter)), byteVal,expectedByte);

               //reportPatternFailure((UINT32)&baseAddress[offset+arrayCounter], byteVal, (unsigned char)(PATTERN_STARTING_VALUE + (PATTERN_INCREMENT * arrayCounter)), true);
               return(1);
           }

           byteAddr[offset + arrayCounter] = ~byteVal;
       }
   }



   //minPrintf(TEXT_HW_MEM_TEST,"257");
   return(AT_SUCCESS);
}


/**********************************************************************
 *
 * Function:    memTestInv257()
 *
 * Description: Writes ~pattern, increments the address, adds PATTERN_INCREMENT
 *              to the pattern and repeats 257 times.  This process is then
 *              repeated until the end of memory is reached.
 *
 **********************************************************************/
int  memTestInv257(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   UINT32 offset, arrayCounter;

   unsigned char byteVal;
   unsigned char *byteAddr = (unsigned char *) baseAddress;
   unsigned char expectedByte;



   //minPrintf("\r\nmemTestInv257: ");
   for(offset = 0; offset < (nBytes - 257); offset = offset + 257)
   {
       for(arrayCounter = 0; arrayCounter < 257; arrayCounter+=inc)
       {
           byteAddr[offset + arrayCounter] = ~((unsigned char)(PATTERN_STARTING_VALUE + (PATTERN_INCREMENT * arrayCounter)));
       }
   }


   for(offset = 0; offset < (nBytes - 257); offset = offset + 257)
   {
       for(arrayCounter = 0; arrayCounter < 257; arrayCounter+=inc)
       {
           byteVal = byteAddr[offset + arrayCounter];
           expectedByte = (unsigned char)(PATTERN_STARTING_VALUE + (PATTERN_INCREMENT * arrayCounter));
           expectedByte = ~expectedByte;
           expectedByte = 0xFF & expectedByte;

           if(byteVal != expectedByte)
           {
               /*
               failure2(">0 ",
                    (void*) &byteAddr[offset+arrayCounter],
                    (void*) (unsigned int) byteVal,
                    (void*) (unsigned int) expectedByte);
               */
               LA_TRIGGER
               dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)byteAddr+((unsigned int)offset+arrayCounter)), byteVal,expectedByte);

               //reportPatternFailure((UINT32)&baseAddress[offset+arrayCounter], byteVal, expectedByte, true);
               return(1);
           }
           byteAddr[offset + arrayCounter] = byteVal;
       }
   }


   //minPrintf(TEXT_HW_MEM_TEST,"Inv257");
   return(AT_SUCCESS);
}

/**********************************************************************
 *
 * Function:    memTestAllNoise()
 *
 * Description: Alternates writing 0x00000000 and 0xFFFFFFFF in order to
 *              generate noise.
 *
 **********************************************************************/
int  memTestAllNoise(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 nLongWords = (nBytes / 4);

   UINT32 offset;
   UINT32 toggleFlag = 0;

   //minPrintf("\r\nmemTestAllNoise: ");
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       if(toggleFlag == 0)
       {
           baseAddress[offset] = 0x00000000;
           toggleFlag = 1;
       }
       else if(toggleFlag == 1)
       {
           baseAddress[offset] = 0xFFFFFFFF;
           toggleFlag = 0;
       }
       else
       {
           toggleFlag = 0;
       }
   }

   toggleFlag = 0;


   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       dataRead = baseAddress[offset];

       if((toggleFlag == 0)&&(dataRead != 0x00000000))
       {
            LA_TRIGGER
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],0x00000000);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0x00000000, false);
           return(1);
       }
       else if((toggleFlag == 1)&&(dataRead != 0xFFFFFFFF))
       {
            LA_TRIGGER
            dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],0xFFFFFFFF);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0xFFFFFFFF, false);
           return(1);
       }
       baseAddress[offset] = ~dataRead;

       if(toggleFlag == 0)
           toggleFlag = 1;
       else if(toggleFlag == 1)
           toggleFlag = 0;
       else
           toggleFlag = 0;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"AllNoise");
   return(AT_SUCCESS);
}

/**********************************************************************
 *
 * Function:    memTestHalfNoise()
 *
 * Description: Alternates writing 0xFFFF0000 and 0x0000FFFF in order to
 *              generate noise.
 *
 **********************************************************************/
int  memTestHalfNoise(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   register UINT32 dataRead;
   UINT32 offset;
   UINT32 nLongWords = (nBytes / 4);

   UINT32 toggleFlag = 0;

   toggleFlag = 0;


   //minPrintf("\r\nmemTestHalfNoise: ");
   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       if(toggleFlag == 0)
       {
           baseAddress[offset] = 0xFFFF0000;
           toggleFlag = 1;
       }
       else if(toggleFlag == 1)
       {
           baseAddress[offset] = 0x0000FFFF;
           toggleFlag = 0;
       }
       else
       {
           toggleFlag = 0;
       }
   }

   for(offset = 0; offset < nLongWords; offset+=inc)
   {
       dataRead = baseAddress[offset];

       if((toggleFlag == 0)&&(dataRead != 0xFFFF0000))
       {
           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0xFFFF0000, false);
            LA_TRIGGER
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],0xFFFF0000);

           return(1);
       }
       else if((toggleFlag == 1)&&(dataRead != 0x0000FFFF))
       {
           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0x0000FFFF, false);
           LA_TRIGGER
           dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],0x0000FFFF);

           return(1);
       }
       baseAddress[offset] = ~dataRead;

       if(toggleFlag == 0)
           toggleFlag = 1;
       else if(toggleFlag == 1)
           toggleFlag = 0;
       else
           toggleFlag = 0;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"HalfNoise");
   return(AT_SUCCESS);
}


/**********************************************************************
 *
 * Function:    memTestConverge()
 *
 * Description: Alternates writing 0xFFFF0000 from baseAddress toward the
 *              end of memory and 0x0000FFFF toward the beginning of memory.
 *
 **********************************************************************/
int  memTestConverge(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   register UINT32 dataRead, dataRead2;
   UINT32 offset, offset2;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestConverge: ");
   for(offset = 0, offset2 = nLongWords - 1; offset < nLongWords/2; offset+=inc, offset2-=inc)
   {
       baseAddress[offset]  = 0xFFFF0000;
       baseAddress[offset2] = 0x0000FFFF;
   }


   for(offset = 0, offset2 = nLongWords - 1; offset < nLongWords/2; offset+=inc, offset2-=inc)
   {
       dataRead = baseAddress[offset];
       dataRead2 = baseAddress[offset2];

       if(dataRead != 0xFFFF0000)
       {
           LA_TRIGGER
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), dataRead,0xFFFF0000);

           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0xFFFF0000, false);
           return(1);
       }
       baseAddress[offset] = ~dataRead;

       if(dataRead2 != 0x0000FFFF)
       {
            LA_TRIGGER
            dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset2*4)), dataRead2,0x0000FFFF);

           //reportPatternFailure((UINT32)&baseAddress[offset2], dataRead2, 0x0000FFFF, false);
           return(1);
       }
       baseAddress[offset2] = ~dataRead2;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"Converge");
   return(AT_SUCCESS);
}

/**********************************************************************
 *
 * Function:    memTestFourths()
 *
 * Description: Divides memory into four equal segments then alternates
 *              between the segments writing 0x0000FFFF, 0xFFFF0000
 *              0xAAAA5555, and 0x5555AAAA to each segment respectively.
 *
 **********************************************************************/
int  memTestFourths(volatile UINT32 * baseAddress, UINT32 nBytes, register UINT32 emptyPatttern, UINT8 inc)
{
   register UINT32 dataRead, dataRead2, dataRead3, dataRead4;
   UINT32 offset, offset2, offset3, offset4;
   UINT32 nLongWords = (nBytes / 4);


   //minPrintf("\r\nmemTestFourths: ");
   for(offset = 0, offset2 = nLongWords/4, offset3 = nLongWords/2, offset4 = (3*nLongWords)/4; offset < nLongWords/4; offset+=inc, offset2+=inc, offset3+=inc, offset4+=inc)
   {
       baseAddress[offset]  = 0x0000FFFF;
       baseAddress[offset2] = 0xFFFF0000;
       baseAddress[offset3] = 0xAAAA5555;
       baseAddress[offset4] = 0x5555AAAA;
   }


   for(offset = 0, offset2 = nLongWords/4, offset3 = nLongWords/2, offset4 = (3*nLongWords)/4; offset < nLongWords/4; offset+=inc, offset2+=inc, offset3+=inc, offset4+=inc)
   {
       dataRead  = baseAddress[offset];
       dataRead2 = baseAddress[offset2];
       dataRead3 = baseAddress[offset3];
       dataRead4 = baseAddress[offset4];

       if(dataRead != 0x0000FFFF)
       {
           //reportPatternFailure((UINT32)&baseAddress[offset], dataRead, 0x0000FFFF, false);
            LA_TRIGGER
            dbg_printf( " Fail_a: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset*4)), baseAddress[offset],0x0000FFFF);

           return(1);
       }
       baseAddress[offset]  = ~dataRead;

       if(dataRead2 != 0xFFFF0000)
       {
            LA_TRIGGER
            dbg_printf( " Fail_b: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset2*4)), baseAddress[offset2],0xFFFF0000);

           //reportPatternFailure((UINT32)&baseAddress[offset2], dataRead2, 0xFFFF0000, false);
           return(1);
       }
       baseAddress[offset]  = ~dataRead2;

       if(dataRead3 != 0xAAAA5555)
       {
           LA_TRIGGER
            dbg_printf( " Fail_c: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset3*4)), baseAddress[offset3],0xAAAA5555);

           //reportPatternFailure((UINT32)&baseAddress[offset3], dataRead3, 0xAAAA5555, false);
           return(1);
       }
       baseAddress[offset3] = ~dataRead3;

       if(dataRead4 != 0x5555AAAA)
       {
            LA_TRIGGER
            dbg_printf( " Fail_d: @ 0x%08X; read 0x%08x expected 0x%08X \n",((unsigned int)baseAddress+((unsigned int)offset4*4)), baseAddress[offset4],0x5555AAAA);

           //reportPatternFailure((UINT32)&baseAddress[offset4], dataRead4, 0x5555AAAA, false);
           return(1);
       }
       baseAddress[offset4] = ~dataRead4;
   }


   //minPrintf(TEXT_HW_MEM_TEST,"Fourths");
   return(AT_SUCCESS);
}

int memtest(unsigned int base, unsigned long len, int en_print)
{
    UINT32 memTestSize = len; //0x2000000;



        if (en_print) dbg_printf("memTestIncPA\n");
        if(memTestIncPA((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestIncPA\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestInvIncPA\n");
        if(memTestInvIncPA((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestInvIncPA\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestAllNoise\n");
        if(memTestAllNoise((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestAllNoise\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestHalfNoise\n");
        if(memTestHalfNoise((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestHalfNoise\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestConverge\n");
        if(memTestConverge((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestConverge\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestFourths\n");
        if(memTestFourths((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestFourths\n");

            return 1;

        }
//        puts("memTestDataMask\n");
//        if(memTestDataMask((volatile void*)base, memTestSize, 0x00000000, 1))
//        {
//            puts("\n===== failed memTestDataMask\n");
//            *_GPIOD_DATA |= 1<<23;
//            *_GPIOD_DATA |= 1<<27;
//            return;
//
//        }
//        puts("memTestDataBus\n");
//        if(memTestDataBus((volatile void*)base, memTestSize, 0x00000000, 1))
//        {
//            puts("\n===== failed memTestDataBus\n");
//            *_GPIOD_DATA |= 1<<23;
//            *_GPIOD_DATA |= 1<<27;
//            return;
//
//        }
//        puts("memTestAddressBus\n");
//        if(memTestAddressBus((volatile void*)base, memTestSize, 0x00000000, 1))
//        {
//            puts("\n===== failed memTestAddressBus\n");
//            *_GPIOD_DATA |= 1<<23;
//            *_GPIOD_DATA |= 1<<27;
//            return;
//
//        }
        if (en_print) dbg_printf("memTestGeneric 0xAAAAAAAA\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0xAAAAAAAA, 1))
        {
            LA_TRIGGER
            dbg_printf("\n===== failed 0xAAAAAAAA\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0x55555555\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0x55555555, 1))
        {
            dbg_printf("\n===== failed 0x55555555\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0xAAAA5555\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0xAAAA5555, 1))
        {
            dbg_printf("\n===== failed 0xAAAA5555\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0x5555AAAA\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0x5555AAAA, 1))
        {
            dbg_printf("\n===== failed 0x5555AAAA\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0xFFFFFFFF\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0xFFFFFFFF, 1))
        {
            dbg_printf("\n===== failed 0xFFFFFFFF\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0x5A5AA5A5\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0x5A5AA5A5, 1))
        {
            dbg_printf("\n===== failed 0x5A5AA5A5\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestGeneric 0xA5A55A5A\n");
        if(memTestGeneric((volatile void*)base, memTestSize, 0xA5A55A5A, 1))
        {
            dbg_printf("\n===== failed 0xA5A55A5A\n");

            return 1;

        }
        if (en_print) dbg_printf("memTest257\n");
        if(memTest257((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTest257\n");

            return 1;

        }
        if (en_print) dbg_printf("memTestInv257\n");
        if(memTestInv257((volatile void*)base, memTestSize, 0x00000000, 1))
        {
            dbg_printf("\n===== failed memTestInv257\n");

            return 1;

        }

//        puts("memTestDevice\n");
//        if(memTestDevice((volatile void*)base, len)>0)
//        {
//            puts("\n===== failed memTestDevice\n");
//            *_GPIOD_DATA |= 1<<23;
//            *_GPIOD_DATA |= 1<<27;
//            return;
//
//        }

//        puts("memTestGeneric 0xAAAAAAAA\n");
//        if(memTestGeneric((volatile void*)base, memTestSize, 0xAAAAAAAA, 1))
//        {
//            puts("\n===== failed 0xAAAAAAAA\n");
//            *_GPIOD_DATA |= 1<<23;
//            *_GPIOD_DATA |= 1<<27;
//            return;
//
//        }
//        if (next_ip != initial_pattern)
//        {
//            initial_pattern = next_ip;
//        }
//    }


    if (en_print) dbg_printf("\n!!!! memory test passed !!!!\n");
    return 0;

}

void mem_cbyte(unsigned int data1, unsigned int data2, unsigned int data3)
{
	int i, iResult;
	if((data2 > 0x20000000) || (data1 > data2))
	{
		dbg_printf("Wrong address. Pls use [start_addr][end_addr][iteration]\n");
		return;
	}

	for (i = 0; i < data3; i++)
	{
		if(i == 0)
			iResult = memtest(data1, data2 - data1, 1);
		else
			iResult = memtest(data1, data2 - data1, 0);
		if (iResult == 1)
			break;
		dbg_printf( ".");
	}
}
#endif