#include "archinc.h"
#include "i2cregs.h"
#include "i2cmaster.h"

#ifdef CONFIG_INCLUDE_LPC210X_I2C

#define GET_STATUS(a)       ((a) = (I2STAT & 0xff))
#define SLAVE_ADDR_WRITE(a) ((a) & 0xfe)
#define SLAVE_ADDR_READ(a)  ((a) | 0x01)

#define RECEIVE_I2C_DATA(i)  (readData[(i)] |= (((unsigned short)I2DAT) & 0xff)
#define SEND_I2C_DATA(i)     (I2DAT = ((unsigned long)(writeData[(i)] & 0xff)))

#define GET_I2C_ADDR_WRITE(i)  ((writeData[(i)] >> 8) & 0xff)
#define GET_I2C_ADDR_READ(i)  ((readData[(i)] >> 8) & 0xff)
#define GET_I2C_DATA_READ(i)   (readData[(i)] & 0xff)

#define PUT_DATA_WRITE(i, a, d) (writeData[(i)] = ((((a) & 0xff) << 8) | ((d) & 0xff)))

typedef enum {none, addr, data, arbLost} i2cError;
typedef enum {idle, ready, busy, error} i2cState;
typedef enum {noop, probe, read, write} i2cOp;

static i2cState currentState = idle;
static i2cOp currentOp = noop;
static i2cError currentError = none;

static boolean sendStop;

static unsigned short readData[CONFIG_I2C_RX_BUFFER_SIZE];
static int readHead = 0;
static int readTail = 0;
static int readLen  = 0;
static int readRequestLen;

static unsigned short writeData[CONFIG_I2C_TX_BUFFER_SIZE];
static int writeHead = 0;
static int writeTail = 0;
static int writeLen  = 0;

static unsigned char mySlaveAddr;
static unsigned char slaveAddr;

static unsigned int isrEntry = 0;
static unsigned int maxTxLen = 0;

