#include <atmel_start.h>
#include <stdlib.h>
#include <checksums.h>
#include "i2c_master_mod_fns.h"

static uint8_t got_block_len = 0;

static bool use_pec = 0;

/* start of code that had to be slightly modified wrt the original at hpl_sercom.c */

#define ERROR_FLAG (1 << 7)
#define SB_FLAG (1 << 1)
#define MB_FLAG (1 << 0)

#define CMD_STOP 0x3
#define I2C_IDLE 0x1
#define I2C_SM 0x0
#define I2C_FM 0x1
#define I2C_HS 0x2
#define TEN_ADDR_FRAME 0x78
#define TEN_ADDR_MASK 0x3ff
#define SEVEN_ADDR_MASK 0x7f

static inline void _sercom_i2c_my_send_stop(void *const hw)
{
	hri_sercomi2cm_set_CTRLB_CMD_bf(hw, CMD_STOP);
}

static inline int32_t _sercom_i2c_sync_my_wait_bus(struct _i2c_m_sync_device *const i2c_dev, uint32_t *flags)
{
	uint32_t timeout = 65535;
	void *   hw      = i2c_dev->hw;

	do {
		*flags = hri_sercomi2cm_read_INTFLAG_reg(hw);

		if (timeout-- == 0) {
			return I2C_ERR_BUS;
		}
	} while (!(*flags & MB_FLAG) && !(*flags & SB_FLAG));

	return I2C_OK;
}

static int32_t _sercom_i2c_sync_my_analyse_flags(void *const hw, uint32_t flags, struct _i2c_m_msg *const msg)
{
	int      sclsm  = hri_sercomi2cm_get_CTRLA_SCLSM_bit(hw);
	uint16_t status = hri_sercomi2cm_read_STATUS_reg(hw);

	if (flags & MB_FLAG) {
		/* tx error */
		if (status & SERCOM_I2CM_STATUS_ARBLOST) {
			hri_sercomi2cm_clear_interrupt_MB_bit(hw);
			msg->flags |= I2C_M_FAIL;
			msg->flags &= ~I2C_M_BUSY;

			if (status & SERCOM_I2CM_STATUS_BUSERR) {
				return I2C_ERR_BUS;
			}

			return I2C_ERR_BAD_ADDRESS;
		} else {
			if (status & SERCOM_I2CM_STATUS_RXNACK) {

				/* Slave rejects to receive more data */
				if (msg->len > 0) {
					msg->flags |= I2C_M_FAIL;
				}

				if (msg->flags & I2C_M_STOP) {
					_sercom_i2c_my_send_stop(hw);
				}

				msg->flags &= ~I2C_M_BUSY;

				return I2C_NACK;
			}

			if (msg->flags & I2C_M_TEN) {
				hri_sercomi2cm_write_ADDR_reg(hw,
				                              ((((msg->addr & TEN_ADDR_MASK) >> 8) | TEN_ADDR_FRAME) << 1) | I2C_M_RD
				                                  | (hri_sercomi2cm_read_ADDR_reg(hw) & SERCOM_I2CM_ADDR_HS));
				msg->flags &= ~I2C_M_TEN;

				return I2C_OK;
			}

			if (msg->len == 0) {
				if (msg->flags & I2C_M_STOP) {
					_sercom_i2c_my_send_stop(hw);
				}

				msg->flags &= ~I2C_M_BUSY;
			} else {
				hri_sercomi2cm_write_DATA_reg(hw, *msg->buffer);
				msg->buffer++;
				msg->len--;
			}

			return I2C_OK;
		}
	} else if (flags & SB_FLAG) {
		if ((msg->len) && !(status & SERCOM_I2CM_STATUS_RXNACK)) {
			msg->len--;

			/* last byte, send nack */
			if ((msg->len == 0 && !sclsm) || (msg->len == 1 && sclsm)) {
				hri_sercomi2cm_set_CTRLB_ACKACT_bit(hw);
			}

			if (msg->len == 0) {
				if (msg->flags & I2C_M_STOP) {
					hri_sercomi2cm_clear_CTRLB_SMEN_bit(hw);
					_sercom_i2c_my_send_stop(hw);
				}

				msg->flags &= ~I2C_M_BUSY;
			}

			/* Accessing DATA.DATA auto-triggers I2C bus operations.
			 * The operation performed depends on the state of
			 * CTRLB.ACKACT, CTRLB.SMEN
			 **/

                        if (!got_block_len) {
                                got_block_len = 1;
                                msg->len = hri_sercomi2cm_read_DATA_reg(hw);
                                if (use_pec)
                                        ++msg->len;
                        } else {
                                *msg->buffer++ = hri_sercomi2cm_read_DATA_reg(hw);
                        }
		} else {
			hri_sercomi2cm_clear_interrupt_SB_bit(hw);
			return I2C_NACK;
		}

		hri_sercomi2cm_clear_interrupt_SB_bit(hw);
	}

	return I2C_OK;
}

