#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <sys/time.h>

#include "rrlib.h"
#include "rr_loader_lib.h"

static int fd;

/* Initialise file descriptor for the library*/
int rr_init(int a_fd)
{
        if(!a_fd) return -1;
	fd = a_fd;
        return 0;
}

/* 32-bit write to BAR0 (=> FPGA) */
void rr_writel(uint32_t data, uint32_t addr)
{
	struct rr_iocmd iocmd;
	iocmd.datasize = 4;
	iocmd.address = addr;
	iocmd.address |= __RR_SET_BAR(0);
	iocmd.data32 = data;
	rr_iwrite(fd, &iocmd);
}

/* 32-bit read from BAR0 (=> FPGA) */
uint32_t rr_readl(uint32_t addr)
{
	struct rr_iocmd iocmd;
	iocmd.datasize = 4;
	iocmd.address = addr;
	iocmd.address |= __RR_SET_BAR(0);
	rr_iread(fd, &iocmd);
	return iocmd.data32;
}

/* 32-bit write to BAR4 (=> Gennum config space) */
void gennum_writel(uint32_t data, uint32_t addr)
{
	struct rr_iocmd iocmd;
	iocmd.datasize = 4;
	iocmd.address = addr;
	iocmd.address |= __RR_SET_BAR(4);
	iocmd.data32 = data;
	rr_iwrite(fd ,&iocmd);
}

/* 32-bit read from BAR4 (=> Gennum config space) */
uint32_t gennum_readl(uint32_t addr)
{
	struct rr_iocmd iocmd;
	iocmd.datasize = 4;
	iocmd.address = addr;
	iocmd.address |= __RR_SET_BAR(4);
	rr_iread(fd, &iocmd);
	return iocmd.data32;
}

/* Gets current time (in tics) */
static inline int64_t get_tics()
{
    struct timezone tz= {0,0};
    struct timeval tv;
    gettimeofday(&tv, &tz);
    return (int64_t)tv.tv_sec * 1000000LL + (int64_t) tv.tv_usec;
}

/*
 * Load FPGA bitstream from a buffer using the Gennum FCL
 * (= FPGA Configuration Loader)
 * Note that the SPEC must be configured in "GENNUM to FPGA" mode
 */
