#include "i3c_master.h"
#include "hal.h"
#include "utils.h"

static uint8_t i3c_master_entdaa_config(struct i3c_master_instance *this_i3cm, uint8_t slave_nums, uint8_t slave_addr)
{
	uint32_t wr_length, rd_length;
	uint8_t control;

	if (NULL == this_i3cm) {
			return -1;
	}

	control = CCC_IN_FRAME|STOP_IN_FRAME|START_OF_CCC;

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,control);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,0x7e); //broadcast

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,0x02); //write length

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO, I3C_CCC_ENTDAA); //ENTDAA CCC

	/* Shift the address to bits 7:1. Bit 0 is parity which is auto calculated by hardware */
	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO, slave_addr << 1);

	return 0;
}

uint8_t i3c_master_daa(struct i3c_master_instance *this_i3cm,struct i3c_dev_info *m,uint8_t *dyn_address,uint32_t device_count, uint8_t slave_addr)
{
	uint64_t device_pid = 0, device_pid_shift = 0;
	uint8_t device_bcr,device_dcr;
	unsigned char read_value;
	int count;
	int cnt=0;
	uint32_t int_status = 0, num_da_acked = 0;

	if (NULL == this_i3cm) {
			return -1;
	}

	/* Slave address takes only 7-bit */
	if (slave_addr > 127) {
		return -1;
	}

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA, 0xff);

	i3c_master_entdaa_config(this_i3cm, device_count, slave_addr);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_START_REG, START);

	/* delayMS is defined in utils.c which auto-generated when creating RISC-V MC
	 * software template
	 * */
	delayMS(1);

	while(1)
	{
		reg_32b_read(this_i3cm->base_addr | I3C_MASTER_INTR_STA, &int_status);

		if(int_status & I3C_INT_COMMAND_DONE)
		{
			reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,I3C_INT_COMMAND_DONE);

			if(int_status & I3C_INT_SLAVE_NACK) // wait until all slaves ACK on DAA
			{
				reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA, I3C_INT_SLAVE_NACK);

				reg_32b_read(this_i3cm->base_addr | I3C_MASTER_DA_ACK_REG, &num_da_acked);

				if (num_da_acked)
				{
					if(int_status & I3C_INT_R_FIFO_n_EMPTY)
					{
						reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA, I3C_INT_R_FIFO_n_EMPTY);

						for(cnt=0;cnt<device_count;cnt++)//get info from readfifo
						{
							for(count=5;count>=0;count--)
							{
								reg_8b_read(this_i3cm->base_addr | I3C_MASTER_READ_FIFO, &read_value);
								device_pid_shift = (uint64_t) read_value;
								device_pid = device_pid | (device_pid_shift << (8*count));
							}

							reg_8b_read(this_i3cm->base_addr | I3C_MASTER_READ_FIFO,&read_value);
							device_bcr=read_value;

							reg_8b_read(this_i3cm->base_addr | I3C_MASTER_READ_FIFO,&read_value);
							device_dcr=read_value;

							m->pid=device_pid;
							m->bcr=device_bcr;
							m->dcr=device_dcr;
							m->init_dyn_addr=*dyn_address;
						}
						break;
					}
					break;
				} else {
					return 1;
				}
			}
		}
	}

	return 0;
}

uint8_t i3c_master_private_i3c_read(struct i3c_master_instance *this_i3cm,uint8_t *rd_buf,uint8_t rd_length,uint8_t slave_dyn_addr)
{
	uint8_t i=0;
	uint32_t int_status=0;

	if (NULL == this_i3cm) {
			return -1;
	}

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff); //clear all pending bits

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_CFG0, 0); //ENTER I3C MODE

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										STOP_IN_FRAME);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										(slave_dyn_addr << 1) | 0x1);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										rd_length);

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_START_REG, START);

	delayMS(1);

	while(1)
	{
		reg_32b_read(this_i3cm->base_addr | I3C_MASTER_INTR_STA,&int_status);

		if(int_status & I3C_INT_COMMAND_DONE)
		{
			if(int_status & I3C_INT_SLAVE_NACK)
			{
				reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);
				return 1;
			}
			else if (int_status & I3C_INT_RD_CMD_DONE)
			{
				for(i=0; i<rd_length; i++)
				{
					reg_8b_read(this_i3cm->base_addr | I3C_MASTER_READ_FIFO, (unsigned char*) rd_buf);
					rd_buf++;
				}

				reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);
				return 0;
			}
		}
	}
}

uint8_t i3c_master_private_i3c_write(struct i3c_master_instance *this_i3cm,uint8_t *wr_buf,uint8_t wr_length,uint8_t slave_dyn_addr)
{
	uint8_t count=0;
	uint32_t int_status=0;

	if (NULL == this_i3cm) {
			return -1;
	}

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA, 0xff); //clear all pending bits

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_CFG0, 0); //ENTER I3C MODE

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
											STOP_IN_FRAME);
	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
											slave_dyn_addr << 1);
	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
											wr_length);

	for(uint8_t i=0;i<wr_length;i++)
	{
		reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO, wr_buf[i]);
	}

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_START_REG, START);

	delayMS(1);

	while(1)
	{
		reg_32b_read(this_i3cm->base_addr | I3C_MASTER_INTR_STA, &int_status);

		if (int_status & I3C_INT_COMMAND_DONE) //wait cmd done
		{
			reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);

			if(int_status & I3C_INT_SLAVE_NACK)
			{
				return 1;
			}
			else
			{
				return 0;
			}
		}
	}
}

