#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <errno.h>
#include <string.h>

#include "osal/osal.h"
#include "osal/regs_sysconfig.h"


#define FPGA_BASE_ADDR          (0x66000000) /* ARM side interface, CS5 */
#define GPIO_REG_BASE           (0x01E26000)


typedef struct GpioBankPair
{
	volatile uint32_t DIR;
	volatile uint32_t OUT_DATA;
	volatile uint32_t SET_DATA;
	volatile uint32_t CLR_DATA;
	volatile uint32_t IN_DATA;
	volatile uint32_t SET_RIS_TRIG;
	volatile uint32_t CLR_RIS_TRIG;
	volatile uint32_t SET_FAL_TRIG;
	volatile uint32_t CLR_FAL_TRIG;
	volatile uint32_t INTSTAT;
} tsGpioBankPair;

typedef struct GpioRegs
{
	volatile uint32_t REVID;
	volatile uint32_t reserved1;
	volatile uint32_t BINTEN;
	volatile uint32_t reserved2;
	tsGpioBankPair    BANKPAIR[4];
} tsGpioRegs;

static tsGpioRegs *mpGpioRegs;
static const unsigned int gnNUMBANKS = 8;
static const unsigned int gnIOPERBANK = 16;

// GPIO Access Functions
static void GPIO_SetPinValue(unsigned int Bank, unsigned int Offset)
{
  int bankpair = Bank >> 1;
  int shift = (Bank&1)*16+Offset;

  mpGpioRegs->BANKPAIR[bankpair].SET_DATA = 1<<shift;
}


static void GPIO_ClearPinValue(unsigned int Bank, unsigned int Offset)
{
  int bankpair = Bank >> 1;
  int shift = (Bank&1)*16+Offset;

  mpGpioRegs->BANKPAIR[bankpair].CLR_DATA = 1<<shift;
}

static int GPIO_GetPinValue(unsigned int Bank, unsigned int Offset)
{
  int bankpair = Bank>>1;
  int shift = (Bank&1)*16+Offset;
  return (mpGpioRegs->BANKPAIR[bankpair].IN_DATA & (1<<shift)) ? 1 : 0;
}

static void GPIO_SetPinDirection(unsigned int Bank, unsigned int Offset, int IsOutput, unsigned int Value)
{
  int bankpair = Bank >> 1;
  int shift = (Bank&1)*16+Offset;

//  ASSERT(Bank < 8);
//  ASSERT(Offset < 16);

  if (IsOutput)
  {
    if (Value)
      GPIO_SetPinValue(Bank,Offset);
    else
      GPIO_ClearPinValue(Bank,Offset);
    mpGpioRegs->BANKPAIR[bankpair].DIR &= ~(1<<shift);
  }
  else
  {
    mpGpioRegs->BANKPAIR[bankpair].DIR |= (1<<shift);
  }
}


static void FPGA_ResetEnable(void)
{
  GPIO_ClearPinValue(6, 15);
}

static void FPGA_ResetDisable(void)
{
  GPIO_SetPinValue(6, 15);
}

static void FPGA_WriteEnable(void)
{
  GPIO_ClearPinValue(3, 9);
}

static void FPGA_WriteDisable(void)
{
  GPIO_SetPinValue(3, 9);
}

static int FPGA_InitResponse(void)
{
  return (GPIO_GetPinValue(3, 9));
}

static void GPIO_Init(void)
{
  // Setup GPIO pin directions
  GPIO_SetPinDirection(6, 15, True,  1); // output: PROGRAM_B (/resetout GP6[15])
  GPIO_SetPinDirection(3,  9, True,  1); // output: RDWR_B    (/EMA_RW   GP3[9] )
  GPIO_SetPinDirection(1, 15, False, 0); // input:  INIT_B    (init      GP1[15])
}