/* ISR declaration */
DECLARE_ISR(i2cmasterSiIsr);

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void setState(i2cState newState)
{
  currentState = newState;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD int sendStartCondition(void)
{
  int retVal = -1;

  if ((currentState == idle) || (currentState == ready)) { 
    I2CONSET = CTRL_SET_START;
    setState(busy);
    retVal = 0;
    }

  return retVal;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void sendStopCondition(void)
{
  I2CONSET = CTRL_SET_STOP;
  setState(idle);
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void waitUntilComplete(void)
{
  while (currentState == busy)
    ;
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void reInitState(void)
{
  setState(idle);
  currentOp = noop;
  currentError = none;
}


static unsigned short tmp;
static unsigned long regData;

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void updateMasterReadState(unsigned int newStatus)
{
  switch(newStatus) {
    case 0x00:
      break;
    case 0x08:
    case 0x10:
      // StartSent: clear start bit and write addr+r to I2DATA
      // RepeatedStartSent: clear start bit and write addr+r to I2DATA
      I2CONCLR = CTRL_CLR_START;
      I2DAT = ((unsigned long)slaveAddr);
      break;
    case 0x38:
      // LostArbitration: return interface to idle state for next xaction
      setState(error);
      currentError = arbLost;
      break;
    case 0x40:
      // AddrSentAck: receive the first data byte
        if (readRequestLen == 1) {
          I2CONCLR = CTRL_CLR_AA;
          }
        else {
          I2CONSET = CTRL_SET_AA;
          }
      break;
    case 0x50:
      // DataReceivedAck: read another data byte
      if ((readRequestLen > 0) && (readLen != CONFIG_I2C_RX_BUFFER_SIZE)) {
        tmp = (unsigned short)((slaveAddr << 8) & 0xff);
        regData = I2DAT;
        tmp |= (regData & 0xff);
        readData[readHead] = tmp;
        circInc(readHead, 0, (CONFIG_I2C_RX_BUFFER_SIZE-1));
        readLen++;
        readRequestLen--;
        if (readRequestLen == 1) {
          I2CONCLR = CTRL_CLR_AA;
          }
        }
      break;
    case 0x48:
      // AddrSentNak: nobody home -- send stop condition and bail 
      I2CONSET = CTRL_SET_STOP;
      setState(error);
      currentError = addr;
      // TODO: reset queue??
      break;
    case 0x58:
      // DataReceivedNak: we should assert a NAK on the last data byte
      // read. Handle this as it is a normal condition if we are finished
      // reading, else indicate an error
      if ((readRequestLen == 1) && (readLen != CONFIG_I2C_RX_BUFFER_SIZE)) {
        tmp = (unsigned short)((slaveAddr << 8) & 0xff);
        regData = I2DAT;
        tmp |= (regData & 0xff);
        readData[readHead] = tmp;
        circInc(readHead, 0, (CONFIG_I2C_RX_BUFFER_SIZE-1));
        readLen++;
        readRequestLen--;
        if (sendStop == true) {
          I2CONSET = CTRL_SET_STOP;
          setState(idle);
          }
        else {
          setState(ready);
          }
        }
      else {
        I2CONSET = CTRL_SET_STOP;
        setState(error);
        currentError = data;
        }
      // TODO: reset queue??
      break;
    case 0xF8:
      // Noop: set interface to idle
      break;
    default:
      break;
    }
}

#ifdef CONFIG_TOOLCHAIN_IAR
#pragma inline
#endif
static INLINE_KEYWORD void updateMasterWriteState(unsigned int newStatus)
{
  switch(newStatus) {
    case 0x08:
    case 0x10:
      // StartSent: clear start bit and write addr+w to I2DATA
      // RepeatedStartSent: clear start bit and write addr+w to I2DATA
      I2CONCLR = CTRL_CLR_START;

#if 0
      if (currentOp == write) {
        slaveAddr = GET_I2C_ADDR_WRITE(writeHead);
        }
#endif

      I2DAT = ((unsigned long)slaveAddr);
      break;
    case 0x18:
    case 0x28:
      // AddrSentAck: write the first data byte
      // DataSentAck: 
      if (currentOp == probe) {
        I2CONSET = CTRL_SET_STOP;
        setState(idle);
        }
      else if ((currentOp == write) && (writeLen > 0)) {
        IOSET = ((unsigned long)0x00000200);
        SEND_I2C_DATA(writeTail);
        circInc(writeTail, 0, (CONFIG_I2C_TX_BUFFER_SIZE-1));
        writeLen--;
        IOCLR = ((unsigned long)0x00000200);
        }
      else {
        // nothing to do: send a stop condition and idle the interface
        if (sendStop == true) {
          I2CONSET = CTRL_SET_STOP;
          setState(idle);
          }
        else {
          setState(ready);
          }
        }
      break;
    case 0x20:
      // AddrSentNak: send stop condition and bail on the xaction
      I2CONSET = CTRL_SET_STOP;
      setState(error);
      currentError = addr;
      break;
    case 0x30:
      // DataSentNak: send stop condition and bail on the xaction
      // TODO: implement retry based on last stored byte value
      I2CONSET = CTRL_SET_STOP;
      setState(error);
      currentError = data;
      break;
    case 0x38:
      // LostArbitration: return interface to idle state for next xaction
      setState(error);
      currentError = arbLost;
      break;
    case 0xF8:
      // Noop: set interface to idle
      break;
    default:
      break;
    }
}

/*
 * SI ISR
 */

static unsigned int pollStatus;

ISR_FUNCTION(i2cmasterSiIsr)
{
  ISR_ENTER()

  isrEntry++;

  GET_STATUS(pollStatus);

  switch(currentOp) {
    case probe:
    case write:
      updateMasterWriteState(pollStatus);
      break;
    case read:
      updateMasterReadState(pollStatus);
      break;
    default:
      // TODO: error handling
      break;
    }

  // acknowledge the interrupt by clearing the SI bit
  I2CONCLR = CTRL_CLR_IRQ;
  
  // acknowledge the interrupt at the VIC
  VICIrqDone;

  ISR_LEAVE()
}

int i2cmasterInit(unsigned char myAddress) 
{
  int retStat;

  mySlaveAddr = myAddress;

  // initialize state
  reInitState();

  // TODO: adjust this based on PLL/peripheral clock
  I2SCLL = (unsigned long)0x8000;
  I2SCLH = (unsigned long)0x8000;

  // enable the I2C interface
  I2CONSET = CTRL_SET_ENABLE;

  // send stop condition to reset the bus to an idle state
  sendStopCondition();

  // connect the interrupt vector
  if ((retStat = armvicIntConnect(2, LPC21XX_IRQVEC_I2C, i2cmasterSiIsr)) != 0)
    return retStat;

  // enable the interrupt
  if ((retStat = armvicIntEnable(LPC21XX_IRQVEC_I2C)) != 0)
    return retStat;

  return 0;
}

int i2cmasterPut(unsigned char putAddr, unsigned char putData)
{
  int retStat = -1;

  if (writeLen != CONFIG_I2C_TX_BUFFER_SIZE) {
    IRQ_DISABLE();
    PUT_DATA_WRITE(writeHead, putAddr, putData);
    writeLen++;
    circInc(writeHead, 0, (CONFIG_I2C_TX_BUFFER_SIZE-1));
    IRQ_ENABLE();
    maxTxLen = ((writeLen > maxTxLen) ? writeLen : maxTxLen);
    retStat = 0;
    }

  return retStat;
}

int i2cmasterGetData(unsigned char *pGetData)
{
  unsigned char tossAddr;
  return i2cmasterGet(&tossAddr, pGetData);
}


// gets transactions that have been completed, possibly filter by address
int i2cmasterGet(unsigned char *pGetAddr, unsigned char *pGetData)
{
  int retStat = -1;
  unsigned short tmpData;

  if (readLen == 0)
    return retStat;
  
  if ((pGetData == NULL) || (pGetAddr == NULL) || (currentState != idle))
    return retStat;

  tmpData = readData[readTail];

  IRQ_DISABLE();
  circInc(readTail, 0, (CONFIG_I2C_RX_BUFFER_SIZE-1));
  readLen--;
  IRQ_ENABLE();

  *pGetAddr = GET_I2C_ADDR_READ(tmpData);
  *pGetData = (unsigned char)(tmpData & 0xff);
  
  retStat = 0;

  return retStat;
}

int i2cmasterProbe(unsigned char probeAddr)
{
  int retVal = -1;

  if ((currentState == busy) || (currentState == error))
    return retVal;

  currentOp = probe;
  slaveAddr = SLAVE_ADDR_WRITE(probeAddr);

  if (sendStartCondition() == 0) {
    waitUntilComplete();
    retVal = ((currentState == error) ? -1 : 0);
    }

  // reset the interface, no stop condition required
  reInitState();

  return retVal;
}

int i2cmasterReceive(unsigned char recvAddr, int maxRecvBytes, boolean recvSendStop)
{
  int retVal = -1;

  if ((currentState == busy) || (currentState == error))
    return retVal;

  currentOp = read;
  slaveAddr = SLAVE_ADDR_READ(recvAddr);
  sendStop = recvSendStop;
  readRequestLen = maxRecvBytes;

  sendStartCondition();

  waitUntilComplete();

  retVal = ((currentState == error) ? -1 : 0);

  reInitState();

  return retVal;
}

int i2cmasterSend(unsigned char sendAddr, boolean sendSendStop)
{
  int retVal = -1;

  if ((currentState == busy) || (currentState == error))
    return retVal;

  currentOp = write;
  slaveAddr = SLAVE_ADDR_WRITE(sendAddr);
  sendStop = sendSendStop;

  sendStartCondition();

  waitUntilComplete();

  retVal = ((currentState == error) ? -1 : 0);
  reInitState();

  return retVal;
}

int i2cmasterSendOne(unsigned char sendAddr, unsigned char sendData, boolean sendSendStop)
{
  int retVal = -1;

  if (i2cmasterPut(sendAddr, sendData) != -1) {
    retVal = i2cmasterSend(sendAddr, sendSendStop);
    }

  return retVal;
}

int i2cmasterSendMult(unsigned char sendAddr, unsigned char * pSendData, int numBytes, boolean sendSendStop)
{
  int i = 0;
  int retVal = 0;

  while ((i < numBytes) && (retVal != -1)) {
    retVal = i2cmasterPut(sendAddr, *(pSendData + i));
    i++;
    }

  if (retVal != -1) {
    retVal = i2cmasterSend(sendAddr, sendSendStop);
    }

  return retVal;
}

#endif /* CONFIG_INCLUDE_LPC210X_I2C */