static int32_t _sercom_i2c_sync_my_send_address(struct _i2c_m_sync_device *const i2c_dev)
{
	void *             hw    = i2c_dev->hw;
	struct _i2c_m_msg *msg   = &i2c_dev->service.msg;
	int                sclsm = hri_sercomi2cm_get_CTRLA_SCLSM_bit(hw);
	uint32_t           flags;

	ASSERT(i2c_dev);

	if (msg->len == 1 && sclsm) {
		hri_sercomi2cm_set_CTRLB_ACKACT_bit(hw);
	} else {
		hri_sercomi2cm_clear_CTRLB_ACKACT_bit(hw);
	}

	/* ten bit address */
	if (msg->addr & I2C_M_TEN) {
		if (msg->flags & I2C_M_RD) {
			msg->flags |= I2C_M_TEN;
		}

		hri_sercomi2cm_write_ADDR_reg(hw,
		                              ((msg->addr & TEN_ADDR_MASK) << 1) | SERCOM_I2CM_ADDR_TENBITEN
		                                  | (hri_sercomi2cm_read_ADDR_reg(hw) & SERCOM_I2CM_ADDR_HS));
	} else {
		hri_sercomi2cm_write_ADDR_reg(hw,
		                              ((msg->addr & SEVEN_ADDR_MASK) << 1) | ((msg->flags & I2C_M_RD) ? I2C_M_RD : 0x0)
		                                  | (hri_sercomi2cm_read_ADDR_reg(hw) & SERCOM_I2CM_ADDR_HS));
	}

	_sercom_i2c_sync_my_wait_bus(i2c_dev, &flags);
	return _sercom_i2c_sync_my_analyse_flags(hw, flags, msg);
}

static int32_t _i2c_m_sync_my_transfer(struct _i2c_m_sync_device *const i2c_dev, struct _i2c_m_msg *msg)
{
	uint32_t flags;
	int      ret;
	void *   hw = i2c_dev->hw;

	ASSERT(i2c_dev);
	ASSERT(i2c_dev->hw);
	ASSERT(msg);

	if (i2c_dev->service.msg.flags & I2C_M_BUSY) {
		return I2C_ERR_BUSY;
	}

	msg->flags |= I2C_M_BUSY;
	i2c_dev->service.msg = *msg;
	hri_sercomi2cm_set_CTRLB_SMEN_bit(hw);

        got_block_len = 0;

	ret = _sercom_i2c_sync_my_send_address(i2c_dev);

        msg->len = i2c_dev->service.msg.len;

	if (ret) {
		i2c_dev->service.msg.flags &= ~I2C_M_BUSY;

		return ret;
	}

	while (i2c_dev->service.msg.flags & I2C_M_BUSY) {
		ret = _sercom_i2c_sync_my_wait_bus(i2c_dev, &flags);

		if (ret) {
			if (msg->flags & I2C_M_STOP) {
				_sercom_i2c_my_send_stop(hw);
			}

			i2c_dev->service.msg.flags &= ~I2C_M_BUSY;

			return ret;
		}

		ret = _sercom_i2c_sync_my_analyse_flags(hw, flags, &i2c_dev->service.msg);
	}

	return ret;
}


