#include "CPLD_Con.h"

int CPLD_Write(u8 addr,u32 data)
{
	u16 reg_addr = addr | 0x80;				//Write bit set to 1'b1
	u8 reg_data[dat_width] = {0};
	//Data Sanity Check
	if ((data >> 24) > 0)
	{
		
		xil_printf("[E] CPLD data length should within 24 bits!\r\n");
		return XST_INVALID_PARAM;
	}
	reg_data[0] = (u8)(data >> 16);
	reg_data[1] = (u8)(((data)&0xff00)>>8);
	reg_data[2] = (u8)data;
	spi_write_onewire(reg_addr, reg_data, dat_width, CPLD_U1);
	spi_write_onewire(reg_addr, reg_data, dat_width, CPLD_U2);
	spi_write_onewire(reg_addr, reg_data, dat_width, CPLD_U3);

	return XST_SUCCESS;
}

int CPLD_Read(u8 CPLDSel, u8 reg_addr, u32* reg_data)
{
    switch (CPLDSel)
    {
        case 1:
        {
            reg_data = uspi_read_onewire(reg_addr, dat_width,CPLD_U1);
            break;
        }
        case 2:
        {
            reg_data = uspi_read_onewire(reg_addr, dat_width,CPLD_U2);
            break;
        }
        case 3:
        {
            reg_data = uspi_read_onewire(reg_addr, dat_width,CPLD_U3);
            break;
        }
        
        default:
        {
            xil_printf("[E] CPLD Select Num Error (Range 1-3)\r\n");
            return XST_INVALID_PARAM;
        }
    }
}

int CPLD_Read_ALL(u8 reg_addr, u32* reg_data)
{
    //TODO:Sanity Check
    reg_data[0]=uspi_read_onewire(reg_addr, dat_width,CPLD_U1);
    reg_data[1]=uspi_read_onewire(reg_addr, dat_width,CPLD_U2);
    reg_data[2]=uspi_read_onewire(reg_addr, dat_width,CPLD_U3);

    //TODO:SUCCESS Check
    return XST_SUCCESS;
}

int CPLD_exec()
{
	u8 reg_addr = 0x01;
    u8 sw_config_data[3] = {0x00,0x00,0x00};
	sw_config_data[0] = 0x00;
	sw_config_data[1] = 0x00;
	sw_config_data[2] = 0x01;

	reg_addr = reg_addr | 0x80;
	spi_write_onewire(reg_addr, sw_config_data, dat_width, CPLD_U1);
	spi_write_onewire(reg_addr, sw_config_data, dat_width, CPLD_U2);
	spi_write_onewire(reg_addr, sw_config_data, dat_width, CPLD_U3);

	return XST_SUCCESS;
}
