#include "libsys_boot.h"
#include "cfiflash.h"

#define	MAGIC_EB	0x4A464931	// "JFI1" in big-endian;
#define	MAGIC_EL	0x3149464A	// "JFI1" in little-endian;

typedef struct _ssrec_t
{
	UINT32 addr;
	UINT32 size;
	UINT32 type;
	UINT8 *data;

} srec_t;
typedef struct _sflash_alloc_tbl_t
{
	UINT32 images[16];

} flash_alloc_tbl_t;

typedef struct _sflash_img_hdr_t
{
	UINT32 magic;
	UINT32 target_base;
	UINT32 img_base;
	UINT32 img_size;
	UINT32 hdr_next;
	UINT8 img_name[128];
	UINT8 res[108];

} flash_img_hdr_t;

enum srec_type
{
	srec_err, srec_sob, srec_data, srec_rec, srec_eob
};

UINT8 h2i(UINT8 *c)
{
	int i;
	UINT8 t, b = 0;

	for (i=0; i < 2; i++)
	{
		t = c[i];
		if (t >= 'A')
			t = t - 'A' + 10;
		else
			t -= '0';

		b = (b << 4) | t;
	}
	return b;
}

int decode_srec(srec_t *pRec, UINT8 *pBuf, int buflen)
{
	UINT8 chksum, byte;
	int alen, i;

	if (!buflen)
		return srec_err;

	pRec->type = srec_rec;
	alen = 0;
	if ((pBuf[1] > '0') && (pBuf[1] < '4'))
	{
	  alen = pBuf[1] - '0' + 1;
	  pRec->type = srec_data;
	}
	else if ((pBuf[1] >= '7') && (pBuf[1] <= '9'))
	{
	  alen = 11 - (pBuf[1] - '0');
	  pRec->type = srec_eob;
	}
	else if (pBuf[1] == '0')
	{
	  alen = 2;
	  pRec->type = srec_sob;
	}

	byte = h2i(&pBuf[2]);
	pRec->size = byte;
	chksum = byte;

	pRec->addr = 0;
	for (i=0; i < alen; i++)
	{
		byte = h2i(&pBuf[4+2*i]);
        pRec->addr = pRec->addr << 8 | byte;
		chksum += byte;
	}
	pRec->size -= (alen+1);

	pRec->data = (UINT8*) &pBuf[4 + 2*alen];
	for (i=0; i < (int)pRec->size; i++)
	{
		byte = h2i(&pRec->data[2*i]);
		pRec->data[i] = byte;
		chksum += byte;
	}
	chksum = ~chksum;
	byte = h2i(&pRec->data[2*i]);
	if (chksum != byte)
		return 0;

	return pRec->type;
}

int srec_getline(UINT8 *pLine)
{
	char c;
	int i = 0;

	do
	{
		c = readchar();

	} while ((c != 's') && (c != 'S'));

	while(((c != 0x0D) && (c != 0x0A)))
	{
		pLine[i++] = c;
		c = readchar();
	};

	return i;
}

void Jump_to(void *pEntry)
{
	__asm
	(
		".set	noreorder\n"
	);
	__asm
	(
		"jr		%[pEntry]\n"				// jump entry
		:
		: [pEntry] "r" (pEntry)
	);
	__asm
	(
		"nop\n"
	);
	__asm
	(
		".set	reorder\n"
	);
}

void Exec_at(void *pEntry)
{
	__asm
	(
		".set	noreorder\n"
	);

	__asm
	(
		"li		$26, 0x10000000\n"
		"mtc0	$26, $12\n"
	);

	__asm
	(
		"jr		%[pEntry]\n"				// jump entry
		:
		: [pEntry] "r" (pEntry)
	);

	__asm
	(
		"rfe\n"
	);

	__asm
	(
		".set	reorder\n"
	);
}

void Exec_RE_at(void *pEntry)
{
	__asm
	(
		".set	noreorder\n"
	);

	__asm
	(
		"mfc0	$26, $12\n"			// change exception vector
		"li		$27, 0xFFBFFFFF\n"
		"and	$26, $27\n"
		"li		$27, 0x02000000\n"	// Reverse Endianess
		"or		$26, $27\n"
		"mtc0	$26, $12\n"
	);

	__asm
	(
		"jr		%[pEntry]\n"				// jump entry
		:
		: [pEntry] "r" (pEntry)
	);

	__asm
	(
		"rfe\n"
	);

	__asm
	(
		".set	reorder\n"
	);
}

#define	MAX_IMG_SIZE		(1024*1024)	// bytes
#define SDRAM_BASE			0x40000000
#define FLASH_USE_BLOCK		1

