#include "mts_error_codes.h"
#include "mts_fpga_spi.h"

/* -------------------------------------------------------------------------- */
/* --- PRIVATE MACROS ------------------------------------------------------- */

#define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
#if DEBUG_SPI == 1
#define DEBUG_MSG(str) fprintf(stderr, str)
#define DEBUG_PRINTF(fmt, args...)                                             \
  fprintf(stderr, "%s:%d: " fmt, __FUNCTION__, __LINE__, args)
#define CHECK_NULL(a)                                                          \
  if (a == NULL) {                                                             \
    fprintf(stderr, "%s:%d: ERROR: NULL POINTER AS ARGUMENT\n", __FUNCTION__,  \
            __LINE__);                                                         \
    return MTAC_SPI_ERROR;                                                     \
  }
#else
#define DEBUG_MSG(str)
#define DEBUG_PRINTF(fmt, args...)
#define CHECK_NULL(a)                                                          \
  if (a == NULL) {                                                             \
    return MTAC_SPI_ERROR;                                                     \
  }
#endif

/* -------------------------------------------------------------------------- */
/* --- PRIVATE CONSTANTS ---------------------------------------------------- */

#define READ_ACCESS 0x00
#define WRITE_ACCESS 0x80
#define SPI_SPEED 8000000
/* -------------------------------------------------------------------------- */
/* --- PUBLIC FUNCTIONS DEFINITION ------------------------------------------ */

/* 
  SPI initialization and configuration 
*/
int mtac_spi_open(char *spidev, void **spi_target_ptr) {
  int *spi_device = NULL;
  int dev;
  int a = 0, b = 0;
  int i;

  /* check input variables */
  CHECK_NULL(spi_target_ptr);

  /* allocate memory for the device descriptor */
  spi_device = malloc(sizeof(int));
  if (spi_device == NULL) {
    DEBUG_MSG("ERROR: MALLOC FAIL\n");
    return MTAC_SPI_OPEN_FAILURE;
  }

  /* open SPI device */
  dev = open(spidev, O_RDWR);
  if (dev < 0) {
    DEBUG_PRINTF("ERROR: failed to open SPI device %s\n", spidev);
    return MTAC_SPI_OPEN_FAILURE;
  }

  /* setting SPI mode to 'mode 0' */
  i = SPI_MODE_3;
  a = ioctl(dev, SPI_IOC_WR_MODE, &i);
  b = ioctl(dev, SPI_IOC_RD_MODE, &i);
  if ((a < 0) || (b < 0)) {
    DEBUG_MSG("ERROR: SPI PORT FAIL TO SET IN MODE 0\n");
    close(dev);
    free(spi_device);
    return MTAC_SPI_OPEN_FAILURE;
  }

  /* setting SPI max clk (in Hz) */
  i = SPI_SPEED;
  a = ioctl(dev, SPI_IOC_WR_MAX_SPEED_HZ, &i);
  b = ioctl(dev, SPI_IOC_RD_MAX_SPEED_HZ, &i);
  if ((a < 0) || (b < 0)) {
    DEBUG_MSG("ERROR: SPI PORT FAIL TO SET MAX SPEED\n");
    close(dev);
    free(spi_device);
    return MTAC_SPI_OPEN_FAILURE;
  }

  /* setting SPI to MSB first */
  i = 0;
  a = ioctl(dev, SPI_IOC_WR_LSB_FIRST, &i);
  b = ioctl(dev, SPI_IOC_RD_LSB_FIRST, &i);
  if ((a < 0) || (b < 0)) {
    DEBUG_MSG("ERROR: SPI PORT FAIL TO SET MSB FIRST\n");
    close(dev);
    free(spi_device);
    return MTAC_SPI_OPEN_FAILURE;
  }

  /* setting SPI to 8 bits per word */
  i = 0;
  a = ioctl(dev, SPI_IOC_WR_BITS_PER_WORD, &i);
  b = ioctl(dev, SPI_IOC_RD_BITS_PER_WORD, &i);
  if ((a < 0) || (b < 0)) {
    DEBUG_MSG("ERROR: SPI PORT FAIL TO SET 8 BITS-PER-WORD\n");
    close(dev);
    return MTAC_SPI_OPEN_FAILURE;
  }

  *spi_device = dev;
  *spi_target_ptr = (void *)spi_device;
  return MTAC_SUCCESS;
}

