/* i2c.c */

#include <avr/io.h>
#include <util/twi.h>
#include <avr/interrupt.h>

#include "common.h"
#include "beep.h"

//#define i2c_wait() do { uint16_t count = 0; while (!(TWCR & (1<<TWINT))) if (count++ > 1000) break; } while (0)
//#define i2c_status() (TWSR & 0xF8)
#define i2c_check(status) if (i2c_status() != (status)) goto ERROR

static void i2c_wait(void)
{
    uint16_t count = 0;
    while (!(TWCR & (1<<TWINT)))
        if (count++ > 1000)
	    break;
}

static __attribute__ ((noinline)) uint8_t i2c_status(void)
{
    return TWSR & 0xF8;
}

uint8_t i2c_xor;

void i2c_init(void)
{
    TWSR = 0;  /* prescaler TWPS = 0 */
    TWBR = 10; /* minimum value allowed */
	       /* f_SCL = clk / (16+(2*TWBR*TWPS)) */

    TWCR = (1<<TWEN);

    i2c_xor = 0;
}

//#define i2c_start() do { TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN); } while (0)
//#define i2c_stop()  do { TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO); } while (0)
//#define i2c_txbyte(data) do { TWDR = data; TWCR = (1 << TWINT) | (1<<TWEN); } while (0)

static void i2c_start(void)
{
    TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN);
}

static void i2c_stop(void)
{
    TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWSTO);
}

static __attribute__((noinline)) void i2c_txbyte(uint8_t data)
{
    TWDR = data;
    TWCR = (1 << TWINT) | (1<<TWEN);
}

void i2c_set_xor(uint8_t xor)
{
    i2c_xor = xor;
}

bool i2c_write(uint8_t address, uint8_t prefix, uint8_t bytes, uint8_t *data)
{
    uint8 i;

    i2c_start();
    i2c_wait();
    i2c_check(TW_START);

    i2c_txbyte((address << 1) + 0);
    i2c_wait();

    i2c_check(TW_MT_SLA_ACK);

    i2c_txbyte(prefix);
    i2c_wait();
    i2c_check(TW_MT_DATA_ACK);

    for (i = 0; i < bytes; i++) {
	i2c_txbyte(data[i] ^ i2c_xor);
	i2c_wait();
	i2c_check(TW_MT_DATA_ACK);
    }

    i2c_stop();
    return TRUE;

    ERROR:
    i2c_stop();
    return FALSE;
}