#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>

#include "rr_io.h"
#include "gpio.h"

#if 0
void sleep_us(uint32_t us)
{
	struct timeval t1,t2;
	int seconds, useconds;
	int cond_1, cond_2;

	seconds = us / 1000000;
	useconds = us % 1000000;

	gettimeofday(&t1, NULL);
	do {

		gettimeofday(&t2, NULL);

		cond_1 = ((t2.tv_sec - t1.tv_sec) >= seconds);

		cond_2 = (((t2.tv_usec + useconds) % 1000000) - t1.tv_usec) >= 0;

	} while(!(cond_1 && cond_2));

}

#endif

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);
}

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);
}

uint8_t gpio_get(uint32_t addr, uint8_t bit)
{
	return (gennum_readl(addr) & (1 << bit)) ? 1 : 0;
}

void gpio_init(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);
}

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;
	}
}

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;
}

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);
}

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;
}

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;
}

static void wait_completion()
{
	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);

	}
}

static void bulk_erase()
{
	int not_done;
	
	//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, tmp;
	
	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;
}

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;
}

int readback_flash(char *filename, uint32_t size, uint32_t addr) 
{
	uint8_t *data;
    	FILE *f;
	uint32_t i;

	data = malloc(size);

    	
	if(!f) 
		return -1;
	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");
	fwrite(data, 1, size, f);
	fclose(f);
	
	free(data);

	return 0;
}

int force_load_fpga_from_flash ()
{
        gpio_bootselect(FPGA_FLASH);
        gennum_writel(0, SPRI_CONFIG); 
        usleep(100);
        gennum_writel(1, SPRI_CONFIG); 
}