/*
  Check Chip ID
*/
int mtac_id(char *spidev, void *spi_target) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int ret;
  size_t command_size = 6;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = 0x90;
  out_buf[1] = 0x00;
  out_buf[2] = 0x00;
  out_buf[3] = 0x00;
  out_buf[4] = 0x00;
  out_buf[5] = 0x00;

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 0;
  k.bits_per_word = 8;
  ret = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);
  mtac_ret = mtac_disconnect();

  /* determine return code */
  if (ret != (int)k.len) {
    DEBUG_MSG("Chip transfer failed\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  }
  else if (((int)in_buf[4] == 0xFF && (int)in_buf[5] == 0xFF) || ((int)in_buf[4] == 0 && (int)in_buf[5] == 0)) {
    printf("Unable to communicate with flash\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  }
  else {
    printf("Manufacturer ID: %x %x",in_buf[4], in_buf[5]);
    mtac_ret = MTAC_SUCCESS;
  }
  return mtac_ret;
}

/*
  Chip erase instruction sets all memory within
  the device to the erased state of all 1s (FFh)
*/
int chip_erase(char *spidev, void *spi_target) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int ret;
  size_t command_size = 1;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = CHIP_ERASE;

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 0;
  k.bits_per_word = 8;
  ret = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);
  sleep(5);
  mtac_ret = mtac_disconnect();

  /* determine return code */
  if (ret != (int)k.len) {
    DEBUG_MSG("Chip Erase transfer failed\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  } else {
    DEBUG_MSG("Chip Erase transfer successful\n");
    mtac_erase_verify(spidev, spi_target);
    mtac_ret = MTAC_SUCCESS;
  }
  return mtac_ret;
}

/*
  Verify that the chip erase was successful
*/
int mtac_erase_verify(char *spidev, void *spi_target) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int a, ret;
  uint16_t command_size = 256 + 5;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = 0x0B;
  out_buf[1] = 0x00;
  out_buf[2] = 0x00;
  out_buf[3] = 0x00;
  out_buf[4] = 0x00;

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 1;
  k.bits_per_word = 8;
  ret = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);

  /* determine return code */
  if (ret != (int)k.len) {
    DEBUG_MSG("Chip transfer failed\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  } else {
    DEBUG_MSG("Chip transfer successful\n");
    mtac_ret = MTAC_SUCCESS;
  }
  mtac_ret = mtac_disconnect();
  /* verify that the chip was erased */
  for (a = 5; a < command_size; a++) {
    if (in_buf[a] != 0xFF) {
      mtac_disconnect();
      mtac_ret = MTAC_CHIP_ERASE_FAILURE;
      return mtac_ret;
    }
  }
  return mtac_ret;
}

/*
  Write enable instruction sets the write enable latch
  in the status register to 1. It needs to be set before
  a page can be programmed or chip erased
*/
int write_enable(char *spidev, void *spi_target) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int ret;
  size_t command_size = 1;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = 0x06;

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 0;
  k.bits_per_word = 8;
  ret = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);
  mtac_ret = mtac_disconnect();

  /* determine return code */
  if (ret != (int)k.len) {
    DEBUG_MSG("Write Enable failed");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  } else {
    usleep(1000);
    DEBUG_MSG("Write Enable successful");
    mtac_ret = MTAC_SUCCESS;
  }
  return mtac_ret;
}

/*
  Release Power-down instruction releases the device from
  said state allowing device communication
*/
int mtac_release(char *spidev, void *spi_target) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int a, ret;
  size_t command_size = 5;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = CHIP_RELEASE;
  out_buf[1] = 0;
  out_buf[2] = 0;
  out_buf[3] = 0;
  out_buf[4] = 0;

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 1;
  k.bits_per_word = 8;
  ret = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);
  mtac_ret = mtac_disconnect();

  /* determine return code */
  if (ret != (int)k.len) {
    DEBUG_MSG("\nRelease failed\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  } else {
    usleep(1000);
    DEBUG_MSG("\nRelease successful\n");
    mtac_ret = MTAC_SUCCESS;
  }
  return mtac_ret;
}

/*
  Page Program instruction allows writing upto 256 bytes (a page) of
  data to be programmed at previously erased memory locations Write
  enable must be issued first
*/
int page_program(char *spidev, void *spi_target, uint8_t adr_lower, uint8_t adr_higher,
                 int data[256]) {
  int mtac_ret;
  int spi_device;
  struct spi_ioc_transfer k;
  int a, ret, h;
  size_t command_size = 260;
  char out_buf[command_size];
  char in_buf[command_size];

  /* prepare frame to be sent */
  out_buf[0] = PAGE_PROGRAM;
  out_buf[1] = adr_higher;
  out_buf[2] = adr_lower;
  out_buf[3] = 0x00;
  for (h = 0; h < 256; h++) {
    out_buf[h + 4] = data[h];
  }

  /* I/O transaction */
  mtac_ret = mtac_spi_open(spidev, &spi_target);
  spi_device = *(int *)spi_target;        /* spi_target must not be null beforehand */
  memset(&k, 0, sizeof(k));               /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 1;
  k.bits_per_word = 8;
  a = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);
  printf("Writing Page %x%x to MTAC\r", adr_higher, adr_lower);
  usleep(10000);
  for (ret = 0; ret < command_size; ret++) {
    DEBUG_PRINTF("%x ", out_buf[ret]);
  }
  mtac_ret = mtac_disconnect();

  /* determine return code */
  if (a != (int)k.len) {
    DEBUG_MSG("ERROR: SPI WRITE FAILURE\n");
    mtac_ret = MTAC_SPI_TRANSFER_FAILURE;
  } else {
    DEBUG_MSG("Note: SPI write success\n");
    mtac_ret = MTAC_SUCCESS;
  }
  return mtac_ret;
}