/* end of code that had to be slightly modified wrt the original at hpl_sercom.c */


// block write-block read process call
int32_t i2c_m_sync_blk_cmd_write_and_read(struct i2c_m_sync_desc *i2c, uint8_t cmd, uint8_t *wr_buffer, uint8_t wr_length, uint8_t *rd_buffer, uint8_t *rd_length)
{
	struct _i2c_m_msg msg;
	int32_t           ret;

        uint8_t *tmp_buf = (uint8_t *)malloc(wr_length + 2);
        tmp_buf[0] = cmd;
        tmp_buf[1] = wr_length;
        for (uint8_t i = 0; i < wr_length; ++i)
                tmp_buf[2+i] = wr_buffer[i];

	msg.addr   = i2c->slave_addr;
	msg.len    = 2+wr_length;
	msg.flags  = 0;
	msg.buffer = tmp_buf;

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
                free(tmp_buf);
		return ret;
	}

	msg.flags  = I2C_M_STOP | I2C_M_RD;
	msg.buffer = rd_buffer;
	msg.len    = 16;

	ret = _i2c_m_sync_my_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
                free(tmp_buf);
		return ret;
	}

        if (use_pec)
                --msg.len;

        if (rd_length)
                *rd_length = msg.len;

        if (!use_pec) {
                free(tmp_buf);
                return ERR_NONE;
        }

        uint8_t pec;
        pec = crc8_bytearray(tmp_buf, 2+wr_length, crc8_byte(i2c->slave_addr << 1));
        free(tmp_buf);
        pec = crc8_byte(pec ^ ((i2c->slave_addr << 1) | 0x01));
        pec = crc8_byte(pec ^ msg.len);
        pec = crc8_bytearray(rd_buffer, msg.len, pec);
        if (pec == rd_buffer[msg.len]) {
                return ERR_NONE;
        } else {
                return ERR_BAD_DATA;
        }
}


// block read - the data buffer has to also fit the maximum amount of
// data the slave can reply with - including the PEC byte, if it's on
int32_t i2c_m_sync_blk_cmd_read(struct i2c_m_sync_desc *i2c, uint8_t cmd, uint8_t *buffer, uint8_t *length)
{
	struct _i2c_m_msg msg;
	int32_t           ret;

	msg.addr   = i2c->slave_addr;
	msg.len    = 1;
	msg.flags  = 0;
	msg.buffer = &cmd;

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
		return ret;
	}

	msg.flags  = I2C_M_STOP | I2C_M_RD;
	msg.buffer = buffer;
	msg.len    = 16;

	ret = _i2c_m_sync_my_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
		return ret;
	}

        if (use_pec)
                --msg.len;

        if (length)
                *length = msg.len;

        if (!use_pec)
                return ERR_NONE;

        uint8_t pec;
        pec = crc8_byte(i2c->slave_addr << 1);
        pec = crc8_byte(pec ^ cmd);
        pec = crc8_byte(pec ^ ((i2c->slave_addr << 1) | 0x01));
        pec = crc8_byte(pec ^ msg.len);
        pec = crc8_bytearray(buffer, msg.len, pec);
        if (pec == buffer[msg.len]) {
                return ERR_NONE;
        } else {
                return ERR_BAD_DATA;
        }
}


// write, adapted from hal_i2c_m_sync.c - the original version had a stop between the cmd and the data
int32_t i2c_m_sync_my_cmd_write(struct i2c_m_sync_desc *i2c, uint8_t cmd, uint8_t *buffer, uint8_t length)
{
	struct _i2c_m_msg msg;
	int32_t           ret;

        uint8_t *tmp_buf;
        if (use_pec)
                tmp_buf = (uint8_t *)malloc(length + 2);
        else
                tmp_buf = (uint8_t *)malloc(length + 1);
        tmp_buf[0] = cmd;
        for (uint8_t i = 0; i < length; ++i)
                tmp_buf[1+i] = buffer[i];
        if (use_pec)
                tmp_buf[1+length] = crc8_bytearray(tmp_buf, 1+length, crc8_byte(i2c->slave_addr << 1));

	msg.addr   = i2c->slave_addr;
        if (use_pec)
                msg.len    = 2+length;
        else
                msg.len    = 1+length;
	msg.flags  = I2C_M_STOP;
	msg.buffer = tmp_buf;

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

        free(tmp_buf);

	if (ret != 0) {
		/* error occurred */
		return ret;
	}

	return ERR_NONE;
}


