#include "xstatus.h"
#include "xspi_l.h"
#include "xil_printf.h"

#include "ad9081_hal_functions.h"
#include "ad9081_api/adi_inc/adi_cms_api_common.h"

#define uint32_t u32
#define int32_t s32

#define SPI_BASEADDR		XPAR_SPI_1_BASEADDR

u32 send_byte(u8 data) {
	XSpi_WriteReg(SPI_BASEADDR, XSP_DTR_OFFSET, data);
	while (!(XSpi_ReadReg(SPI_BASEADDR, XSP_SR_OFFSET) & XSP_SR_TX_EMPTY_MASK));
	u32 recv_byte = XSpi_ReadReg(SPI_BASEADDR, XSP_DRR_OFFSET);
	return recv_byte;
}

u32 read_reg(u32 addr) {
	u32 rw = 1;
	u32 w = 0;
	u32 reg_addr = addr;

	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x1);
	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x0);
	send_byte((rw << 7) | (w << 5) | (reg_addr >> 8));
	send_byte(reg_addr & 0xFF);
	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x2);
	u32 read_data = send_byte(0);
	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x1);
//	xil_printf("Read Data 0x%x\r\n", read_data);

	return read_data;
}

u32 write_reg(u32 addr, u8 data) {
	u32 rw = 0;
	u32 w = 0;
	u32 reg_addr = addr;

	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x1);
	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x0);
	send_byte((rw << 7) | (w << 5) | (reg_addr >> 8));
	send_byte(reg_addr & 0xFF);
	send_byte(data);
	XSpi_WriteReg(SPI_BASEADDR, XSP_SSR_OFFSET, 0x1);
}

void hmc7044_init() {
	u32 Control;

	/*
	 * Set up the device in loopback mode and enable master mode.
	 */
	Control = XSpi_ReadReg(SPI_BASEADDR, XSP_CR_OFFSET);
	Control |= (XSP_CR_MASTER_MODE_MASK);
	XSpi_WriteReg(SPI_BASEADDR, XSP_CR_OFFSET, Control);

	/*
	 * Enable the device.
	 */
	Control = XSpi_ReadReg(SPI_BASEADDR, XSP_CR_OFFSET);
	Control |= XSP_CR_ENABLE_MASK;
	Control &= ~XSP_CR_TRANS_INHIBIT_MASK;
	XSpi_WriteReg(SPI_BASEADDR, XSP_CR_OFFSET, Control);
}

int32_t hal_spi_xfer_hmc7044(void *user_data, uint8_t *in_data, uint8_t *out_data, uint32_t size_bytes)
{
    uint32_t value;
    char msg[100];
    va_list argp = {0};

//    if (user_data == NULL || in_data == NULL || out_data == NULL) {
	if (in_data == NULL || out_data == NULL) {
        return API_CMS_ERROR_NULL_PARAM;
    }
    uint32_t addr = (in_data[0] << 8) | in_data[1];
    if (in_data[0] & 0x80) {
//        ads9_spi_read(addr, &value, 8);
        value = read_reg(addr);
        out_data[2] = (uint8_t)value;
        sprintf(msg, "  7044: r@%.4x = %.2x", addr, out_data[2]);
        ad9081_hal_log_write(user_data, ADI_CMS_LOG_MSG, msg, argp);
    }
    else {
//        ads9_spi_write(addr, in_data[2], 8);
        write_reg(addr, in_data[2]);
        sprintf(msg, "  7044: w@%.4x = %.2x", addr, in_data[2]);
        ad9081_hal_log_write(user_data, ADI_CMS_LOG_MSG, msg, argp);
    }

    return API_CMS_ERROR_OK;
}

void hmc7044_configure() {
	read_reg(0x78);
	read_reg(0x79);
	read_reg(0x7A);
}
