#include <atmel_start.h>
#include <stdlib.h>

#include <hpl_sercom_config.h>
#include <i2c_slave.h>
#include <flash_utils.h>
#include <checksums.h>

#include <COAST.h>

#define send_ack(descr) hri_sercomi2cs_clear_CTRLB_ACKACT_bit(descr->device.hw)
#define send_nack(descr) hri_sercomi2cs_set_CTRLB_ACKACT_bit(descr->device.hw)
#define set_cmd(descr, cmd) hri_sercomi2cs_set_CTRLB_CMD_bf(descr->device.hw, cmd)

#define EXT_CMD_ADDR 0xff

static struct io_descriptor *io;

static uint16_t dev_addr __xMR;
static uint16_t rx_state __xMR = CMD;
static uint8_t tmp_cmd_addr;
static uint16_t cmd_addr __xMR;
static int16_t cmd_index __xMR;
static int16_t cmd_len __xMR;
static uint16_t blk_wrote_len_byte __xMR;
static uint16_t last_byte_read_got_nack __xMR;

static cmd_space_t *cmds;

uint16_t use_pec __xMR = 0;
static uint16_t pec_checksum __xMR;
static uint8_t tmp_pec_checksum;
static uint8_t checksum_from_m;
static uint16_t added_tx_pec __xMR = 0;
static uint16_t sent_checksum __xMR = 0;
static uint8_t tx_tmp;

extern uint8_t status_b;
uint8_t status_cml;

/*! This function checks the command array for a specific command. Returns its
 *  index if it finds it, and -1 if it doesn't. */
int16_t in_addr_space(cmd_space_t *addr_space, uint16_t addr)
{
        for (uint8_t i = 0; i < addr_space->n_cmds; ++i)
                if (addr == addr_space->cmds[i].addr)
                        return i;
        return -1;
}

/*! We need this dummy function to bypass the triplication of calls to function
 *  pointers */
static void __attribute__((noinline)) __NO_xMR notmr_call_helper(void(*fn)(void))
{
        fn();
}

/*! \defgroup I2C_int_handlers Command space integrating interrupt handlers
 *  @{
 */

/*! we get here every time we get a byte from the master */
static void __xMR I2C_rx_complete(const struct i2c_s_async_descriptor *const descr)
{
        if (last_byte_read_got_nack) {
                hri_sercomi2cs_clear_STATUS_RXNACK_bit(descr->device.hw);
                last_byte_read_got_nack = 0;
        }
        blk_wrote_len_byte = 0;
        if (use_pec)
                added_tx_pec = 0;
        switch(rx_state) {
        case CMD:
                // first state after getting the address + write bit,
                // set the PEC checksum
                if (use_pec)
                        pec_checksum = crc8_byte(dev_addr << 1);

                io_read(io, &tmp_cmd_addr, 1);
                cmd_addr = tmp_cmd_addr;

                if (use_pec)
                        pec_checksum = crc8_byte(pec_checksum ^ cmd_addr);

                cmd_index = in_addr_space(cmds, cmd_addr);

                if (cmd_index != -1) {
                        cmd_len = 0;
                        if (*cmds->cmds[cmd_index].data_len < 0)
                                rx_state = BLK_GET_LEN;
                        else
                                rx_state = DATA;
                        send_ack(descr);
                        set_cmd(descr, 3);
                        if (cmds->cmds[cmd_index].a_callback)
                                notmr_call_helper(cmds->cmds[cmd_index].a_callback);
                } else {
                        // invalid CMD, return NACK and terminate transaction
                        send_nack(descr);
                        set_cmd(descr, 2);
                }
                break;

        case BLK_GET_LEN:
                io_read(io, &tx_tmp, 1); /* read received byte to a random variable */
                if (!((cmds->cmds[cmd_index].query_byte) & QUERY_WR)) {
                        rx_state = CMD;
                        send_nack(descr);
                        set_cmd(descr, 2);
                        break;
                }
                *cmds->cmds[cmd_index].data_len = tx_tmp;
                if (use_pec)
                        pec_checksum = crc8_byte(pec_checksum ^ *cmds->cmds[cmd_index].data_len);
                *cmds->cmds[cmd_index].data_len = -(*cmds->cmds[cmd_index].data_len);
                send_ack(descr);
                set_cmd(descr, 3);
                rx_state = DATA;
                break;

        case DATA:
                io_read(io, &tx_tmp, 1); /* read received byte to a random variable */
                if (!((cmds->cmds[cmd_index].query_byte) & QUERY_WR)) {
                        rx_state = CMD;
                        send_nack(descr);
                        set_cmd(descr, 2);
                        break;
                }
                cmds->cmds[cmd_index].data_pnt[cmd_len] = tx_tmp;
                if (use_pec)
                        pec_checksum = crc8_byte(pec_checksum ^ cmds->cmds[cmd_index].data_pnt[cmd_len]);
                ++cmd_len;
                if (cmd_len < abs(*cmds->cmds[cmd_index].data_len)) {
                        send_ack(descr);
                        set_cmd(descr, 3);
                } else {
                        if (use_pec && !cmds->cmds[cmd_index].wr_pec_disabled) {
                                send_ack(descr);
                                set_cmd(descr, 3);
                                rx_state = GET_CHECKSUM;
                        } else {
                                send_ack(descr);
                                set_cmd(descr, 2);
                                rx_state = CMD;
                                if (cmds->cmds[cmd_index].w_callback)
                                        notmr_call_helper(cmds->cmds[cmd_index].w_callback);
                        }
                }
                break;

        case GET_CHECKSUM:
                io_read(io, &checksum_from_m, 1);
                if (checksum_from_m == pec_checksum) {
                        send_ack(descr);
                        if (cmds->cmds[cmd_index].w_callback)
                                notmr_call_helper(cmds->cmds[cmd_index].w_callback);
                } else {
                        status_b |= STATUS_CML;
                        status_cml = CML_PEC; // PEC error
                        send_nack(descr);
                }
                set_cmd(descr, 2);
                rx_state = CMD;
                break;

        default:
                break;
        }
}