// write, adapted from hal_i2c_m_sync.c to add PEC
int32_t i2c_m_sync_my_cmd_read(struct i2c_m_sync_desc *i2c, uint8_t reg, uint8_t *buffer, uint8_t length)
{
	struct _i2c_m_msg msg;
	int32_t           ret;

	msg.addr   = i2c->slave_addr;
	msg.len    = 1;
	msg.flags  = 0;
	msg.buffer = &reg;

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
                memset(buffer, 0xFF, length);
		return ret;
	}

        uint8_t *pec_buf = 0;
        if (use_pec)
                pec_buf = (uint8_t *)malloc(length + 1);
	msg.flags  = I2C_M_STOP | I2C_M_RD;
        if (use_pec) {
                msg.buffer = pec_buf;
                msg.len    = 1+length;
        } else {
                msg.buffer = buffer;
                msg.len    = length;
        }

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

	if (ret != 0) {
		/* error occurred */
                if (use_pec)
                        free(pec_buf);
                memset(buffer, 0xFF, length);
		return ret;
	}

        if (!use_pec)
                return ERR_NONE;

        uint8_t pec;
        pec = crc8_byte(i2c->slave_addr << 1);
        pec = crc8_byte(pec ^ reg);
        pec = crc8_byte(pec ^ ((i2c->slave_addr << 1) | 0x01));
        pec = crc8_bytearray(pec_buf, length, pec);
        memcpy(buffer, pec_buf, length);
        if (pec == pec_buf[length]) {
                free(pec_buf);
                return ERR_NONE;
        } else {
                free(pec_buf);
                return ERR_BAD_DATA;
        }
}


// block write
int32_t i2c_m_sync_blk_cmd_write(struct i2c_m_sync_desc *i2c, uint8_t cmd, uint8_t *buffer, uint8_t length)
{
	struct _i2c_m_msg msg;
	int32_t           ret;

        uint8_t *tmp_buf;
        if (use_pec)
                tmp_buf  = (uint8_t *)malloc(length + 3);
        else
                tmp_buf  = (uint8_t *)malloc(length + 2);
        tmp_buf[0] = cmd;
        tmp_buf[1] = length;
        for (uint8_t i = 0; i < length; ++i)
                tmp_buf[2+i] = buffer[i];
        if (use_pec)
                tmp_buf[2+length] = crc8_bytearray(tmp_buf, 2+length, crc8_byte(i2c->slave_addr << 1));

	msg.addr   = i2c->slave_addr;
        if (use_pec)
                msg.len    = 3+length;
        else
                msg.len    = 2+length;
	msg.flags  = I2C_M_STOP;
	msg.buffer = tmp_buf;

	ret = _i2c_m_sync_transfer(&i2c->device, &msg);

        free(tmp_buf);

	if (ret != 0) {
		/* error occurred */
		return ret;
	}

	return ERR_NONE;
}


int32_t enable_pec(struct i2c_m_sync_desc *i2c)
{
        uint8_t wr_pec = 1;
        uint32_t ret;

        use_pec = 0;

        ret = i2c_m_sync_blk_cmd_write(i2c, ADDR_USE_PEC, &wr_pec, 1);

        use_pec = 1;

        return ret;
}


int32_t disable_pec(struct i2c_m_sync_desc *i2c)
{
        uint8_t wr_pec = 0;
        uint32_t ret;

        use_pec = 0;

        ret = i2c_m_sync_blk_cmd_write(i2c, ADDR_USE_PEC, &wr_pec, 1);

        return ret;
}
