#include "libsys.h"
#include "cfiflash.h"

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

} srec_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 Exec_at(void *pEntry)
{
	__asm
	(
		".set	noreorder\n"
		"mfc0	$26, $12\n"			// change exception vector
		"li		$27, 0xFFBFFFFF\n"
		"and	$26, $27\n"
		"mtc0	$26, $12\n"
		"jr		$4\n"				// jump entry
		"rfe\n"
		".set	reorder\n"

	);
}

void PrintCPUinfo(void)
{
	int result, rev_id;

	char *cpu_type_str[7] = {"invalid", "R2000", "R3000", "R6000", "R4000", "reserved", "R6000A"};

	// Print Status register
	result = CP0_SR_read();

	sputs("Status  : ");
	print_word(result);
	sputs("\n");

	// Print Revision
	result = CP0_PRID_read();
	rev_id = (result >> 8) & 0xFF;
	sputs("CPU type: ");
	if ((rev_id > 0) && (rev_id < 0x07))
	{
		sputs(cpu_type_str[rev_id]);
		sputs(" Rev.");
		rev_id = result & 0xFF;
		print_byte((char)rev_id);
	}
	else
		sputs("Unknown");

	sputs("\n");

}


#define	TEST_SIZE			(1024*1024)	// bytes
//#define	TEST_SIZE			(1024)	// bytes
#define SDRAM_BASE			0x40000000
#define	IMAGE_OFFSET		0x01000000
#define	FLASH_OFFSET		0x00700000

