#include "main.h"
#include "uart.h"
#include "flash.h"
#include "frser.h"
#include "udelay.h"

/* Flashrom serial interface AVR implementation */


const char pgmname[16] = "ATMega168 SFR"; /* An ATMega168 with ShiFt Registers */


#define S_ACK 0x10
#define S_NAK 0xBA
/* The biggest ok parameter length - currently held by the Read n bytes command */
#define S_MAXLEN 6
/* The biggest valid command value */
#define S_MAXCMD 0x0D
/* The length of the operation buffer */
#define S_OPBUFLEN 700

unsigned char opbuf[S_OPBUFLEN];
unsigned int opbuf_bytes = 0;
unsigned char opbuf_PrevOpIsWrite = 0;
unsigned int opbuf_prevWriteLen;
						/* The commands :				*/
#define S_CMD_Q_IFACE		0x00		/* Query interface version			*/
#define S_CMD_Q_PGMNAME		0x01		/* Query programmer name			*/
#define S_CMD_NOPACK 		0x02		/* No operation					*/
#define S_CMD_Q_SERBUF		0x03		/* Query Serial Buffer Size			*/
#define S_CMD_Q_BUSTYPE		0x04		/* Query supported bustypes			*/
#define S_CMD_Q_CHIPSIZE	0x05		/* Query supported chipsize (2^n format)	*/
#define S_CMD_Q_OPBUF		0x06		/* Query operation buffer size			*/
#define S_CMD_R_BYTE		0x07		/* Read a single byte				*/
#define S_CMD_R_NBYTES		0x08		/* Read n bytes					*/
#define S_CMD_O_INIT		0x09		/* Initialize operation buffer			*/
#define S_CMD_O_WBA		0x0A		/* Write opbuf: Write byte with address		*/
#define S_CMD_O_WBC		0x0B		/* Write opbuf: Write byte (addr=previous+1)	*/
#define S_CMD_O_DELAY		0x0C		/* Write opbuf: udelay				*/
#define S_CMD_O_EXEC		0x0D		/* Execute operation buffer			*/

#define OPBUF_WRITEOP 0x00
#define OPBUF_DELAYOP 0x01

static unsigned char opbuf_addbyte(unsigned char c) {
	if (opbuf_bytes == S_OPBUFLEN) return 1;
	opbuf[opbuf_bytes++] = c;
	return 0;
}

static unsigned long int buf2u24(unsigned char*buf) {
	unsigned long int u24;
	u24  = (((unsigned long int)buf[0])<< 0);
	u24 |= (((unsigned long int)buf[1])<< 8);
	u24 |= (((unsigned long int)buf[2])<<16);
	return u24;
}

static void do_cmd_readbyte(unsigned char* parbuf) {
	unsigned char c;
	unsigned long int addr;
	addr = buf2u24(parbuf);
	c = flash_readcycle_single(addr);
	SEND(S_ACK);
	SEND(c);
}

static void do_cmd_readnbytes(unsigned char* parbuf) {
	unsigned long int i,addr,n;
	addr = buf2u24(parbuf);
	n = buf2u24(parbuf+3);
	flash_read_init();
	SEND(S_ACK);
	for(i=addr;i<(addr+n);i++) {
		unsigned char c;
		c = flash_readcycle(i);
		SEND(c);
	}
	flash_output_disable();
}

static void do_cmd_opbuf_wba(unsigned char* parbuf) {
	if (opbuf_addbyte(OPBUF_WRITEOP)) goto nakret;
	opbuf_prevWriteLen = opbuf_bytes; // here is where the len is
	if (opbuf_addbyte(1)) goto nakret;
	if (opbuf_addbyte(parbuf[0])) goto nakret;
	if (opbuf_addbyte(parbuf[1])) goto nakret;
	if (opbuf_addbyte(parbuf[2])) goto nakret;
	if (opbuf_addbyte(parbuf[3])) goto nakret;
	opbuf_PrevOpIsWrite = 1;
	SEND(S_ACK);
	return;
nakret:
	SEND(S_NAK);
	return;
}


static void do_cmd_opbuf_wbc(unsigned char* parbuf) {
	unsigned char len;
	if (!opbuf_PrevOpIsWrite) goto nakret;
	len = opbuf[opbuf_prevWriteLen];
	if (len == 0) { /* Create a new cmd then */
		unsigned long int newaddr;
		unsigned char fakeparbuf[4];
		newaddr = buf2u24(opbuf+opbuf_prevWriteLen+1);
		newaddr += 256;
		fakeparbuf[0] = ((newaddr>> 0)&0xFF);
		fakeparbuf[1] = ((newaddr>> 8)&0xFF);
		fakeparbuf[2] = ((newaddr>>16)&0xFF);
		fakeparbuf[3] = parbuf[0];
		do_cmd_opbuf_wba(fakeparbuf);
		return;
	}
	if (opbuf_addbyte(parbuf[0])) goto nakret;
	len++;
	opbuf[opbuf_prevWriteLen] = len;
	SEND(S_ACK);
	return;
nakret:
	SEND(S_NAK);
	return;
}