int rr_load_bitstream(const void *data, int size8)
{
	int i, ctrl, done = 0, wrote = 0;
	//unsigned long j;
	uint8_t val8;
	const uint8_t *data8 = data;
	const uint32_t *data32 = data;
	int size32 = (size8 + 3) >> 2;

        //fprintf(stderr,"Loading %d bytes...\n", size8);

	if (1) {
		/*
		 * Hmmm.... revers bits for xilinx images?
		 * We can't do in kernel space anyways, as the pages are RO
		 */
                uint8_t *d8 = (uint8_t *)data8; /* Horrible: kill const */
		for (i = 0; i < size8; i++) {
			val8 = d8[i];
			d8[i] = 0
				| ((val8 & 0x80) >> 7)
				| ((val8 & 0x40) >> 5)
				| ((val8 & 0x20) >> 3)
				| ((val8 & 0x10) >> 1)
				| ((val8 & 0x08) << 1)
				| ((val8 & 0x04) << 3)
				| ((val8 & 0x02) << 5)
				| ((val8 & 0x01) << 7);
		}
	}

	/* Do real stuff */
	gennum_writel(0x00, FCL_CLK_DIV);
	gennum_writel(0x40, FCL_CTRL); /* Reset */
	i = gennum_readl(FCL_CTRL);
	if (i != 0x40) {
		fprintf(stderr, "%s: %i: error\n", __func__, __LINE__);
		return -1;
	}
	gennum_writel(0x00, FCL_CTRL);

	gennum_writel(0x00, FCL_IRQ); /* clear pending irq */

	switch(size8 & 3) {
        case 3: ctrl = 0x116; break;
	case 2: ctrl = 0x126; break;
	case 1: ctrl = 0x136; break;
	case 0: ctrl = 0x106; break;
	}
	gennum_writel(ctrl, FCL_CTRL);

	gennum_writel(0x00, FCL_CLK_DIV); /* again? maybe 1 or 2? */

	gennum_writel(0x00, FCL_TIMER_CTRL); /* "disable FCL timer func" */

	gennum_writel(0x10, FCL_TIMER_0); /* "pulse width" */
	gennum_writel(0x00, FCL_TIMER_1);

	/* Set delay before data and clock is applied by FCL after SPRI_STATUS is
		detected being assert.
	*/
	gennum_writel(0x08, FCL_TIMER2_0); /* "delay before data/clock..." */
	gennum_writel(0x00, FCL_TIMER2_1);
	gennum_writel(0x17, FCL_EN); /* "output enable" */

	ctrl |= 0x01; /* "start FSM configuration" */
	gennum_writel(ctrl, FCL_CTRL);

	while(size32 > 0){
		/* Check to see if FPGA configuation has error */
		i = gennum_readl(FCL_IRQ);
		if ( (i & 8) && wrote) {
			done = 1;
                        //fprintf(stderr,"%s: %idone after %i\n", __func__, __LINE__, wrote);
		} else if ( (i & 0x4) && !done) {
			fprintf(stderr,"Error after %i\n", wrote);
			return -1;
		}
                //fprintf(stderr,".");

		while(gennum_readl(FCL_IRQ) & (1<<5)); // wait until at least 1/2 of the FIFO is empty

		/* Write 64 dwords into FIFO at a time. */
		for (i = 0; size32 && i < 32; i++) {
			gennum_writel(*data32, FCL_FIFO);
			data32++; size32--; wrote++;
                        //udelay(20);
		}
	}

	gennum_writel(0x186, FCL_CTRL); /* "last data written" */

	int64_t tstart = get_tics();
	//j = jiffies + 2 * HZ;

	/* Wait for DONE interrupt  */
	while(!done) {
                //fprintf(stderr,stderr, "Wait!");
		i = gennum_readl(FCL_IRQ);
		if (i & 0x8) {
                        //fprintf(stderr,"done after %i\n", wrote);
			done = 1;
		} else if( (i & 0x4) && !done) {
			fprintf(stderr,"Error after %i\n",   wrote);
			return -1;
		}
		usleep(10000);

		if(get_tics() - tstart > 1000000LL)
		{
                        fprintf(stderr,"Loader: DONE timeout. Did you choose proper bitgen options?\n");
                        return -1;
		}
		/*if (time_after(jiffies, j)) {
			printk("%s: %i: tout after %i\n", __func__, __LINE__,
			       wrote);
			return;
		} */
	}
	return done?0:-1;
}

/*
 * Load FPGA bitstream from a file using the Gennum FCL
 * (= FPGA Configuration Loader)
 * Note that the SPEC must be configured in "GENNUM to FPGA" mode
 */
int rr_load_bitstream_from_file(const char *file_name)
{
        uint8_t *buf;
        FILE *f;
        uint32_t size;

        f=fopen(file_name,"rb");
        if(!f) return -1;
        fseek(f, 0, SEEK_END);
        size = ftell(f);
        buf = malloc(size);
        if(!buf)
        {
                fclose(f);
                return -1;
        }
        fseek(f, 0, SEEK_SET);
        fread(buf, 1, size, f);
        fclose(f);
        int rval = rr_load_bitstream(buf, size);
        free(buf);
        return rval;
}

/* Set Gennum GPIO to '1' */
void gpio_set1(uint32_t addr, uint8_t bit)
{
	uint32_t reg;

	reg = gennum_readl(addr);
	//printf("register:%.8X ", reg);
	reg |= (1 << bit);
	//printf("SET  :%.8X(%.2d):%.8X\n", addr, bit, reg);
	gennum_writel(reg, addr);
}

/* Set Gennum GPIO to '0' */
void gpio_set0(uint32_t addr, uint8_t bit)
{
	uint32_t reg;

	reg = gennum_readl(addr);
	//printf("register:%.8X ", reg);
	reg &= ~(1 << bit);
	//printf("CLEAR:%.8X(%.2d):%.8X\n", addr, bit, reg);
	gennum_writel(reg, addr);
}

