/*
 * Copyright
 * (c) 2010 Eukrea Electromatique, Eric Bénard <eric@eukrea.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; version 2 of
 * the License.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 * MA 02111-1307 USA
 * 
 * v0.1 - 11-03-09 - initial release to barebox users
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <pthread.h>
#include <errno.h>
#include <sys/stat.h>
#include <arpa/inet.h>

#define BLACK		0
#define RED		1
#define GREEN		2
#define BROWN		3
#define BLUE		4
#define MAGENTA		5
#define CYAN		6
#define GRAY		7
#define BLACK2		8
#define WHITE		9
#define FG_COLOR	BLACK
#define BG_COLOR	WHITE

#define LOG_DEBUG	1
#define LOG_INFO	1
#define LOG_DATA	1
#define LOG_ERROR	1
#define LOG_CMD		1

#define setcolor(fg,bg)		printf("\033[%d;%dm", fg +30, bg+40)

#ifdef LOG_DEBUG
#define DBG(fmt...) \
	do{	setcolor(RED,BLACK); \
		printf(fmt); \
		setcolor(FG_COLOR,BG_COLOR); \
		fflush(stdout); \
	} while(0)
#else
#define DBG(fmt...) while (0) {};
#endif

#ifdef LOG_INFO
#define INFO(fmt...) \
	do{	setcolor(GREEN,BLACK); \
		printf(fmt); \
		setcolor(FG_COLOR,BG_COLOR); \
		fflush(stdout); \
	} while(0)
#else
#define INFO(fmt...) while (0) {};
#endif

#ifdef LOG_ERROR
#define ERROR(fmt...) \
	do{	setcolor(BLACK,RED); \
		printf(fmt); \
		setcolor(FG_COLOR,BG_COLOR); \
		fflush(stdout); \
	} while(0)
#else
#define ERROR(fmt...) while (0) {};
#endif

#ifdef LOG_DATA
#define DATA(fmt...) \
	do{	setcolor(MAGENTA,BLACK); \
		printf(fmt); \
		setcolor(FG_COLOR,BG_COLOR); \
		fflush(stdout); \
	} while(0)
#else
#define DATA(fmt...) while (0) {};
#endif

#ifdef LOG_CMD
#define CMD(a) \
	do{	int tmp; \
		setcolor(BLUE,BLACK); \
		for (tmp = 0; tmp < sizeof(a); tmp++) \
		printf("%02x ", *(unsigned char *)((void *)& a +tmp)); \
		printf("\n"); \
		setcolor(FG_COLOR,BG_COLOR); \
		fflush(stdout); \
	} while(0)
#else
#define CMD(fmt) while (0) {};
#endif

#define DEFAULT_SPEED	B115200
struct termios sauvegarde;

#define B	0x08
#define HW	0x10
#define W	0x20

#define TIMEOUT	5

int init_UART(char * port, int speed) {
	struct termios configuration;
	int fd;

	if (! port) {
		DBG("\n%s: error, no port!", __FUNCTION__);
		return -1;
	}
	if ((fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY )) < 0) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	};
	fcntl(fd, F_SETFL, 0);
	INFO("UART opened on %s ....\n", port);
	tcgetattr(fd, &configuration);
	INFO("serial open - speed : %08x\n", speed);
	tcgetattr(fd, &configuration);
	bzero(&configuration, sizeof(configuration));
	cfsetispeed(&configuration, speed);
	cfsetospeed(&configuration, speed);
	configuration.c_cflag |= (CLOCAL | CREAD);
	configuration.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
	configuration.c_oflag &= ~OPOST;
	configuration.c_cc[VMIN] = 0;
	configuration.c_cc[VTIME] = 1;
	configuration.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
	configuration.c_cflag |= CS8;
	configuration.c_cflag &= ~CRTSCTS;
	configuration.c_iflag &= ~(IXON | IXOFF | IXANY);

	if (tcsetattr(fd, TCSANOW, &configuration) < 0) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	INFO("%s:Serial port configured.\n", __FUNCTION__);
	return fd;
}

struct command_t {
	unsigned short cmd;
	unsigned int a;
	unsigned char ds;
	unsigned int c;
	unsigned int d;
	unsigned char ft;
} __attribute__((__packed__));


int close_UART(int fd) {
	if (tcsetattr(fd, TCSANOW, &sauvegarde) < 0) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	close(fd);
	return 0;
}

int get_sync(int fd) {
	struct command_t getstatus;
	unsigned int tmp;

	memset(&getstatus, 0, sizeof(getstatus));
	getstatus.cmd = 0x0505;
	CMD(getstatus);

	tmp = write(fd, &getstatus, sizeof(getstatus));
	if (tmp != sizeof(getstatus)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}

	return 0;;
}

int get_status(int fd) {
	struct command_t getstatus;
	unsigned char answer[4];
	unsigned int tmp;
	int timeout = 0;

	memset(&getstatus, 0, sizeof(getstatus));
	getstatus.cmd = 0x0505;
	CMD(getstatus);

	tmp = write(fd, &getstatus, sizeof(getstatus));
	if (tmp != sizeof(getstatus)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	tmp = 0;
	while (tmp < 4) {
		tmp += read(fd, answer + tmp, 1);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes\n");
	DATA("Status : %x %x %x %x\n", answer[0], answer[1], answer[2], answer[3]);

	return answer[0];
}

int write_memory(int fd, unsigned int a, unsigned char ds, unsigned int d) {
	struct command_t writemem;
	unsigned int answer;
	int tmp;
	int timeout = 0;

	tcflush(fd, TCIOFLUSH);
	memset(&writemem, 0, sizeof(writemem));
	writemem.cmd = 0x0202;
	writemem.a = htonl(a);
	writemem.ds = ds;
	writemem.d = htonl(d);
	CMD(writemem);

	tmp = write(fd, &writemem, sizeof(writemem));
	if (tmp != sizeof(writemem)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	tmp = 0;
	while (tmp < 4) {
		tmp += read(fd, (unsigned char *) &answer + tmp, 1);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes but : %i\n", tmp);
	if (answer != 0x56787856) {
		ERROR("bad answer : 0x%08x\n", answer);
		return -1;
	}
	DATA("Answer : %08x\n", answer);
	tmp = 0;
	timeout = 0;
	while (tmp < 4) {
		tmp += read(fd, (unsigned char *) &answer + tmp, 1);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes but : %i\n", tmp);
	if (answer != 0x128a8a12) {
		ERROR("bad answer : 0x%08x\n", answer);
		return -1;
	}
	DATA("Answer : %08x\n", answer);

	return answer;
}

int read_memory(int fd, unsigned int a, unsigned int ds, unsigned int c, void * data) {
	struct command_t writemem;
	unsigned int answer;
	int tmp;
	int timeout = 0;

	memset(&writemem, 0, sizeof(writemem));
	writemem.cmd = 0x0101;
	writemem.a = htonl(a);
	writemem.ds = ds;
	writemem.c = htonl(c);
	CMD(writemem);

	tmp = write(fd, &writemem, sizeof(writemem));
	if (tmp != sizeof(writemem)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		free(data);
		return -1;
	}
	tmp = 0;
	while (tmp < 4) {
		tmp += read(fd, (unsigned char *) &answer + tmp, 1);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes but : %i\n", tmp);
	if (answer != 0x56787856) {
		ERROR("bad answer : 0x%08x\n", answer);
		free(data);
		return -1;
	}
	DATA("Answer : %08x\n", answer);
	tmp = 0;
	timeout = 0;
	while (tmp < (c * (ds >> 3))) {
		tmp += read(fd, data + tmp, 16);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != c*(ds >> 3))
		DBG("error didn't receive %i bytes\n", ds*c);
	DATA("Read : ");
	for (tmp = 0; tmp < c*(ds >>3); tmp+=(ds >> 3)) {
		DATA("0x");
		switch (ds) {
			case B :
				DATA("%02x", (* (unsigned int *) (data + tmp)));
				break;
			case HW :
				DATA("%04x", (* (unsigned short *) (data + tmp)));
				break;
			case W :
				DATA("%08x", (* (unsigned int *) (data + tmp)));
				break;
			default :
				break;
			}
		DATA(" ");
	}
	DATA("\n");

	return answer;
}

int read_reg32(int fd, unsigned int a) {
	unsigned int reg;
	read_memory(fd, a, W, 1, &reg);
	return reg;
}

int read_reg16(int fd, unsigned int a) {
	unsigned short reg;
	read_memory(fd, a, HW, 1, &reg);
	return reg;
}

int read_reg8(int fd, unsigned int a) {
	unsigned char reg;
	read_memory(fd, a, B, 1, &reg);
	return reg;
}

#define SET_REG32(reg, value) \
	write_memory(fd, reg, W, value);
#define SET_REG16(reg, value) \
	write_memory(fd, reg, HW, value);
#define SET_REG8(reg, value) \
	write_memory(fd, reg, B, value);

#define GET_REG32(reg)  \
	read_reg32(fd, reg);
#define GET_REG16(reg)  \
	read_reg16(fd, reg);
#define GET_REG8(reg)  \
	read_reg8(fd, reg);

int write_file(int fd, unsigned int a, char * file) {
	struct command_t writemem;
	unsigned int answer;
	int tmp;
	struct stat status;
	FILE * bin;
	int size;
	int timeout = 0;

	tcflush(fd, TCIOFLUSH);
	if (stat(file, &status) < 0) {
		ERROR("file : %s - %i %s\n", file, errno, strerror(errno));
		return -1;
	}
	size = status.st_size;
	INFO("file size : %i\n", size);
	bin = fopen(file, "rb");
	memset(&writemem, 0, sizeof(writemem));
	writemem.cmd = 0x0404;
	writemem.a = htonl(a);
	writemem.c = htonl(size);
	writemem.ft = 0xAA;
	CMD(writemem);

	tcflush(fd, TCIOFLUSH);
	tmp = write(fd, &writemem, sizeof(writemem));
	if (tmp != sizeof(writemem)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	tmp = 0;
	while (tmp < 4) {
		tmp += read(fd, (unsigned char *) &answer + tmp, 1);
		if (timeout++ > TIMEOUT)
			return -1;
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes but : %i\n", tmp);
	if (answer != 0x56787856) {
		ERROR("bad answer : 0x%08x\n", answer);
		return -1;
	}
	DATA("Answer : %08x\n", answer);
	tcflush(fd, TCIOFLUSH);
	tmp = 0;
	while (tmp < size) {
		char data;
		tmp += fread(&data, sizeof(char), 1, bin);
		if (write(fd, &data, 1) != 1)
			printf("Error writing file !\n");
	}
	INFO("file %s uploaded to 0x%08x\n", file, a);
	fclose(bin);
	return answer;
}

int exec_file(int fd, unsigned int a) {
	struct command_t writemem;
	unsigned int answer;
	int tmp;

	memset(&writemem, 0, sizeof(writemem));
	writemem.cmd = 0x0404;
	writemem.a = htonl(a);
	writemem.ds = 0;
	writemem.c = 4;
	writemem.d = htonl(a);
	writemem.ft = 0xAA;
	CMD(writemem);

	tmp = write(fd, &writemem, sizeof(writemem));
	if (tmp != sizeof(writemem)) {
		DBG("error %i - %s\n", errno, strerror(errno));
		return -1;
	}
	tmp = 0;
	while (tmp < 4) {
		tmp += read(fd, (unsigned char *) &answer + tmp, 1);
	}
	if (tmp != 4)
		DBG("error didn't receive 4 bytes but : %i\n", tmp);
	if (answer != 0x56787856) {
		ERROR("bad answer : 0x%08x\n", answer);
		return -1;
	}
	DATA("Answer : %08x\n", answer);
	return answer;
}

int main(int argc, char *argv[]) {
	int fd;
	unsigned int tmp;
	FILE * conf_file;
	char * line = NULL;
	size_t length = 0;
	char binfile[256];
	int load_adr; 
	int exec_adr;
	struct stat status;
	fd_set mux;
	struct timeval timeout;

	if (argc != 3) {
		printf("Usage: %s uart_device config_file.cfg\n", argv[0]);
		return 1;
	}

	conf_file = fopen(argv[2], "r");
	if (conf_file == NULL) {
		ERROR("Can't open file %s\n", argv[2]);
		return 1;
	}
	fd = init_UART(argv[1], DEFAULT_SPEED);
	if (fd < 0) {
		fclose(conf_file);
		return 1;
	}

	if (get_status(fd) < 0) {
		fclose(conf_file);
		close_UART(fd);
		setcolor(FG_COLOR,BG_COLOR);
		return 1;
	};

	if (getline(&line, &length, conf_file) != -1) {
		if (sscanf(line, "file %s load 0x%x exec 0x%x", binfile, &load_adr, &exec_adr) != 3) {
			DBG("sscan failed with line : %s\n", line);
			ERROR("Bad config file format");
			if (line)
				free(line);
			fclose(conf_file);
			close_UART(fd);
			setcolor(FG_COLOR,BG_COLOR);
			return 1;
		} else {
			DBG("file : %s @ load : %x - exec : %x\n", binfile, load_adr, exec_adr);
			if (stat(binfile, &status) < 0) {
				ERROR("Can't open file %s\n", binfile);
				if (line)
					free(line);
				fclose(conf_file);
				close_UART(fd);
				setcolor(FG_COLOR,BG_COLOR);
				return -1;
			}
		}
		if (line)
			free(line);
		line = NULL;
	} else {
		ERROR("Can't read data from config file\n");
		fclose(conf_file);
		close_UART(fd);
		setcolor(FG_COLOR,BG_COLOR);
		return 1;
	}
	while (getline(&line, &length, conf_file) != -1) {
		char type[256];
		char size[256];
		int adr; 
		int val;
		DBG("line : %s\n", line);
		if (sscanf(line, "%s %s 0x%x %x", type, size, &adr, &val) != 4) {
			DBG("sscan failed with line : %s\n", line);
		} else {
			int ret;
			DBG("@ %x - %x - %s - %s\n", adr, val, size, type);
			if ((strcmp("wr", size) == 0) | (strcmp("write32", size) == 0) | (strcmp("write", size) == 0)) {
				ret = SET_REG32(adr, val);
			} else if ((strcmp("wr16", size) == 0) | (strcmp("write16", size) == 0)) {
				ret = SET_REG16(adr, val);
			} else if ((strcmp("wr8", size) == 0) | (strcmp("write8", size) == 0)) {
				ret = SET_REG8(adr, val);
			}
			if ((strcmp("rd", size) == 0) | (strcmp("read32", size) == 0) | (strcmp("read", size) == 0)) {
				ret = GET_REG32(adr);
			} else if ((strcmp("rd16", size) == 0) | (strcmp("read16", size) == 0)) {
				ret = GET_REG16(adr);
			} else if ((strcmp("rd8", size) == 0) | (strcmp("read8", size) == 0)) {
				ret = GET_REG8(adr);
			}
			if (ret < 0) {
				fclose(conf_file);
				close_UART(fd);
				setcolor(FG_COLOR,BG_COLOR);
				return -1;
			}
		}
		if (line)
			free(line);
		line = NULL;
		length = 0;
	}
	if (line)
		free(line);

	if (write_file(fd, load_adr, binfile) < 0) {
		fclose(conf_file);
		close_UART(fd);
		setcolor(FG_COLOR,BG_COLOR);
		return -1;
	}

	/* the bootrom expects a "app_code_jump_vector" at the
	 * base address of the flash header so make him happy */
	INFO("Now executing @ 0x%x08x\n", exec_adr);
	tmp = SET_REG32(load_adr, exec_adr);
	if (tmp < 0) {
		fclose(conf_file);
		close_UART(fd);
		setcolor(FG_COLOR,BG_COLOR);
		return -1;
	}

	get_sync(fd);

	printf("%i %i\n", fd, fileno(stdin));
	while (1) {
		char out[256];
		int tmp;
		FD_ZERO(&mux);
		FD_SET(fd, &mux);
		FD_SET(0, &mux);
		timeout.tv_sec = 0;
		timeout.tv_usec = 1000;
		if ((tmp = select(FD_SETSIZE, &mux, NULL, NULL, &timeout)) < 0) {
			DBG("select error %i - %s\n", errno, strerror(errno));
		}
		if (FD_ISSET(fd, &mux)) {
			memset(out, 0, 256);
			tmp = read(fd, out, 256);
			printf("%s", out);
			fflush(stdout);
		}
		if (FD_ISSET(0, &mux)) {
			int i = 0;
			memset(out, 0, 256);
			tmp = read(0, out, 256);
			while (i < tmp)
				i += write(fd, out + i, tmp -i);
		}
	}
	close_UART(fd);
	setcolor(FG_COLOR,BG_COLOR);
}