int main(int argc, char *argv[])
{
	UINT8 buf[256];
	srec_t srec;
	int srec_state, i;
	UINT32 haddr, result;
	flash_t flash;
	flash_img_hdr_t img_hdr = {0};
	flash_img_hdr_t *pImg_hdr;
	UINT32 img_size, block_sel;

	volatile UINT32 *pLED = (UINT32*)SYS_LED_PORT;
	volatile UINT32 *pBtn = (UINT32*)SYS_GPIO0;
	volatile UINT32 *pDip = (UINT32*)SYS_GPIO1;
	volatile UINT8 *pMem;
	volatile UINT32 *pFlashIO = (UINT32*)SYS_FLASH_IO;		// Flash in uncached area
	volatile UINT32 *pFlashMem = (UINT32*)SYS_FLASH_MEM;	// Flash in cached area
	volatile UINT32 *ram32 = (UINT32*)SDRAM_BASE;
	UINT32 flash_offset;

	*pLED = 0;
	haddr = 0;
	block_sel = 0x7F & (UINT32)*pDip;

	// ----------------------------------------------------------
	if (IS_ERROR(flash_find(&flash, SYS_FLASH_IO)))
	{
		sputs("Cannot find flash device. Exit now!\n\n");
		return 1;
	}

	// Get flash offset by value of DIP-switch
	flash_offset = flash_get_offset_by_blocknum(&flash, block_sel);

	ram32 = (UINT32*)(SDRAM_BASE);
	pFlashIO = (UINT32*)(SYS_FLASH_IO + flash_offset);

	if (!(*pBtn & 8))
	{
		sputs("\n\n");
		pImg_hdr = (flash_img_hdr_t*)pFlashIO;
		if ((pImg_hdr->magic != MAGIC_EL) && (pImg_hdr->magic != MAGIC_EB))
		{
			sputs("\n\n");
			sputs("Invalid image found at ");print_word(flash_offset); sputs(". Abort.\n");
			return 1;
		}

		sputs("Booting application from flash (");print_word(flash_offset);sputs(")...");

		ram32 = (UINT32*)pImg_hdr->target_base;
		img_size = pImg_hdr->img_size;

		// It's safe to use cached Flash,
		// because  D-Cache was invalidated during execution of start code
		pFlashMem = (UINT32*)(SYS_FLASH_MEM + pImg_hdr->img_base);

		for (i=0; i < img_size/4; i++)
			ram32[i] = pFlashMem[i];

		sputs("done\n");

		sputs("Execute at ");print_word(pImg_hdr->target_base); sputs("\n\n");
		Exec_at((void*)ram32);
	}
	else
	{

		sputs("\n\n");

		sputs("Reading HEX-Record from UART..");

		// Erase SDRAM
		for (i=0; i < MAX_IMG_SIZE/4; i++)
			ram32[i] = 0;


		while(1)
		{
			sputs(".");
			while(1)
			{
				result = srec_getline(buf);
				srec_state = decode_srec(&srec, buf, result);
				*pLED = 0;
				switch (srec_state)
				{
					case srec_data:
						if ((srec.addr + srec.size) > haddr)
							haddr = srec.addr + srec.size;

						pMem = (UINT8*)srec.addr;
						for (i=0; i < srec.size; i++)
							*pMem++ = srec.data[i];

						break;

					case srec_eob:
						sputs("done\n\n");
						img_hdr.magic = MAGIC_EB;
						img_hdr.target_base = srec.addr;
						img_hdr.img_size = haddr - srec.addr;
						img_hdr.img_base = flash_offset + sizeof(flash_img_hdr_t);
						img_hdr.hdr_next = flash_get_offset_blockaligned(&flash, img_hdr.img_base + img_hdr.img_size);
						sputs("Size of application    : ");print_word(img_hdr.img_size); sputs("\n");
						sputs("Programming flash at   : ");print_word(flash_offset); sputs("\n");
						sputs("Next free flash offset : ");print_word(img_hdr.hdr_next); sputs("\n");

						sputs("Flash erase...");
						result = flash_erase(&flash, flash_offset, img_hdr.img_size + sizeof(flash_img_hdr_t));
						if (IS_ERROR(result))
						{
							sputs("failed (");print_word(result);sputs(")\n");
							break;
						}
						sputs("done\n");

						sputs("Flash write...");
						result = flash_program(&flash, flash_offset, (UINT8*)&img_hdr, sizeof(flash_img_hdr_t));
						if (IS_ERROR(result))
						{
							sputs("failed (");print_word(result);sputs(")\n");
							break;
						}
						result = flash_program(&flash, img_hdr.img_base, (UINT8*)img_hdr.target_base, img_hdr.img_size);
						if (IS_ERROR(result))
						{
							sputs("failed (");print_word(result);sputs(")\n");
							break;
						}
						sputs("done\n");

						sputs("Flash verify...");
						result = flash_verify(&flash, img_hdr.img_base, (UINT8*)img_hdr.target_base, img_hdr.img_size);
						if (IS_ERROR(result))
						{
							sputs("failed (");print_word(result);sputs(")\n");
							break;
						}
						sputs("passed\n");

						// Jump to reset vector
						Jump_to((void*)0xBFC00000);

						break;

					default:
						result = -1;
						break;
				}
				if (IS_ERROR(result))
				{
					*pLED = 0x40000000;
					break;
				}

			};
		}
	}
	return 0;
}