#include <board.h>
#include "libsys.h"
#include "srec.h"

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

extern uint32_t getBootOffset(uint32_t sel);

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

} flash_img_hdr_t;

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

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

	__asm
	(
		// Enable User Mode and Interrupts on RFE
		// Exception Vector = 0xBFC00080
		"li		$26, 0x10000008\n"
		"mtc0	$26, $12\n"
	);

	__asm
	(
		"mtc0	%[pEntry], $14\n"
		"nop\n"
		"jr		%[pEntry]\n"				// jump entry
		"rfe\n"
		:
		: [pEntry] "r" (pEntry)
	);

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

#define	MAX_IMG_SIZE		(1024*1024)	// bytes

int main(int argc, char *argv[])
{
	uint8_t buf[256];
	srec_t srec;
	int result, srec_state, i;
	uint32_t haddr = 0;
	volatile uint8_t *pMem;

	flash_img_hdr_t img_hdr;

	volatile uint32_t *pFlashMem = (uint32_t*)(FLASH_BASE_MEM);	// Flash in cached area
	volatile uint32_t *pSdram32 = (uint32_t*)SDRAM_BASE;
	volatile uint32_t *pGPIO_DATA = (uint32_t*)SYS_GPIO_0_DATA;
	volatile uint32_t *pGPIO_DIR = (uint32_t*)SYS_GPIO_0_DIR;

	*pGPIO_DIR  = GPIO_0_DFLT_DIR;
	*pGPIO_DATA = GPIO_0_DFLT_DATA;
	uint32_t block_sel = (uint32_t)((*pGPIO_DATA >> GPIO_0_ALIGN_DIP) & GPIO_0_MASK_DIP);
	uint32_t flash_offset = getBootOffset(block_sel);
			
	sputs("\nMIPS bootloader\n");
	pFlashMem += flash_offset >> 2;
	
	// ----------------------------------------------------------
	sputs("Loading image from flash at ");print_word((int)pFlashMem);sputs("\n");
	img_hdr = *((flash_img_hdr_t*)pFlashMem);
	if ((img_hdr.magic != MAGIC_EL) && (img_hdr.magic != MAGIC_EB))
	{
		sputs("\n\n");
		sputs("Invalid image found at ");print_word((int)pFlashMem); sputs(". Abort.\n");
		return 1;
	}

	sputs("target_base = ");print_word(img_hdr.target_base);sputs("\n");
	sputs("img_base    = ");print_word(img_hdr.img_base);sputs("\n");
	sputs("img_size    = ");print_word(img_hdr.img_size);sputs("\n");

	pSdram32 = (uint32_t*)img_hdr.target_base;

	// It's safe to use cached Flash,
	// because  D-Cache was invalidated during execution of start code
	pFlashMem += img_hdr.img_base/4;

	for (i=0; i < img_hdr.img_size/4; i += 8)
	{
		pSdram32[i+0] = pFlashMem[i+0];
		pSdram32[i+1] = pFlashMem[i+1];
		pSdram32[i+2] = pFlashMem[i+2];
		pSdram32[i+3] = pFlashMem[i+3];
		pSdram32[i+4] = pFlashMem[i+4];
		pSdram32[i+5] = pFlashMem[i+5];
		pSdram32[i+6] = pFlashMem[i+6];
		pSdram32[i+7] = pFlashMem[i+7];
	}
	for (i; i < img_hdr.img_size/4; i++)
	{
		pSdram32[i] = pFlashMem[i];
	}

	sputs("Booting from ");print_word(img_hdr.target_base);sputs("\n\n");
	Exec_at((void*)pSdram32);

	// ----------------------------------------------------------
	sputs("Booting from UART..");

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

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

					break;

				case srec_eob:
					sputs("done\n\n");
					sputs("Execute program at ");
					print_word(srec.addr);
					sputs("\n\n");

					Exec_at((void*)srec.addr);
					break;

				default:
					result = -1;
					break;
			}
			if (result < 0)
			{
				break;
			}

		};
	}
	return 0;
}