/* Simple spi write to fpga*/
int mtac_spi_w(void *spi_target, uint8_t spi_mux_mode, uint8_t spi_mux_target,
               uint8_t address, uint8_t data) {
  int spi_device;
  uint8_t out_buf[3];
  uint8_t command_size;
  struct spi_ioc_transfer k;
  int a;

  /* check input variables */
  CHECK_NULL(spi_target);
  if ((address & 0x80) != 0) {
    DEBUG_MSG("WARNING: SPI address > 127\n");
  }

  spi_device = *(int *)spi_target; /* spi_target must not be null beforehand */

  /* prepare frame to be sent */
  if (spi_mux_mode == MTAC_SPI_MUX_MODE1) {
    out_buf[0] = spi_mux_target;
    out_buf[1] = WRITE_ACCESS | (address & 0x7F);
    out_buf[2] = data;
    command_size = 3;
  } else {
    out_buf[0] = WRITE_ACCESS | (address & 0x7F);
    out_buf[1] = data;
    command_size = 2;
  }

  /* I/O transaction */
  memset(&k, 0, sizeof(k)); /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.len = command_size;
  k.speed_hz = SPI_SPEED;
  k.cs_change = 1;
  k.bits_per_word = 8;
  a = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);

  /* determine return code */
  if (a != (int)k.len) {
    DEBUG_MSG("ERROR: SPI WRITE FAILURE\n");
    return MTAC_SPI_TRANSFER_FAILURE;
  } else {
    DEBUG_MSG("Note: SPI write success\n");
    return MTAC_SUCCESS;
  }
}

/* Simple spi read to fpga */
int mtac_spi_r(void *spi_target, uint8_t spi_mux_mode, uint8_t spi_mux_target,
               uint8_t address, uint8_t *data) {
  int spi_device;
  uint8_t out_buf[3];
  uint8_t in_buf[ARRAY_SIZE(out_buf)];
  uint8_t command_size;
  struct spi_ioc_transfer k;
  int a;

  /* check input variables */
  CHECK_NULL(spi_target);
  if ((address & 0x80) != 0) {
    DEBUG_MSG("WARNING: SPI address > 127\n");
  }
  CHECK_NULL(data);

  spi_device = *(int *)spi_target; /* spi_target cannot be null beforehand */

  /* prepare frame to be sent */
  if (spi_mux_mode == MTAC_SPI_MUX_MODE1) {
    out_buf[0] = spi_mux_target;
    out_buf[1] = READ_ACCESS | (address & 0x7F);
    out_buf[2] = 0x00;
    command_size = 3;
  } else {
    out_buf[0] = READ_ACCESS | (address & 0x7F);
    out_buf[1] = 0x00;
    command_size = 2;
  }

  /* I/O transaction */
  memset(&k, 0, sizeof(k)); /* clear k */
  k.tx_buf = (unsigned long)out_buf;
  k.rx_buf = (unsigned long)in_buf;
  k.len = command_size;
  k.cs_change = 1;
  a = ioctl(spi_device, SPI_IOC_MESSAGE(1), &k);

  /* determine return code */
  if (a != (int)k.len) {
    DEBUG_MSG("ERROR: SPI READ FAILURE\n");
    return MTAC_SPI_TRANSFER_FAILURE;
  } else {
    DEBUG_MSG("Note: SPI read success\n");
    *data = in_buf[command_size - 1];
    return MTAC_SUCCESS;
  }
}

/* 
  SPI release 
*/
int mtac_spi_close(void *spi_target) {
  int spi_device;
  int a;

  /* check input variables */
  CHECK_NULL(spi_target);

  /* close file & deallocate file descriptor */
  spi_device = *(int *)spi_target; /* check that spi_target is not null */
  a = close(spi_device);
  free(spi_target);

  /* determine return code */
  if (a < 0) {
    DEBUG_MSG("ERROR: SPI PORT FAILED TO CLOSE\n");
    return MTAC_SPI_CLOSE_FAILURE;
  } else {
    DEBUG_MSG("Note: SPI port closed\n");
    return MTAC_SUCCESS;
  }
}