/* Get Gennum GPIO state*/
uint8_t gpio_get(uint32_t addr, uint8_t bit)
{
	return (gennum_readl(addr) & (1 << bit)) ? 1 : 0;
}

/* Configures Gennum GPIO */
void gpio_config(void)
{
	gennum_writel(0x00000000, FCL_CTRL); // FCL mode
	gennum_writel(0x00000017, FCL_EN); // FCL output enable
	gennum_writel(0x00000000, FCL_IODATA_OUT); // FCL outputs to 0

	gennum_writel(0x00002000, GPIO_DIRECTION_MODE); // GPIO direction (1=input)
	gennum_writel(0x0000D000, GPIO_OUTPUT_ENABLE); // GPIO output enable
	gennum_writel(0x00000000, GPIO_OUTPUT_VALUE); // GPIO output to 0
	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
}

/*
 * Selects SPEC boot mode
 *
 * 1 = Gennum to flash
 * 2 = Gennum to FPGA
 * 3 = Flash to FPGA
 */
void gpio_bootselect(uint8_t select)
{
	switch(select){

	case GENNUM_FLASH:
		gpio_set0(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL0);
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL1);
		break;

	case GENNUM_FPGA:
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL0);
		gpio_set0(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL1);
		break;

	case FPGA_FLASH:
		gennum_writel(0x00000000, FCL_EN);// FCL output all disabled
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL0);
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_BOOTSEL1);
		break;

	default:
		break;
	}
}

/* Bit bang SPI read byte */
static uint8_t spi_read8(void)
{
	uint8_t rx;
	int i;

	gpio_set0(FCL_IODATA_OUT, SPRI_CLKOUT);
	for(i = 0; i < 8;i++){
                //usleep(SPI_DELAY);
		rx <<= 1;
		if (gpio_get(GPIO_INPUT_VALUE, GPIO_SPRI_DIN))
			rx |= 1;
		//usleep(SPI_DELAY);
		gpio_set1(FCL_IODATA_OUT, SPRI_CLKOUT);
                //usleep(SPI_DELAY);
		gpio_set0(FCL_IODATA_OUT, SPRI_CLKOUT);
	}
        //usleep(SPI_DELAY);
	return rx;
}

/* Bit bang SPI write byte */
static void spi_write8(uint8_t tx)
{
	int i;

	gpio_set0(FCL_IODATA_OUT, SPRI_CLKOUT);
	for(i = 0; i < 8;i++){
		//usleep(SPI_DELAY);
		if(tx & 0x80)
			gpio_set1(FCL_IODATA_OUT, SPRI_DATAOUT);
		else
			gpio_set0(FCL_IODATA_OUT, SPRI_DATAOUT);
		tx<<=1;
                //usleep(SPI_DELAY);
		gpio_set1(FCL_IODATA_OUT, SPRI_CLKOUT);
                //usleep(SPI_DELAY);
		gpio_set0(FCL_IODATA_OUT, SPRI_CLKOUT);
	}
        //usleep(SPI_DELAY);
}

/* Reads Flash status byte */
uint8_t flash_read_status(void)
{
	uint8_t val;

	gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	usleep(SPI_DELAY);

	spi_write8(FLASH_RDSR);
	val = spi_read8();

	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	usleep(SPI_DELAY);

	return val;
}

/* Reads Flash ID */
uint32_t flash_read_id(void)
{
	uint32_t val=0;

	gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	usleep(SPI_DELAY);

	spi_write8(FLASH_RDID);
	val = (spi_read8() << 16);
	val += (spi_read8() << 8);
	val += spi_read8();

	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	usleep(SPI_DELAY);

	return val;
}

/* Waits for Flash bulk erase to finish */
static void wait_completion(void)
{
	int not_done;

	/* Wait completion of the Bulk erase Operation */
	while(not_done) {
		gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
		spi_write8(FLASH_RDSR); /* Read Status register */

		not_done = (spi_read8() & 0x01);

		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
		//usleep(SPI_DELAY);
	}
}