// FPGA Load Routine
// returns 0 on success
int32_t loadFpga (const char_t *filename, bool reset_only=false, const char_t * board="")
{
  FILE *file;
  uint8_t *filedata;
  long filesize, i;
  uint8_t *fpgamem;
  bool_t boBitSwap;
  sysconfig_regs_t *mpSysRegs;
  uint32_t w32;

  // Open memory device (O_SYNC option disables cache)
  int fd;
  fd = open("/dev/mem", O_RDWR | O_SYNC);
  if (fd < 0)
  {
    printf("Failed to open /dev/mem\n");
    return -1;
  }

  // Get access to GPIO memory area (MMU) - getpagesize()
  mpGpioRegs = (tsGpioRegs *) mmap(0, sizeof(tsGpioRegs), PROT_READ|PROT_WRITE, MAP_SHARED, fd, GPIO_REG_BASE);
  if (mpGpioRegs == NULL)
  {
    printf("mmap Failed for mpGpioRegs\n");
    close(fd);
    return -1;
  }

  // Get access to FPGA memory address space for programming
  fpgamem = (uint8_t *) mmap(0, getpagesize(), PROT_READ|PROT_WRITE, MAP_SHARED, fd, FPGA_BASE_ADDR);
  if (fpgamem == NULL)
  {
    printf("mmap Failed for fpgamem\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    close(fd);
    return -1;
  }


  // Get access to the syscfg registers
//  mpSysRegs = (sysconfig_regs_t *) mmap(0, sizeof(sysconfig_regs_t), PROT_READ|PROT_WRITE, MAP_SHARED, fd, SYSCFG0_REG_BASE);
  // Only read access (write access to the sys cfg registers is only possible from kernel space - by chip design)
  mpSysRegs = (sysconfig_regs_t *) mmap(0, sizeof(sysconfig_regs_t), PROT_READ, MAP_SHARED, fd, SYSCFG0_REG_BASE);
  if (mpSysRegs == NULL)
  {
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    printf("mmap Failed for mpSysRegs\n");
    close(fd);
    return -1;
  }

  // Check pin mux configuration for EMA_A_Rw must be GPIO to enable fpga load
  w32 = mpSysRegs->PINMUX[7] & 0x0F000000;
  if (w32 != 0x08000000)
  {
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    munmap(mpSysRegs, sizeof(sysconfig_regs_t));
    printf("Pin EMA_A_Rw not configured as GPIO, fpga will not be loaded\n");
    printf("pinmux7: %08X\n", w32);
    close(fd);
    return -1;
  }

#if 0
  printf("PINMUX[7] = 0x%08X\n", w32);
  w32 &= 0xF0FFFFFF;  // Set EMA_A_Rw pin to work as GPIO required for fpga loading (else used as GPIO in fpga loader on ARM!)
  w32 |= 0x08000000;
  mpSysRegs->PINMUX[7] = w32;
  printf("PINMUX[7] = 0x%08X\n", w32);
  printf("PINMUX[7] = 0x%08X\n", mpSysRegs->PINMUX[7]);
#endif

  // Unmap access to SYS CONFIG registers
  munmap(mpSysRegs, sizeof(sysconfig_regs_t));

  // Done using the filedescriptor
  close(fd);

  if (reset_only)
  {
    GPIO_Init();
    FPGA_ResetEnable();
    munmap(fpgamem, getpagesize());
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    return 0;
  }

  // Open fpga file
  file = fopen(filename, "r");
  if (file == NULL)
  {
    printf("Failed to open fpga file\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (errno);
  }

  //Extract header
  long offset = 0;
  if(strlen(board) != 0)
  {
    char header_buf[50]; // max header size is 50 char
    int header_size = 0;
    while(header_size <= 50)
    {
      header_buf[header_size] = fgetc(file);
      if(header_buf[header_size] == ':')
      {
        header_size ++;
        break;
      }
      header_size ++;
    }
    header_buf[header_size]='\0';
    offset =  (long)header_size;
    printf("header: '%s'\n",header_buf);

    // Extract version from header
    char * version = strtok(header_buf,"v=");
    // Remove end marker ':' from version string
    version[strlen(version)-1] = '\0';

    printf("FPGA version: %s\n",version);

    // Compare board version and FPGA version
    if(strcmp(board,version) != 0)
    {
      printf("Error: FPGA version doesn't match Board \"%s\" != \"%s\"\n",version, board);
      munmap(mpGpioRegs, sizeof(tsGpioRegs));
      munmap(fpgamem, getpagesize());
      return (-1);
    }
  }

  // return -1; //Debug dont load new fpga


  // Get file size
  fseek(file, 0L, SEEK_END);
  filesize = ftell(file);
  printf("filesize %ld\n",filesize);
  filesize = filesize - offset;
  if (filesize < 0)
  {
    printf("Error reading fpga filesize\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (errno);
  }

  // Verify max filesize
  if (filesize > (4 * 1024 * 1024))
  {
    printf("Error too large fpga filesize, max. 4MB\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (-1);
  }

  // Verify in filesize
  if (filesize < (200 * 1024))
  {
    printf("FPGA Bin file too small\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (-1);
  }

  // Allocate memory for file
  filedata = (uint8_t*) malloc(filesize);
  if (filedata == NULL)
  {
    printf("Error allocating memory for fpga file\n");
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (-1);
  }

  // Read fpga file
  fseek(file, offset, SEEK_SET);
  if (fread(filedata, 1, filesize, file) != (size_t) filesize)
  {
    printf("Error reading fpga file\n");
    free(filedata);
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (-1);
  }

  // Close file
  fclose(file);

  // Initialize fpga interface gpios
  GPIO_Init();

  // Reset fpga TPROGRAM low pulse time, must be above 500ns, hmmm we are way over the top here
  FPGA_ResetEnable();
  OSAL_TaskSleep(OSAL_SEC_TO_TICK(0.01));

  // Stop resetting
  FPGA_ResetDisable();
  OSAL_TaskSleep(OSAL_SEC_TO_TICK(0.01));

  // Verify reset OK
  if (FPGA_InitResponse() == 0)
  {
    printf("FPGA reset timeout\n");
    free(filedata);
    munmap(mpGpioRegs, sizeof(tsGpioRegs));
    munmap(fpgamem, getpagesize());
    return (-1);
  }

  // Read 32bit pattern 55 99 AA 66, if AA 99 55 66 we need to bit swap, else abort
  switch (*((uint32_t*) &filedata[0x10]))
  {
    case 0x665599AA:   // actually => 0xAA995566
      boBitSwap = True;
      break;

    // Already swapped
    case 0x66AA9955:   // actually => 0x5599AA66
    default:
      boBitSwap = False;
      break;
  }

  // Enable writing program to fpga
  FPGA_WriteEnable();
  OSAL_TaskSleep(OSAL_SEC_TO_TICK(0.01));

  // Send program to fpga
  for (i = 0; i < filesize; i++)
  {
     byte_t bIn = filedata[i];
     byte_t bOut = 0;
     int bit;

     if (boBitSwap)
     {
       for (bit = 0; bit <= 7; bit++)
       {
         bOut <<= 1;
         bOut |= (bIn & 0x01);
         bIn >>= 1;
       }
     }
     else
     {
       bOut = bIn;
     }
     *fpgamem = bOut;
  }

  // Done programming
  FPGA_WriteDisable();
  OSAL_TaskSleep(OSAL_SEC_TO_TICK(0.01));

  // Verify programming done (CRC) ok
  if (FPGA_InitResponse() == 0)
  {
    printf("FPGA programming failed\n");
    free(filedata);
    return (-1);
  }

  // Cleanup
  munmap(fpgamem, getpagesize());
  munmap(mpGpioRegs, sizeof(tsGpioRegs));
  free(filedata);

  return 0;
}