#include <stdlib.h>
#include <stdio.h>
#include "libsys.h"
#include "xcpt.h"

#define MAX_NUM_EXCEPTIONS	10
void *g_fp_badsubs[MAX_NUM_EXCEPTIONS];

void _exc_dtlbl_err(UINT32 vaddr)
{
	__asm
	(
		".set	noreorder\n"
		" move	$v0, %[vaddr]\n"
		" lw	$zero, 0($v0)\n"
		"nop\n"
		".set	reorder\n"
		:
		: [vaddr] "r" (vaddr)
		: "v0"

	);

}

void _exc_dtlbs_err(UINT32 vaddr, UINT32 data)
{
	__asm
	(
		".set	noreorder\n"
		" move	$a0, %[data]\n"
		" move	$v0, %[vaddr]\n"
		" sw	$a0, 0($v0)\n"
		".set	reorder\n"
		:
		: [vaddr] "r" (vaddr), [data] "r" (data)
		: "v0", "a0"

	);

}

void _exc_itlbl_err(UINT32 vaddr)
{
	__asm
	(
		".set	noreorder\n"
		" move	$v0, %[vaddr]\n"
		" jr    $v0\n"
		"nop\n"
		".set	reorder\n"
		:
		: [vaddr] "r" (vaddr)
		: "v0"

	);

}

int xcp_handler(struct xcptcontext * xcp)
{
	sputs("----------------------------------------\n");
	sputs("Exception Type: ");xcpt_exctype_print(xcp);sputs("\n");
	sputs("----------------------------------------\n");
	xcpt_registers_print(xcp);
	sputs("\n");

	sputs("CP0(ENTRY_HI) = "); print_word(CP0_read(10)); sputs("\n");
	sputs("CP0(ENTRY_LO) = "); print_word(CP0_read(2)); sputs("\n");

	xcp->epc += 4;
	if (xcp->cr & 0x80000000)
	{
		xcp->epc += 4;
	}

	interrupt_clr(0);
	interrupt_clr(1);

	return 0;
}

int main(void)
{
	int buf[256];
	int i, sel;
	int *pMem;

	xcpt_register(EXC_MOD, xcp_handler);
	xcpt_register(EXC_TLBL, xcp_handler);
	xcpt_register(EXC_TLBS, xcp_handler);

	// Tell CP0 we want to access D-TLB
	CP0_write(3,  0x00000001);

	// Write 1. TLB entry
	CP0_write(0,  0x00000800);
	CP0_write(10, 0x3110A540);
	CP0_write(2,  0x00008300);
	CP0_TLBWI;

	// Write 2. TLB entry
	CP0_write(0,  0x00000900);
	CP0_write(10, 0x3220B540);
	CP0_write(2,  0x00009300);
	CP0_TLBWI;

	CP0_write(0,  0x00000800);
	CP0_TLBR;
	sputs("CP0(INDEX)    = "); print_word(CP0_read(0)); sputs("\n");
	sputs("CP0(ENTRY_HI) = "); print_word(CP0_read(10)); sputs("\n");
	sputs("CP0(ENTRY_LO) = "); print_word(CP0_read(2)); sputs("\n");
	CP0_write(0,  0x00000900);
	CP0_TLBR;
	sputs("CP0(INDEX)    = "); print_word(CP0_read(0)); sputs("\n");
	sputs("CP0(ENTRY_HI) = "); print_word(CP0_read(10)); sputs("\n");
	sputs("CP0(ENTRY_LO) = "); print_word(CP0_read(2)); sputs("\n");


	sputs(" 1 : DTLBS (BadVAddr = 0x3110A234)\n");
	_exc_dtlbs_err(0x3110A234, 0xDECAFED);

	sputs(" 2 : DTLBL (BadVAddr = 0x3220B234)\n");
	_exc_dtlbl_err(0x3220B234);

	sputs(" 3 : ITLBL (BadVAddr = 0x3220B234)\n");
	_exc_itlbl_err(0x3220B234);

	return 0;

}