#include "mcom02.h"
#include "i2c.h"

void init_I2C(unsigned int slave_addr) {
  volatile unsigned int temp;

  GPIO0(SWPORTA_CTL) |=
		  GPIOA29_I2C0_SDA | GPIOA30_I2C0_SCL; // Swith GPIO to I2C mode
  I2C0(IC_ENABLE) = 0;                     // Disable i2c
  I2C0(IC_INTR_MASK) = 0;                  // Mask interrupts
  temp = I2C0(IC_CLR_INTR);                // Clear interrupts
  (void)temp;
  I2C0(IC_CON) = 1 | (0x1 << 1) | (0x0 << 3) | (1 << 5) |
                 (1 << 6); // Master mode, standart speed, 7 bit addr, restart
                           // enable, disable slave.
  I2C0(IC_TAR) = (slave_addr & 0x3FF) | (0x0 << 10) |
                 (0x0 << 12); // Set slave addr, TX mode, 7 bit addr
  I2C0(IC_RX_TL) = 256;       // RX fifo size
  I2C0(IC_TX_TL) = 256;       // TX fifi size
  // I2C0 (IC_INTR_MASK) = 0xfff;
  I2C0(IC_ENABLE) = 1; // Enable i2c
}

int readI2CBytes(unsigned int adrSize, unsigned int regNum, void *dest,
                 int size) {
  int i;
  unsigned char *p;
  regNum &= 0xFF;

  if (dest == 0 || size < 1) {
    return 0;
  }
  // Big Endian to Little Endian
  p = (unsigned char *)dest + (size - 1);

  // Send register number
  for (i = adrSize - 8; i >= 0; i -= 8) {
    I2C0(IC_DATA_CMD) = ((regNum >> i) & 0xFF) | (0 << 8);
  }
  // Wait sending
  while (!(I2C0(IC_STATUS) & (1 << 2)))
    ;
  // Read answer
  for (i = 0; i < size && i < 256; i++) {
    I2C0(IC_DATA_CMD) = (1 << 8);
  }
  size = i;
  // Wait data

  while (I2C0(IC_RXFLR) < size && (I2C0(IC_STATUS) & 0x1 != 0))
    ;

  for (i = 0; i < size; i++) {
    *p-- = (I2C0(IC_DATA_CMD) & 0xFF);
    // Check valid read
    if ((I2C0(IC_RAW_INTR_STAT) & 0x1) == 0x1) {
      return i;
    }
  }

  return size;
}

int writeI2CBytes(unsigned int adrSize, unsigned int regNum, void *src,
                  int size) {
  int i;
  unsigned char *p = (unsigned char *)src;
  regNum &= 0xFF;

  if (src == 0 || size < 1) {
    return 0;
  }
  // Big Endian to Little Endian
  p = (unsigned char *)src + (size - 1);

  // Send register number
  for (i = adrSize - 8; i >= 0; i -= 8) {
    I2C0(IC_DATA_CMD) = ((regNum >> i) & 0xFF) | (0 << 8);
  }

  // Send register value
  for (i = 0; i < size && i < 256; i++) {
    I2C0(IC_DATA_CMD) = *p-- | (0 << 8);
  }
  // Wait sending
  while (!(I2C0(IC_STATUS) & (1 << 2)))
    ;

  return i;
}

unsigned char readI2CRegByte(unsigned int adrSize, unsigned int regNum) {
  unsigned char val = 0;
  readI2CBytes(adrSize, regNum, &val, sizeof(char));
  return val;
}
unsigned short readI2CRegShort(unsigned int adrSize, unsigned int regNum) {
  unsigned short val = 0;
  readI2CBytes(adrSize, regNum, &val, sizeof(short));
  return val;
}
unsigned int readI2CRegInt(unsigned int adrSize, unsigned int regNum) {
  unsigned int val = 0;
  readI2CBytes(adrSize, regNum, &val, sizeof(int));
  return val;
}
unsigned long long readI2CRegLongLong(unsigned int adrSize,
                                      unsigned int regNum) {
  unsigned long long val = 0;
  readI2CBytes(adrSize, regNum, &val, sizeof(long long));
  return val;
}
int writeI2CRegByte(unsigned int adrSize, unsigned int regNum,
                    unsigned char regVal) {
  unsigned char val = regVal;
  return writeI2CBytes(adrSize, regNum, &val, sizeof(char));
}
int writeI2CRegShort(unsigned int adrSize, unsigned int regNum,
                     unsigned short regVal) {
  unsigned short val = regVal;
  return writeI2CBytes(adrSize, regNum, &val, sizeof(short));
}
int writeI2CRegInt(unsigned int adrSize, unsigned int regNum,
                   unsigned int regVal) {
  unsigned int val = regVal;
  return writeI2CBytes(adrSize, regNum, &val, sizeof(int));
}
int writeI2CRegLongLong(unsigned int adrSize, unsigned int regNum,
                        unsigned long long regVal) {
  unsigned long long val = regVal;
  return writeI2CBytes(adrSize, regNum, &val, sizeof(long long));
}