#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "irq.h"
#include "libsys.h"
#include "mipsdis.h"

#define	MAX_USER_BP				16
#define	BP_MASK_ID				0x00FFF
#define	BP_MASK_TYPE			0xFF000

#define	BP_GET_ID(bp)			((bp >> 6) & 0xFFF)

#define	BP_USER_BASE			0x10000
#define	BP_USER(id)				(((BP_USER_BASE + id) << 6) | 0x0D)

#define	BP_MGMT_BASE_SS			0x20000
#define	BP_MGMT_SS(id)			(((BP_MGMT_BASE_SS + id) << 6) | 0x0D)

#define	BP_MGMT_BASE_RU			0x30000
#define	BP_MGMT_RU(id)			(((BP_MGMT_BASE_RU + id) << 6) | 0x0D)

static char g_buf[80];

typedef struct _sbp_t
{
	UINT32 *pAddr;
	UINT32 instr;
} bp_t;

static bp_t user_bp[MAX_USER_BP];
static bp_t mgmt_bp_ss[2];
static bp_t mgmt_bp_ru[2];
static int g_is_ss;
static int g_bp_index;

int IsBreak(UINT32 instr)
{
	return ((instr & 0xFC00003F) == 0x0000000D);
}

UINT32 BreakGetType(UINT32 instr)
{
	return (instr >> 6) & BP_MASK_TYPE;
}