static void do_cmd_opbuf_delay(unsigned char* parbuf) {
	opbuf_PrevOpIsWrite = 0;
	if (opbuf_addbyte(OPBUF_DELAYOP)) goto nakret;
	if (opbuf_addbyte(parbuf[0])) goto nakret;
	if (opbuf_addbyte(parbuf[1])) goto nakret;
	if (opbuf_addbyte(parbuf[2])) goto nakret;
	if (opbuf_addbyte(parbuf[3])) goto nakret;
	SEND(S_ACK);
	return;
nakret:
	SEND(S_NAK);
	return;
	}
	
static void do_cmd_opbuf_exec(void) {
	unsigned int readptr;
	for(readptr=0;readptr<opbuf_bytes;) {
		unsigned char op;
		op = opbuf[readptr++];
		if (readptr >= opbuf_bytes) goto nakret;
		if (op == OPBUF_WRITEOP) {
			unsigned long int addr;
			unsigned char len,i;
			len = opbuf[readptr++];
			if (readptr >= opbuf_bytes) goto nakret;
			addr = buf2u24(opbuf+readptr);
			readptr += 3;
			if (readptr >= opbuf_bytes) goto nakret;
			for(i=0;;) {
				unsigned char c;
				c = opbuf[readptr++];
				if (readptr > opbuf_bytes) goto nakret;
				flash_writecycle(addr,c);
				addr++;
				i++;
				if (i==len) break;
			}
			continue;
		}
		if (op == OPBUF_DELAYOP) {
			unsigned long int usecs;
			usecs  = (((unsigned long int)(opbuf[readptr++])) << 0);
			usecs |= (((unsigned long int)(opbuf[readptr++])) << 8);
			usecs |= (((unsigned long int)(opbuf[readptr++])) << 16);
			usecs |= (((unsigned long int)(opbuf[readptr++])) << 24);
			if (readptr > opbuf_bytes) goto nakret;
			udelay(usecs);
			continue;
		}
		goto nakret;
	}
	opbuf_bytes = 0;
	opbuf_PrevOpIsWrite = 0;
	SEND(S_ACK);
	return;
nakret:
	opbuf_bytes = 0;
	opbuf_PrevOpIsWrite = 0;
	SEND(S_NAK);
	return;
}
	
const unsigned char op2len[S_MAXCMD+1] = { /* A table to verify that the length of a received op is correct */
		0x00, 0x00, 0x00, 0x00, 0x00,	/* Query,Query,NOPACK,Query,Query */
		0x00, 0x00, 0x03, 0x06, 0x00,	/* Query, Query, readb, readn, init opbuf */
		0x04, 0x01, 0x04, 0x00		/* op buf wba, wbc, delay, exec */
	};
	

void frser_main(void) {
	for(;;) {
		unsigned char parbuf[S_MAXLEN]; /* Parameter buffer */
		unsigned char len;
		unsigned char op;
		unsigned char i;
		len = RECEIVE();
		op = RECEIVE();
		if ((len > S_MAXLEN)||((len)&&(op > S_MAXCMD))) {
			do { RECEIVE(); } while(--len);
			SEND(S_NAK);
			continue;
		}
		if (op > S_MAXCMD) {
			SEND(S_NAK);
			continue;
		}
		for (i=0;i<len;i++) parbuf[i] = RECEIVE();
		if (len != op2len[op]) {
			SEND(S_NAK);
			continue;
		}
		switch (op) {
			case S_CMD_Q_IFACE:
				SEND(S_ACK);
				SEND(0x01);
				SEND(0x00);
				break;
			case S_CMD_Q_PGMNAME:
				SEND(S_ACK);
				for(i=0;i<16;i++) SEND(pgmname[i]);
				break;
			case S_CMD_NOPACK:
				SEND(S_ACK);
				break;
			case S_CMD_Q_SERBUF:
				SEND(S_ACK);
				SEND(UART_BUFLEN&0xFF);
				SEND((UART_BUFLEN>>8)&0xFF);
				break;
			case S_CMD_Q_BUSTYPE:
				SEND(S_ACK);
				SEND(0x01); /* Only parallel */
				break;
			case S_CMD_Q_CHIPSIZE: /* 256k */
				SEND(S_ACK);
				SEND(18);
				break;
			case S_CMD_Q_OPBUF:
				SEND(S_ACK);
				SEND(S_OPBUFLEN&0xFF);
				SEND((S_OPBUFLEN>>8)&0xFF);
				break;
			case S_CMD_R_BYTE:
				do_cmd_readbyte(parbuf);
				break;
			case S_CMD_R_NBYTES:
				do_cmd_readnbytes(parbuf);
				break;
			case S_CMD_O_INIT:
				opbuf_bytes = 0;
				opbuf_PrevOpIsWrite = 0;
				SEND(S_ACK);
				break;
			case S_CMD_O_WBA:
				do_cmd_opbuf_wba(parbuf);
				break;
			case S_CMD_O_WBC:
				do_cmd_opbuf_wbc(parbuf);
				break;
			case S_CMD_O_DELAY:
				do_cmd_opbuf_delay(parbuf);
				break;
			case S_CMD_O_EXEC:
				do_cmd_opbuf_exec();
				break;
		}
	}
}