/*! we get here when a master read transaction expects another byte (we
 *  haven't terminated it using CMD=2 */
static void __xMR I2C_tx_pending(const struct i2c_s_async_descriptor *const descr)
{
        if (last_byte_read_got_nack) {
                set_cmd(descr, 2);
                rx_state = CMD;
                return;
        }
        if (use_pec && !added_tx_pec) {
                pec_checksum = crc8_byte(pec_checksum ^ ((dev_addr << 1) | 0x01));
                added_tx_pec = 1;
                sent_checksum = 0;
        }
        if ((*cmds->cmds[cmd_index].data_len < 0) && (!blk_wrote_len_byte)) { /* handling of block data */
                cmd_len = 0;
                tx_tmp = -*cmds->cmds[cmd_index].data_len;
                if (use_pec)
                        pec_checksum = crc8_byte(pec_checksum ^ tx_tmp);
                io_write(io, (uint8_t *)&tx_tmp, 1);
                set_cmd(descr, 3);
                blk_wrote_len_byte = 1;
        } else if (cmd_len < abs(*cmds->cmds[cmd_index].data_len)) {
                if (use_pec)
                        pec_checksum = crc8_byte(pec_checksum ^ cmds->cmds[cmd_index].data_pnt[cmd_len]);
                io_write(io, &cmds->cmds[cmd_index].data_pnt[cmd_len], 1);
                set_cmd(descr, 3);
                ++cmd_len;
        } else if (use_pec && !sent_checksum) {
                tmp_pec_checksum = pec_checksum;
                io_write(io, (uint8_t *)&tmp_pec_checksum, 1);
                set_cmd(descr, 3);
                sent_checksum = 1;
        } else {
                set_cmd(descr, 2);
                rx_state = CMD;
                if (cmds->cmds[cmd_index].r_callback)
                        notmr_call_helper(cmds->cmds[cmd_index].r_callback);
        }
}


/*! we get here when a byte has been sent to the master */
static void __xMR I2C_tx_complete(const struct i2c_s_async_descriptor *const descr)
{
        last_byte_read_got_nack = hri_sercomi2cs_get_STATUS_RXNACK_bit(descr->device.hw);
}


/*! we get here when something bad has happened */
static void __xMR I2C_error(const struct i2c_s_async_descriptor *const descr)
{
        status_b |= STATUS_CML;
        status_cml = CML_COMM_ERR; // Other comm fault
        hri_sercomi2cs_clear_INTFLAG_ERROR_bit(descr->device.hw);
        set_cmd(descr, 2);
}

/*! @} */

static void  __attribute__((noinline)) __xMR set_dev_addr(uint8_t dev_addr_notmr)
{
        dev_addr = dev_addr_notmr;
}


/*! this configures the I2C interrupt handlers with the PMBus and
    extended command structures */
void setup_I2C_slave(cmd_space_t *impl_cmds)
{
        uint8_t dev_addr_notmr = 0x12 + 4*(!gpio_get_pin_level(ADDR2)) + 2*(!gpio_get_pin_level(ADDR1)) + (!gpio_get_pin_level(ADDR0));

        cmds = impl_cmds;
        hri_sercomi2cs_clear_CTRLB_SMEN_bit(SERCOM2);
	i2c_s_async_get_io_descriptor(&I2C_0, &io);
	i2c_s_async_register_callback(&I2C_0, I2C_S_RX_COMPLETE, I2C_rx_complete);
	i2c_s_async_register_callback(&I2C_0, I2C_S_TX_PENDING, I2C_tx_pending);
	i2c_s_async_register_callback(&I2C_0, I2C_S_TX_COMPLETE, I2C_tx_complete);
	i2c_s_async_register_callback(&I2C_0, I2C_S_ERROR, I2C_error);
        i2c_s_async_set_addr(&I2C_0, dev_addr_notmr);
	i2c_s_async_enable(&I2C_0);
        set_dev_addr(dev_addr_notmr);
}
