#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>


/* For memmem() */
#define __USE_GNU 
#include <string.h>

#include <fcntl.h>
#include <assert.h>
#include "peekpoke.h"
#include "load_ts7kv.h"
#include "ts_id.h"

static int ts7kvs_found = 0;	
static int verbose = 0;

void usage(char **argv) {
	fprintf(stderr, "Usage: %s [OPTION] [BITSTREAM] ...\n"
	  "  -0                      Load TS-7KV at loc #0\n"
	  "  -1                      Load TS-7KV at loc #1\n"
	  "  -2                      Load TS-7KV at loc #2\n"
	  "  -3                      Load TS-7KV at loc #3\n"
	  "  -v                      Be verbose\n"
	  "  -h                      This help\n"
	  "\n"
	  "By default, all TS-7KVs found will be loaded\n", argv[0]
	);
}

int find_ts7kvs(unsigned long io_loc)
{
	int i, num = 0;

	for(i = 0; i < 4; i++) 
		if (PEEK16(io_loc + 0xe0 + (8 * i)) == TS7KV_ID) {
			ts7kvs_found |= (1 << i);
			num++;
		}

	if (verbose) {
		printf("ts7kv_numfound=%d\n", num);
		for (i = 0; i < 4; i++) if (ts7kvs_found & (1 << i))
			printf("ts7kv_found_at_loc%d=1\n", i);
	}
	return ts7kvs_found;
}

/* PEEK command in this macro is only to prevent traffic jams. */
/* 		PEEK8(io_loc + 0xe0 + (8 * i));				\  */
/* Okay that wasn't important */

#define POKE8_TS7KV(x, y)	{					\
	int i = 0;							\
	for (i = 0; i < 4; i++) if (ts7kvs_found & (1 << i)) {		\
		POKE8(io_loc + 0xe0 + (8 * i) + (x), (y)); 		\
	}								\
}

void reset_ts7kvs(unsigned long io_loc)
{
	POKE8_TS7KV(CONFIG_REG, 0x2);
	usleep(350);

	POKE8_TS7KV(CONTROL_REG, 0x16);
	usleep(350);
	
	POKE8_TS7KV(CONTROL_REG, 0x1e);
	usleep(350);

	POKE8_TS7KV(CONTROL_REG, 0x16);
	usleep(350);

	POKE8_TS7KV(DATA_REG, 0xff);
	POKE8_TS7KV(DATA_REG, 0xff);
	POKE8_TS7KV(DATA_REG, 0xff);
	POKE8_TS7KV(DATA_REG, 0xff);
}

int load_ts7kvs(unsigned long io_loc, FILE *bitstream)
{
	int dat, len;
	int i = 0;
	unsigned char buffer[129 * 1024];
	unsigned char *start;
	unsigned char magic[4] = {0xaa, 0x99, 0x55, 0x66};
	bzero(buffer, sizeof(buffer));
	while ((dat = fgetc(bitstream)) != EOF && i < sizeof(buffer)) 
		buffer[i++] = dat;
	assert(i != sizeof(buffer));
	start = memmem(buffer, i, magic, sizeof(magic));
	assert(start != NULL);

	len = i - (start - buffer);
	while (len--) 
	{
		//start = start++;
		POKE8_TS7KV(DATA_REG, REVERSEBITS(*start));
		start++;
	}
	
	// Let FPGAs run...
	POKE8_TS7KV(CONFIG_REG, 0x1);

	if (verbose) {
		int numloaded = 0;
		for (i = 0; i < 4; i++) if (ts7kvs_found & (1 << i)) {
			printf("ts7kv_loaded_at_loc%d=1\n", i);
			numloaded++;
		}
		printf("ts7kv_numloaded=%d\n", numloaded);
	}
	
	return 0;
}

#define DIO_REG(x) *(dio_ptr + (x)/sizeof(unsigned int))

/* enable_ISA and disable_ISA are only needed for the 7800 (so far) */
void enable_ISA(int devmem)
{
	volatile unsigned int *dio_ptr;

	dio_ptr = (unsigned int *)mmap(0, getpagesize(), 
		PROT_READ|PROT_WRITE, MAP_SHARED, devmem, 0xe8000000);
	/* These registers must be set to use ISA: */
	DIO_REG(0x30) = 0x55555555;
	DIO_REG(0x34) = 0x55555555;
	DIO_REG(0x38) = 0x55555;
	DIO_REG(0x3C) = 0x55555;
	/* Clear and set ISA parameters: */
	DIO_REG(0x0C) &= ~0x1fff;
	DIO_REG(0x0C) |= 0x181C;
}

void disable_ISA(int devmem)
{
	volatile unsigned int *dio_ptr;

	dio_ptr = (unsigned int *)mmap(0, getpagesize(), 
		PROT_READ|PROT_WRITE, MAP_SHARED, devmem, 0xe8000000);
	DIO_REG(0x30) = 0;
	DIO_REG(0x34) = 0;
	DIO_REG(0x38) = 0;
	DIO_REG(0x3C) = 0;
}

int main(int argc, char *argv[])
{
	unsigned long io_loc;
	unsigned int addr_space = 0;
	unsigned int cpu_id = 0;
	FILE *bitstream;
	int i, ts7kv_mask = 0, ts7kv_mask_set = 0;
	int devmem = open("/dev/mem", O_RDWR|O_SYNC);
	char *optstring = "v0123h";

	while ((i = getopt(argc, argv, optstring)) != -1) {
		switch (i) {
		case '0':
		case '1':
		case '2':
		case '3':
			ts7kv_mask_set = 1;
			ts7kv_mask |= (1 << (i - '0'));
			break;
		case 'v': 
			verbose = 1;
			break;
		default:
			usage(argv);
			return 4;
			break;
		}
	}

	if (optind >= argc) {
		usage(argv);
		return 5;
	} else {
		assert(devmem != -1);
		cpu_id = id_cpu();
		if (cpu_id == ARM920) addr_space = IO_LOC_7200;
		if (cpu_id == ARM926) addr_space = IO_LOC_7800;
		assert(addr_space != 0);
		io_loc = (unsigned long)mmap(0, getpagesize(), 
			PROT_READ|PROT_WRITE, MAP_SHARED, devmem, addr_space);
		assert((void *)io_loc != MAP_FAILED);
		if (cpu_id == ARM926) enable_ISA(devmem);
		if (!find_ts7kvs(io_loc)) {
			if (cpu_id == ARM926) disable_ISA(devmem);
			return 1;
		}
		if (ts7kv_mask_set) ts7kvs_found &= ts7kv_mask;
		if (strcmp(argv[optind], "-") == 0)
			bitstream = stdin;
		else
			bitstream = fopen(argv[optind], "r");
		if(bitstream != NULL) {
			reset_ts7kvs(io_loc);
			load_ts7kvs(io_loc, bitstream);
		} else {
			fprintf(stderr, "%s: can't open %s\n", argv[0], 
			  argv[1]);
			if (cpu_id == ARM926) disable_ISA(devmem);
			return 2;
		}
	}

	for (i = 0; i < 4; i++) if (ts7kvs_found & (1 << i)) {		
		if (!(PEEK8(io_loc + 0xe0 + (8 * i) + CONFIG_REG) & DONE_MSK)) {
			fprintf(stderr, 
			  "%s: FPGA load failure on TS-7KV #%d\n", argv[0], i);
			if (cpu_id == ARM926) disable_ISA(devmem);
			return 3;
		}
	}
	if (cpu_id == ARM926) disable_ISA(devmem);
	return 0;
}