#include <stdio.h>
#include "xparameters.h"	/* XPAR parameters */
#include "xspi.h"		/* SPI device driver */
#include "xspi_l.h"
#include "xil_printf.h"
#include "spi.h"


int spi_init(XSpi *SpiInstancePtr, u16 SpiDeviceId)
{
	int Status;
	XSpi_Config *ConfigPtr;	/* Pointer to Configuration data */

	/*
	 * Initialize the SPI driver so that it is  ready to use.
	 */
	ConfigPtr = XSpi_LookupConfig(SpiDeviceId);
	if (ConfigPtr == NULL) {
		return XST_DEVICE_NOT_FOUND;
	}

	Status = XSpi_CfgInitialize(SpiInstancePtr, ConfigPtr,
				  ConfigPtr->BaseAddress);
	if (Status != XST_SUCCESS && Status != XST_DEVICE_IS_STARTED) {
		return XST_FAILURE;
	}

	/*
	 * Perform a self-test to ensure that the hardware was built correctly.
	 */
	Status = XSpi_SelfTest(SpiInstancePtr);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Run loopback test only in case of standard SPI mode.
	 */
	if (SpiInstancePtr->SpiMode != XSP_STANDARD_MODE) {
		return XST_SUCCESS;
	}

	/*
	 * Set the Spi device as a master and in loopback mode.
	 */
	Status = XSpi_SetOptions(SpiInstancePtr, XSP_MASTER_OPTION);
	if (Status != XST_SUCCESS) {
		return XST_FAILURE;
	}

	/*
	 * Start the SPI driver so that the device is enabled.
	 */
	XSpi_Start(SpiInstancePtr);

	/*
	 * Disable Global interrupt to use polled mode operation
	 */
	XSpi_IntrGlobalDisable(SpiInstancePtr);

	return XST_SUCCESS;
}

#define SPI_READ(x)		{x |= 0x80;}
#define SPI_WRITE(x)	{x &= 0x7f;}

int spi_rw_reg(XSpi *spi, uint16_t reg_addr, uint8_t *reg_val, int write)
{
	uint8_t wbuf[3] = {reg_addr >> 8, reg_addr & 0xff, 0};
	uint8_t rbuf[3] = {0, 0, 0};
	int status = 0;

	if(spi == NULL){
		xil_printf("%s spi is null\n\r", __func__);
		return 1;
	}

	if(reg_val == NULL && write == 0){
		xil_printf("%s reg_val is null\n\r", __func__);
		return 1;
	}

	if(write){
		wbuf[2] = *reg_val;
		SPI_WRITE(*wbuf);
	}else{
		SPI_READ(*wbuf);
	}

	if((status = XSpi_Transfer(spi, wbuf, rbuf, sizeof(wbuf))))
		return 1;

	if(write == 0)
		*reg_val = rbuf[2];

	return 0;
}

int spi_write_reg(XSpi *spi, uint16_t reg_addr, uint8_t reg_val)
{
	return spi_rw_reg(spi, reg_addr, &reg_val, 1);
}

int spi_write_conf(XSpi *spi, struct reg_t *conf, int size)
{

	if(conf == NULL){
		xil_printf("%s conf parameter is null\n\r", __func__);
		return 1;
	}

	if(size <= 0){
		xil_printf("%s size parameter is zero or less\n\r", __func__);
		return 1;
	}

	if(spi == NULL){
		xil_printf("%s spi parameter is null\n\r", __func__);
		return 1;
	}

	for(int i = 0; i < size; i++){
		if(spi_rw_reg(spi, conf[i].reg, &conf[i].val, 1)){
			xil_printf("%s SPI write failed (%04x, %02x)\n\r", __func__, conf[i].reg, conf[i].val);
			return 1;
		}
	}

	return 0;
}

int spi_compare_conf(XSpi *spi, struct reg_t *conf, int size)
{
	uint8_t reg_val = 0;

	if(conf == NULL){
		xil_printf("%s conf parameter is null\n\r", __func__);
		return 1;
	}

	if(size <= 0){
		xil_printf("%s size parameter is zero or less\n\r", __func__);
		return 1;
	}

	if(spi == NULL){
		xil_printf("%s spi parameter is null\n\r", __func__);
		return 1;
	}

	for(int i = 0; i < size; i++){
		if(spi_rw_reg(spi, conf[i].reg, &reg_val, 0)){
			xil_printf("%s SPI read failed (%04x)\n\r", __func__, conf[i].reg);
			return 1;
		}
		if(conf[i].val != reg_val){
			xil_printf("%s register %04x has different value  %02x (%02x)\n\r", __func__, conf[i].reg, reg_val, conf[i].val);
			return 1;
		}
	}

	return 0;
}
