#include "i2c.h"

volatile bool _i2c_locked;

int i2c_send(uint8_t addr, const void *buf, size_t len)
{
    return i2c_send_recv(addr, data, len, NULL, 0);
}

int i2c_recv(uint8_t addr, void *buf, size_t len)
{
    return i2c_send_recv(addr, NULL, 0, buf, len);
}

static int i2c_send_byte_err(uint8_t byte, uint8_t ctl)
{
    uint32_t wdata  = (byte << 8) | I2C_NAK | ctl;
    uint32_t status = (wdata & ~I2C_NAK) | I2C_SCL | ((~ctl & 4) << 2);

    i2c_wait();
    I2C_WDATA = wdata;
    i2c_wait();
    status ^= I2C_RDATA & ~I2C_SDA;

    if (!status)
	return 0;
    else if (status & 0x7f)
	return I2C_ERR_IO;
    else if (status & I2C_NAK)
	return I2C_ERR_NAK;
    else
	return I2C_ERR_CONFLICT;
}

static int i2c_recv_byte_err(uint8_t ctl)
{
    uint32_t status = I2C_SCL | (ctl & 6) | ((~ctl & 4) << 2);
    
    i2c_wait();
    I2C_WDATA = (0xff << 8) | ctl;
    i2c_wait();
    status ^= I2C_RDATA & ~I2C_SDA;

    if (status & 0x7f)
	return I2C_ERR_IO;
    else
	return status >> 8;	/* Data byte (will be >= 0) */
}

int i2c_send_recv(uint8_t addr,
		  const void *obuf, size_t olen,
		  void *ibuf, size_t ilen)
{
    int sda_retry_count = 16;
    int err = 0;
    uint8_t ctl = I2C_P;

    if (unlikely(!olen && !ilen))
	return 0;
    
    if (!i2c_lock())
	return I2C_BUSY;

    /* SDA held low? */
    while (unlikely(!(I2C_RDATA & I2C_SDA))) {
	i2c_send_byte(0xff, I2C_DUMMY);
	i2c_wait();

	if (!sda_retry_count--) {
	    err = I2C_ERR_STUCK;
	    goto done;
	}
    }

    if (olen) {
	const uint8_t wcmd = addr << 1;
	const uint8_t *p = obuf;

	ctl = 0;
	err = i2c_send_byte_err(wcmd, ctl);
	if (err) {
	    if (err == I2C_ERR_NAK)
		err = I2C_ERR_NODEV;
	    goto done;
	}

	while (olen--) {
	    ctl = olen ? 0 : ilen ? I2C_SR : I2C_P;
	    err = i2c_send_byte_err(*p++, ctl);
	    if (err) {
		if (!(ctl & I2C_P))
		    i2c_send_byte(0xff, I2C_P);
		goto done;
	    }
	}
    }

    if (ilen) {
	const uint8_t rcmd = (addr << 1) + 1;
	uint8_t *q = ibuf;

	ctl = 0;
	err = i2c_send_byte_err(rcmd, ctl);
	if (err) {
	    if (err == I2C_ERR_NAK)
		err = I2C_ERR_NODEV;
	    goto done;
	}

	while (ilen--) {
	    int data;
	    
	    ctl = ilen ? 0 : I2C_P;
	    data = i2c_recv_byte_err(ctl);

	    if (data < 0) {
		err = data;
		goto done;
	    }

	    *q++ = data;
	}
    }

    I2C_RESET = I2C_RESET_UNSTART; /* If transaction aborted */
    i2c_unlock();
    return err;
}