int main(int argc, char *argv[])
{
	UINT8 buf[256];
	srec_t srec;
	int result, i;
	UINT32 haddr, addr;
	flash_t flash;

	volatile UINT32 *pReg = (UINT32*)sys_led_port;
	volatile UINT32 *pBtn = (UINT32*)sys_gpio0;
	volatile UINT8 *pMem;
	volatile UINT32 *pROM32;
	volatile UINT32 *pMem32;
	volatile UINT32 *pFlash = (UINT32*)sys_flash_io;
	volatile UINT32 *pUSB = (UINT32*)sys_usb_data;

	volatile UINT8 *ram8 = (UINT8*)SDRAM_BASE;
	volatile UINT16 *ram16 = (UINT16*)SDRAM_BASE;
	volatile UINT32 *ram32 = (UINT32*)SDRAM_BASE;

	*pReg = 0;
	haddr = 0;
/*
	__asm __volatile
	(
		".set	noreorder\n"
		"li $a0, 0xA0020000\n"
		"li $t0, 0x12345678\n"
		"li $t1, 0x5555AAAA\n"
		"sw $t0, 0($a0)\n"
//		"nop\n"
		"sw $t1, 4($a0)\n"
//		"nop\n"
		"lw $v0, 0($a0)\n"
//		"nop\n"
		"lw $v0, 4($a0)\n"
//		"nop\n"
		"sw $t0, 0($a0)\n"
//		"nop\n"
		"lw $v0, 4($a0)\n"
//		"nop\n"
		"sw $t1, 4($a0)\n"
//		"nop\n"
		"lw $v0, 0($a0)\n"
		".set	reorder\n"
	);
	__asm __volatile
	(
		".set	noreorder\n"
		"lui    $v1, 0xA002\n"
		"lui	$v0,0x70\n"
		"ori	$v0,$v0,0x70\n"
		"sw	$v0,0($v1)\n"
		"lui	$a0,0x4002\n"
		"lw	$a1,0($v1)\n"
		"lw	$ra,12($sp)\n"
		"lw	$s1,8($sp)\n"
		"lw	$s0,4($sp)\n"
		".set	reorder\n"
	);

*/
//	PrintCPUinfo();
/*
	*(pFlash+0) = 0x12345678;
	*(pFlash+1) = 0xAAAA5555;
	*(pFlash+2) = 0xFFFF0000;
	*(pFlash+3) = 0x0000FFFF;

	*pReg = *(pFlash+0);
	*pReg = *(pFlash+1);
	*pReg = *(pFlash+2);
	*pReg = *(pFlash+3);

	*(pUSB+0) = 0x12345678;
	*(pUSB+1) = 0xAAAA5555;
	*(pUSB+2) = 0xFFFF0000;
	*(pUSB+3) = 0x0000FFFF;

	*pReg = *(pUSB+0);
	*pReg = *(pUSB+1);
	*pReg = *(pUSB+2);
	*pReg = *(pUSB+3);
	sputs("BOOT ");

	pMem32 = (UINT32*)0x40000000;
	pROM32 = (UINT32*)0x00000000;
	for (i=0; i < 0x3500; i+=4)
		*pMem32++ = *pROM32++;

	Exec_at((void*)0x40000000);


	// Memtest BEGIN
	sputs("SDRAM Memory Test\r\n");
	sputs("Write (8-Bit access)...");
	for (i=0; i < TEST_SIZE; i++)
		ram8[i] = (UINT8)i;

	sputs("done\r\n");

	sputs("Verify (8-Bit access)...");
	for (i=TEST_SIZE-1; i >= 0; i--)
		if (ram8[i] != (UINT8)i)
			break;
	i++;
	if (i)
		sputs("failed\r\n");
	else
		sputs("passed\r\n");


	sputs("Write (16-Bit access)...");
	for (i=0; i < TEST_SIZE/2; i++)
		ram16[i] = (UINT16)i;

	sputs("done\r\n");

	sputs("Verify (16-Bit access)...");
	for (i=TEST_SIZE/2-1; i >= 0; i--)
		if (ram16[i] != (UINT16)i)
			break;
	i++;
	if (i)
		sputs("failed\r\n");
	else
		sputs("passed\r\n");


	sputs("Write (32-Bit access)...");
	for (i=0; i < TEST_SIZE/4; i++)
		ram32[i] = (UINT32)i;

	sputs("done\r\n");

	sputs("Verify (32-Bit access)...");
	for (i=TEST_SIZE/4-1; i >= 0; i--)
		if (ram32[i] != (UINT32)i)
			break;
	i++;
	if (i)
		sputs("failed\r\n");
	else
		sputs("passed\r\n");

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

	// Memtest END
	// ----------------------------------------------------------
*/
	ram32 = (UINT32*)(SDRAM_BASE + IMAGE_OFFSET);
	pFlash = (UINT32*)(sys_flash_io + FLASH_OFFSET);
	print_word((UINT32)*pBtn);
	if (!*pBtn)
	{
		sputs("Booting from flash..");
		for (i=0; i < TEST_SIZE/4; i++)
			ram32[i] = pFlash[i];

		sputs("done");

		Exec_at((void*)ram32);
	}
	else
	{

		sputs("SDRAM erase at        : ");
		print_word((UINT32)ram32);
		sputs("\n");
		for (i=0; i < TEST_SIZE/4; i++)
			ram32[i] = 0;

		cfi_init(&flash, sys_flash_io);
		if (cfi_find(&flash) < 0)
			return 1;

		sputs("Found Flash at ");
		print_word((UINT32)flash.pBase);
		sputs("\n");

		addr = FLASH_OFFSET;
		sputs("Flash erase at        : ");
		print_word(addr);
		sputs("\n");
		cfi_block_erase(&flash, addr/4);

		while(1)
		{
			sputs("BOOT ");

			while(1)
			{
				result = srec_getline(buf);
				result = decode_srec(&srec, buf, result);
				*pReg = 0;
				srec.addr += IMAGE_OFFSET;
				switch (result)
				{
					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:
						*pReg = 1;
						print_word(srec.addr);
						sputs(" to ");
						print_word(haddr-4);
						sputs("\n");

						addr = FLASH_OFFSET;
						sputs("Program Flash at      : ");
						print_word(addr);
						sputs("\n");
						cfi_program_multi(&flash, addr/4, (UINT32*)ram32, TEST_SIZE/4);

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

					default:
						break;
				}
				if (result == srec_err)
				{
					*pReg = 0x40000000;
					break;
				}

			};
		}
	}
	return 0;
}