/* Flash bulk erase */
static void bulk_erase(void)
{
	//printf("Erasing flash memory.....");

	gpio_bootselect(GENNUM_FLASH);

	gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	spi_write8(FLASH_WREN); /* Write Enable */
	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	usleep(SPI_DELAY);

	gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	spi_write8(FLASH_BE); /* Bulk erase */
	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);

	wait_completion();
	//printf("OK\n");

}

static int __do_load_mcs_to_flash(const uint8_t *data, uint32_t size)
{
	int i, j;
	int limit = 0;
	int num_pages = 0;
	unsigned int addr = 0;
	int not_done;

	bulk_erase();

	/* Round up if the size is not a multiply of a page (256 bytes) */
	num_pages = size / 256 + !!(size % 256);

	gpio_bootselect(GENNUM_FLASH);

	/* Select the FLASH using Chip Select signal */
        //printf("Programming....\n");
	for (j = 0; j < num_pages; j ++) {
		if (j == (num_pages-1))
			limit = size%256;
		else
			limit = 256;

		gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
		spi_write8(FLASH_WREN); /* Write Enable */
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
		usleep(SPI_DELAY);
		gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
		spi_write8(FLASH_PP); /* Page Program */

		spi_write8((addr >> 16) & 0x00ff); /* Address to start writing (MSB)*/
		spi_write8((addr >> 8) & 0x00ff); /* Address to start writing */
		spi_write8(addr & 0x00ff); /* Address to start writing (LSB) */
		for (i=0; i < limit; i++) {
			spi_write8(data[j*256 + i]);
			//printf("Data[%d] = 0x%x\n", i+j*256, data[i]);
		}
		gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);

		addr += 256;
		not_done = 1;
                //usleep(SPI_DELAY);

		wait_completion();
                //printf("[%d] ", j);
	}
	//printf("\nFinished\n");
	return 0;
}

/*
 * Loads .mcs file into Flash
 * Note that the SPEC must be configured in "GENNUM to Flash" mode
 */
int load_mcs_to_flash(char * filename)
{
        uint8_t *buf;
        FILE *f;
        uint32_t size;

        f=fopen(filename,"rb");
        if(!f) return -1;
        fseek(f, 0, SEEK_END);
        size = ftell(f);

        //printf("Filename: %s. Size: %d bytes\n", filename, size);

        buf = malloc(size);
        if(!buf){
                fclose(f);
                return -1;
        }
        fseek(f, 0, SEEK_SET);
        fread(buf, 1, size, f);
        fclose(f);
        int rval = __do_load_mcs_to_flash(buf, size);
        free(buf);
        return rval;
}

/* Readback data from Flash and write them to a file */
int readback_flash(char *filename, uint32_t size, uint32_t addr)
{
	uint8_t *data;
    	FILE *f;
	uint32_t i;

	data = malloc(size);

	gpio_bootselect(GENNUM_FLASH);

	gpio_set0(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);
	spi_write8(FLASH_READ); /* Read Data bytes */

	spi_write8((addr >> 16) & 0x00ff); /* Address to start writing (MSB)*/
	spi_write8((addr >> 8) & 0x00ff); /* Address to start writing */
	spi_write8(addr & 0x00ff); /* Address to start writing (LSB) */

	for (i=0; i < size; i++) {
		spi_write8(data[i]);
		//printf("Data[%d] = %d\n", i, data[i]);
	}
	gpio_set1(GPIO_OUTPUT_VALUE, GPIO_FLASH_CS);

    	f=fopen(filename,"wb");
	if(!f) return -1;
	fwrite(data, 1, size, f);
	fclose(f);
	free(data);
	return 0;
}

/*
 * Reload FPGA from Flash
 * Note the the SPEC must be in "Flash to FPGA" mode
 */
int force_load_fpga_from_flash ()
{
        gpio_bootselect(FPGA_FLASH);
        gennum_writel(0, SPRI_CONFIG);
        usleep(100);
        gennum_writel(1, SPRI_CONFIG);
        return 0;
}