uint8_t i3c_master_init(struct i3c_master_instance *this_i3cm,uint32_t addr,uint8_t mode,uint8_t clk,uint8_t slave_nums)
{

	uint32_t clk_value;
	unsigned char reg_value;

	if (NULL == this_i3cm) {
			return -1;
	}

	this_i3cm->base_addr = addr;
	this_i3cm->devs_nums = slave_nums;

	/* Enable the I2C mode */
	reg_32b_modify(this_i3cm->base_addr | I3C_MASTER_CFG0, I2C_MODE_ALLOWED, I2C_MODE_ALLOWED);

#ifdef ENABLE_DAA_UID
	reg_32b_modify(this_i3cm->base_addr |
							I3C_MASTER_CFG0,
							EN_DAA_UID_IN_RXFIFO,
							EN_DAA_UID_IN_RXFIFO);
#endif

	reg_8b_write(this_i3cm->base_addr |
							I3C_MASTER_INTR_ENABLE,
									0xFF);//enable all interrupt

	/*
	 * Uncomment these code if you want to reconfigure the SCL
	 * and open-drain clocks via software. The value written here
	 * is n-1. Example, if the value is Propel Builder GUI is 10
	 * then it is 9 to be written into the register.
	 */
	//reg_8b_write(this_i3cm->base_addr | I3C_MASTER_CLKPERIOD, 9); //10 is working (1.4 MHz)
	//reg_8b_write(this_i3cm->base_addr | I3C_MASTER_OPEN_DRAIN_TIMER, 0); // 1 is working value

	reg_8b_read(this_i3cm->base_addr | I3C_MASTER_CLKPERIOD, &reg_value);
	printf("Sys clk register: %d\n", reg_value);

	reg_8b_read(this_i3cm->base_addr | I3C_MASTER_OPEN_DRAIN_TIMER, &reg_value);
	printf("Open drain timer register: %d\n", reg_value);

	return 0;
}

uint8_t i3c_master_private_i2c_read(struct i3c_master_instance *this_i3cm,uint8_t *rd_buf,uint8_t rd_length,uint8_t slave_dyn_addr)
{
	uint32_t int_status=0;

	if (NULL == this_i3cm) {
			return -1;
	}

	reg_32b_modify(this_i3cm->base_addr | I3C_MASTER_CFG0, I2C_MODE_ALLOWED, I2C_MODE_ALLOWED); // make sure I2C mode is on

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
											STOP_IN_FRAME|I2C_COMMAND);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										(slave_dyn_addr << 1) | 0x1);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
											rd_length);

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_START_REG, START);

	delayMS(1);

	while(1)
	{
		reg_32b_read(this_i3cm->base_addr | I3C_MASTER_INTR_STA,&int_status);

		if(int_status & I3C_INT_COMMAND_DONE)
		{
			if(int_status & I3C_INT_SLAVE_NACK)
			{
				reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);
				return 1;
			}
			else if (int_status & I3C_INT_RD_CMD_DONE)
			{
				for(uint8_t i=0; i<rd_length; i++)
				{
					reg_8b_read(this_i3cm->base_addr | I3C_MASTER_READ_FIFO, (unsigned char*) rd_buf);
					rd_buf++;
				}

				reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);

				return 0;
			}
		}
	}
}

uint8_t i3c_master_private_i2c_write(struct i3c_master_instance *this_i3cm,uint8_t *wr_buf,uint8_t wr_length,uint8_t slave_dyn_addr)
{
	uint8_t count=0;
	uint32_t int_status=0;

	if (NULL == this_i3cm) {
			return -1;
	}

	reg_32b_modify(this_i3cm->base_addr | I3C_MASTER_CFG0, I2C_MODE_ALLOWED, I2C_MODE_ALLOWED);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										STOP_IN_FRAME|I2C_COMMAND);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										slave_dyn_addr << 1);

	reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
										wr_length);

	for(uint8_t i=0;i<wr_length;i++)
	{
		reg_8b_write(this_i3cm->base_addr | I3C_MASTER_WRITE_FIFO,
														wr_buf[i]);
	}

	reg_32b_write(this_i3cm->base_addr | I3C_MASTER_START_REG, START);

	delayMS(1);

	while(1)
	{
		reg_32b_read(this_i3cm->base_addr | I3C_MASTER_INTR_STA,&int_status);

		if(int_status & I3C_INT_COMMAND_DONE)
		{
			reg_32b_write(this_i3cm->base_addr | I3C_MASTER_INTR_STA,0xff);

			if(int_status & I3C_INT_SLAVE_NACK)
			{
				return 1;
			}
			else
			{
				return 0;
			}
		}
	}
}