void dbg_handler(struct xcptcontext * xcp)
{

 	UINT32 bp_addr, bp_index = 0, branch_addr, reg, result, bp_type;
    int junk, i, leave_isr, is_branch_shadow, is_user_bp;
	UINT32 volatile *pBtn = (UINT32*)sys_gpio0;
	UINT32 *pInstr, *pAddr_curr, *pAddr_prev, *pAddr_next;

	sputs("\n");
	PRINT_REG(" Status : ", xcp->sr);
	PRINT_REG("  Cause : ", xcp->cr);
	PRINT_REG("    EPC : ", xcp->epc);
	PRINT_REG("BadAddr : ", xcp->baddr);
	sputs("\n");
	PRINT_REG("   MDLO : ", xcp->mdlo);
	PRINT_REG("   MDHI : ", xcp->mdhi);

	sputs("\n");

	sputs("Registers:\n");
	PRINT_REG(" 0 (ze) : ", xcp->regs[0]);
	PRINT_REG(" 1 (at) : ", xcp->regs[1]);
	PRINT_REG(" 2 (v0) : ", xcp->regs[2]);
	PRINT_REG(" 3 (v1) : ", xcp->regs[3]);
	sputs("\n");
	PRINT_REG(" 4 (a0) : ", xcp->regs[4]);
	PRINT_REG(" 5 (a1) : ", xcp->regs[5]);
	PRINT_REG(" 6 (a2) : ", xcp->regs[6]);
	PRINT_REG(" 7 (a3) : ", xcp->regs[7]);
	sputs("\n");
	PRINT_REG(" 8 (t0) : ", xcp->regs[8]);
	PRINT_REG(" 9 (t1) : ", xcp->regs[9]);
	PRINT_REG("10 (t2) : ", xcp->regs[10]);
	PRINT_REG("11 (t3) : ", xcp->regs[11]);
	sputs("\n");
	PRINT_REG("12 (t4) : ", xcp->regs[12]);
	PRINT_REG("13 (t5) : ", xcp->regs[13]);
	PRINT_REG("14 (t6) : ", xcp->regs[14]);
	PRINT_REG("15 (t7) : ", xcp->regs[15]);
	sputs("\n");
	PRINT_REG("16 (s0) : ", xcp->regs[16]);
	PRINT_REG("17 (s1) : ", xcp->regs[17]);
	PRINT_REG("18 (s2) : ", xcp->regs[18]);
	PRINT_REG("19 (s3) : ", xcp->regs[19]);
	sputs("\n");
	PRINT_REG("20 (s4) : ", xcp->regs[20]);
	PRINT_REG("21 (s5) : ", xcp->regs[21]);
	PRINT_REG("22 (s6) : ", xcp->regs[22]);
	PRINT_REG("23 (s7) : ", xcp->regs[23]);
	sputs("\n");
	PRINT_REG("24 (t8) : ", xcp->regs[24]);
	PRINT_REG("25 (t9) : ", xcp->regs[25]);
	PRINT_REG("26 (k0) : ", xcp->regs[26]);
	PRINT_REG("27 (k1) : ", xcp->regs[27]);
	sputs("\n");
	PRINT_REG("28 (gp) : ", xcp->regs[28]);
	PRINT_REG("29 (sp) : ", xcp->regs[29]);
	PRINT_REG("30 (fp) : ", xcp->regs[30]);
	PRINT_REG("31 (ra) : ", xcp->regs[31]);
	sputs("\n");
	sputs("\n");

	is_user_bp = 0;
	pAddr_curr = (UINT32*)xcp->epc;
	is_branch_shadow = ((xcp->cr & 0x80000000) == 0x80000000);
	if (is_branch_shadow)
		pAddr_curr++;

	pAddr_prev = pAddr_curr - 1;
	pAddr_next = pAddr_curr + 1;

	if (IsBreak(*pAddr_curr))
	{
		bp_type = BreakGetType(*pAddr_curr);

		switch (bp_type)
		{
			case BP_MGMT_BASE_SS:

			// Restore original instructions after single-step
			sputs("1. Restore after single-step at ");print_word((long)mgmt_bp_ss[0].pAddr);sputs("\n");
			*(mgmt_bp_ss[0].pAddr) = mgmt_bp_ss[0].instr;
			ICACHE_invalidate_at(mgmt_bp_ss[0].pAddr);
			if (mgmt_bp_ss[1].pAddr)
			{
				sputs("2. Restore after single-step at ");print_word((long)mgmt_bp_ss[1].pAddr);sputs("\n");
				*(mgmt_bp_ss[1].pAddr) = mgmt_bp_ss[1].instr;
				ICACHE_invalidate_at(mgmt_bp_ss[1].pAddr);
			}

			break;

			case BP_MGMT_BASE_RU:

			bp_index = BP_GET_ID(*pAddr_curr);
			sputs("Restore user break at ");print_word((long)user_bp[bp_index].pAddr);sputs("\n");

			*(user_bp[bp_index].pAddr) = BP_USER(bp_index);
			ICACHE_invalidate_at(user_bp[bp_index].pAddr);

			*(mgmt_bp_ru[0].pAddr) = mgmt_bp_ru[0].instr;
			ICACHE_invalidate_at(mgmt_bp_ru[0].pAddr);
			sputs("1. Restore after RU at ");print_word((long)mgmt_bp_ru[0].pAddr);sputs("\n");
			if (mgmt_bp_ru[1].pAddr)
			{
				*(mgmt_bp_ru[1].pAddr) = mgmt_bp_ru[1].instr;
				ICACHE_invalidate_at(mgmt_bp_ru[1].pAddr);
				sputs("2. Restore after RU at ");print_word((long)mgmt_bp_ru[1].pAddr);sputs("\n");
			}
			if (!g_is_ss)
				return;

			break;

			case BP_USER_BASE:

			is_user_bp = 1;
			bp_index = BP_GET_ID(*pAddr_curr);
			sputs("Restore instruction at ");print_word((long)user_bp[bp_index].pAddr);sputs("\n");
			*(user_bp[bp_index].pAddr) = user_bp[bp_index].instr;
			ICACHE_invalidate_at(user_bp[bp_index].pAddr);
			break;

			default:
			break;

		}
	}

	pInstr = pAddr_prev;
	// Disassemble instructions
    for (i=0; i < 3; i++)
    {
		print_word((long)pInstr);
		if (IsBreak(*pInstr) && (BreakGetType(*pInstr) == BP_USER_BASE))
		{
			tdisasm(g_buf, (long)user_bp[BP_GET_ID(*pInstr)].pAddr, user_bp[BP_GET_ID(*pInstr)].instr, 0, &junk, &junk, &junk);
			sputs(": b   ");
		}
		else
		{
			tdisasm(g_buf, (long)pInstr, *pInstr, 0, &junk, &junk, &junk);
			if (pInstr == pAddr_curr)
				if (is_user_bp == 1)
					sputs(": b-> ");
				else
					sputs(":  -> ");
			else
				sputs(":     ");
		}
		sputs(g_buf);
		sputs("\n");
		pInstr++;
	}

	if (is_user_bp)
	{
		// Save original instruction
		mgmt_bp_ru[0].pAddr = pAddr_next;
		mgmt_bp_ru[0].instr = *(mgmt_bp_ru[0].pAddr);
		// Replace instruction with managment breakpoint
		*(mgmt_bp_ru[0].pAddr) = BP_MGMT_RU(bp_index);
		ICACHE_invalidate_at(mgmt_bp_ru[0].pAddr);
		mgmt_bp_ru[1].pAddr = NULL;
		mgmt_bp_ru[1].pAddr = 0;
		sputs("1. Insert RU-break at ");print_word((long)mgmt_bp_ru[0].pAddr);sputs("\n");

		// We have two possible management breaks, if previous instruction was branch or jump
		if (is_branch_shadow)
		{
			result = tdisasm(g_buf, (long)pAddr_prev, *pAddr_prev, 0, &junk, &branch_addr, &reg);
			// Is jump target stored in register
			if (result == 3)
			{
				branch_addr = xcp->regs[reg&0x1F];
			}
			// Save original instruction
			mgmt_bp_ru[1].pAddr = (UINT32*)branch_addr;
			mgmt_bp_ru[1].instr = *(mgmt_bp_ru[1].pAddr);
			// Replace instruction with managment breakpoint
			*(mgmt_bp_ru[1].pAddr) = BP_MGMT_RU(bp_index);
			ICACHE_invalidate_at(mgmt_bp_ru[1].pAddr);
			sputs("2. Insert RU-break at ");print_word((long)mgmt_bp_ru[1].pAddr);sputs("\n");
		}
	}

	do
	{
		leave_isr = 0;
		switch(_getchar())
		{
			case 'g':
			leave_isr = 1;
			g_is_ss = 0;
			break;

			case 'b':
			printf("Enter breakpoint: ");
			scanf("%x", &bp_addr);
			printf("Insert breakpoint at %.8X\n", bp_addr);
			user_bp[g_bp_index].pAddr = (UINT32*)bp_addr;
			user_bp[g_bp_index].instr = *(user_bp[g_bp_index].pAddr);
			*(user_bp[g_bp_index].pAddr) = BP_USER(g_bp_index);

		    tdisasm(g_buf, (long)user_bp[g_bp_index].pAddr, user_bp[g_bp_index].instr, 0, &junk, &junk, &junk);
			print_word((long)user_bp[g_bp_index].pAddr);sputs(": b   ");sputs(g_buf);sputs("\n");
			ICACHE_invalidate_at(user_bp[g_bp_index].pAddr);
			g_bp_index++;
			break;

			case 's':
			g_is_ss = 1;
			leave_isr = 1;
			// Don't overwrite RU breaks
			if (is_user_bp)
				break;
			// Don't overwrite user breaks
			if (IsBreak(*pInstr) && (BreakGetType(*pInstr) == BP_USER_BASE))
				break;

			// Save original instruction
			mgmt_bp_ss[0].pAddr = pAddr_next;
			mgmt_bp_ss[0].instr = *(mgmt_bp_ss[0].pAddr);
			// Replace instruction with managment breakpoint
			*(mgmt_bp_ss[0].pAddr) = BP_MGMT_SS(1);
			ICACHE_invalidate_at(mgmt_bp_ss[0].pAddr);
			mgmt_bp_ss[1].pAddr = NULL;
			mgmt_bp_ss[1].pAddr = 0;
			// We have two possible management breaks, if previous instruction was branch or jump
			if (is_branch_shadow)
			{
				result = tdisasm(g_buf, (long)pAddr_prev, *pAddr_prev, 0, &junk, &branch_addr, &reg);
				// Is jump target stored in register
				if (result == 3)
				{
					branch_addr = xcp->regs[reg&0x1F];
				}
				// Save original instruction
				mgmt_bp_ss[1].pAddr = (UINT32*)branch_addr;
				mgmt_bp_ss[1].instr = *(mgmt_bp_ss[1].pAddr);
				// Replace instruction with managment breakpoint
				*(mgmt_bp_ss[1].pAddr) = BP_MGMT_SS(2);
				ICACHE_invalidate_at(mgmt_bp_ss[1].pAddr);
			}
			break;

			case 'P':
			g_is_ss = 1;
			leave_isr = 1;
			// Don't overwrite RU breaks
			if (is_user_bp)
				break;
			// Don't overwrite user breaks
			if (IsBreak(*pInstr) && (BreakGetType(*pInstr) == BP_USER_BASE))
				break;

			// Save original instruction
			mgmt_bp_ss[0].pAddr = (UINT32*)xcp->regs[0x1F];
			mgmt_bp_ss[0].instr = *(mgmt_bp_ss[0].pAddr);
			// Replace instruction with managment breakpoint
			*(mgmt_bp_ss[0].pAddr) = BP_MGMT_SS(1);
			ICACHE_invalidate_at(mgmt_bp_ss[0].pAddr);
			mgmt_bp_ss[1].pAddr = NULL;
			mgmt_bp_ss[1].pAddr = 0;
			break;

			default:
			break;
		}
	} while (!leave_isr);

}

void debug_int(struct xcptcontext * xcp)
{
	dbg_handler(xcp);

}

int debug_break(struct xcptcontext * xcp)
{
	dbg_handler(xcp);
	return 0;

}

void dbg_init(void)
{

	interrupt_register(2, debug_int);
	interrupt_enable(2);

	xcpt_register(Bp, debug_break);
}