#include "CPLD_Con.h"

int CPLD_Checkin()
{
    int iStatus = -255;
    u32 ureg_data = 0x0;
    u8  uCPLD_Count = 3;

    xil_printf("[I] Start CPLD Check In...\r\n");
    xil_printf("[-]\t Checking CPLD #1\r\n");
    //Read CPLD IDENT Reg
	iStatus = CPLD_Read(0x1,0x0,&ureg_data);
	if (iStatus != XST_SUCCESS)
	{
		xil_printf("[E] CPLD #1 Communication Failed!\r\n");
		uCPLD_Count -= 1;
	}
	else
	{
		if (ureg_data == 0x0)
		{
			xil_printf("[E] CPLD #1 Not present! Or SPI communication Failed!\r\n");
			uCPLD_Count -= 1;
		}
		else
		{
			if ((ureg_data & 0x200000) != 0x200000)
			{
				u8 uCID = 0;
				uCID = (u8)(ureg_data >> 16);
				xil_printf("[E] CPLD #1 Firmware mismatch!\r\n[D] Expect CPLD ID == 1, but we received CPLD ID = 0x%02X\r\n",uCID);
				uCPLD_Count -= 1;
			}
			else
			{
				u8 uMajorV = 0;
				u8 uMinorV = 0;
				u8 uFixV = 0;
				
				uMajorV = (u8)((ureg_data & 0x0f00)>>8);
				uMinorV = (u8)((ureg_data & 0xf0)>>4);
				uFixV = (u8)(ureg_data & 0xf);

				xil_printf("[I]\t Successfully Found CPLD #1!\r\n");
				xil_printf("[I]\t CPLD #1 Version: v%02d.%02d.%02d\r\n",uMajorV,uMinorV,uFixV);
			}		
		}
	}

    xil_printf("[-]\t Checking CPLD #2\r\n");
    //Read CPLD IDENT Reg
	iStatus = CPLD_Read(0x2,0x0,&ureg_data);
	if (iStatus != XST_SUCCESS)
	{
		xil_printf("[E] CPLD #2 Communication Failed!\r\n");
		uCPLD_Count -= 1;
	}
	else
	{
		if (ureg_data == 0x0)
		{
			xil_printf("[E] CPLD #2 Not present! Or SPI communication Failed!\r\n");
			uCPLD_Count -= 1;
		}
		else
		{
			if ((ureg_data & 0x400000) != 0x400000)
			{
				u8 uCID = 0;
				uCID = (u8)(ureg_data >> 16);
				xil_printf("[E] CPLD #2 Firmware mismatch!\r\n[D] Expect CPLD ID == 2, but we received CPLD ID = 0x%02X\r\n",uCID);
				uCPLD_Count -= 1;
			}
			else
			{
				u8 uMajorV = 0;
				u8 uMinorV = 0;
				u8 uFixV = 0;
				
				uMajorV = (u8)((ureg_data & 0x0f00)>>8);
				uMinorV = (u8)((ureg_data & 0xf0)>>4);
				uFixV = (u8)(ureg_data & 0xf);

				xil_printf("[I]\t Successfully Found CPLD #2!\r\n");
				xil_printf("[I]\t CPLD #2 Version: v%02d.%02d.%02d\r\n",uMajorV,uMinorV,uFixV);
			}		
		}
	}

    xil_printf("[-]\t Checking CPLD #3\r\n");
    //Read CPLD IDENT Reg
	iStatus = CPLD_Read(0x3,0x0,&ureg_data);
	if (iStatus != XST_SUCCESS)
	{
		xil_printf("[E] CPLD #3 Communication Failed!\r\n");
		uCPLD_Count -= 1;
	}
	else
	{
		if (ureg_data == 0x0)
		{
			xil_printf("[E] CPLD #3 Not present! Or SPI communication Failed!\r\n");
			uCPLD_Count -= 1;
		}
		else
		{
			if ((ureg_data & 0x800000) != 0x800000)
			{
				u8 uCID = 0;
				uCID = (u8)(ureg_data >> 16);
				xil_printf("[E] CPLD #3 Firmware mismatch! Expect CPLD ID == 3, but we received CPLD ID = 0x%02X\r\n",uCID);
				uCPLD_Count -= 1;
			}
			else
			{
				u8 uMajorV = 0;
				u8 uMinorV = 0;
				u8 uFixV = 0;
				
				uMajorV = (u8)((ureg_data & 0x0f00)>>8);
				uMinorV = (u8)((ureg_data & 0xf0)>>4);
				uFixV = (u8)(ureg_data & 0xf);

				xil_printf("[I]\t Successfully Found CPLD #3!\r\n");
				xil_printf("[I]\t CPLD #3 Version: v%02d.%02d.%02d\r\n",uMajorV,uMinorV,uFixV);
			}		
		}
	}
	if(uCPLD_Count == 3)
	{
		return XST_SUCCESS;
	}
	else
	{
		xil_printf("[E] Only %d/3 CPLD Device is/are healthy\r\n",uCPLD_Count);
        return XST_DEVICE_NOT_FOUND;
	}
}

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;
        }
    }
    return XST_SUCCESS;
}

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;
}
