#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "xcpt.h"
#include "libsys.h"
#include "mips_dis.h"
#include "cop0.h"

extern int instr_info(unsigned iword, instr_info_t *pInfo);
extern int tdisasm(char *buffer, unsigned address, unsigned iword, unsigned char ext, unsigned *regmask, unsigned *symbol_value, unsigned *ls_register);

#define DBG_PRINT_REG_CHG(tag_str, reg, reg_last)			\
			dbg_puts(tag_str);				\
			dbg_print_word(reg);			\
			dbg_puts(reg != reg_last ? "*" : " ");			\
			dbg_puts("  ");

#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];
static int DEBUG_PRINT = 0;
static int SHOW_NUM_INSTR = 5;

enum
{
	STATE_INIT = 0,
	STATE_RUN,
	STATE_STEP_SINGLE,
	STATE_STEP_PROC,
	STATE_NUM_ENTRIES
};

static int g_state = STATE_INIT;
static char *g_state_names[STATE_NUM_ENTRIES] = {"Init", "Run", "Single step", "Precedure step"};

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

static EXCEPTION_CONTEXT xcp_last;
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_count;

extern void dbg_putchar(char c);
extern char dbg_getchar();

void dbg_puts(const char *buffer)
{
	while(*buffer)
	{
		dbg_putchar(*buffer);
		buffer++;
	}
}

void dbg_print_byte(char byte)
{
	int i;
	unsigned char c, nibble;

	for (i=0; i < 2; i++)
	{
		nibble = (char)((byte >> 4) & 0xF);
		byte <<= 4;

		if (nibble < 10)
			c = nibble + '0';
		else
			c = nibble + 'A' - 10;

		dbg_putchar(c);
	}
}

void dbg_print_word(int word)
{
	int i;
	unsigned char c;

	for (i=0; i < 4; i++)
	{
		c = (char) (word >> 24);
		dbg_print_byte(c);
		word <<= 8;
	}
}

int dbg_gets(char *buffer, size_t size)
{
	size_t len = 0;
	while(size)
	{
		char c = dbg_getchar();
		if (c == 0x0A || c == 0x0D)
		{
			if (len)
			{
				break;
			}
		}
		else
		{
			buffer[len++] = c;
			dbg_putchar(c);
			if (len == size)
			{
				break;
			}
		}
	}
	buffer[len] = 0;
	dbg_putchar(0x0D);
	dbg_putchar(0x0A);
	return len;
}

void dbg_puts_d(const char *buffer)
{
	if (DEBUG_PRINT)
	{
		dbg_puts(buffer);
	}
}

void dbg_print_word_d(int word)
{
	if (DEBUG_PRINT)
	{
		dbg_print_word(word);
	}
}

void dbg_strcpy(char *pDst, const char *pSrc)
{
	while(*pSrc)
	{
		*(pDst++) = *(pSrc++);
	}
	*pDst = *pSrc;
}

void* dbg_memset(void *pDst, int value, size_t size)
{
	char *dst = (char*)pDst;
	
	while(size)
	{
		*(dst++) = (char)value;
		size--;
	}
	return (void*)dst;
}

void* dbg_memcpy(void *pDst, const void *pSrc, size_t size )
{
	char *dst = (char*)pDst;
	char *src = (char*)pSrc;
	
	while(size)
	{
		*(dst++) = *(src++);
		size--;
	}
	return (void*)dst;
}

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

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

void print_regs(struct xcptcontext * xcp, struct xcptcontext * xcp_last)
{
	DBG_PRINT_REG_CHG(" Status : ", xcp->sr, xcp_last->sr);
	DBG_PRINT_REG_CHG("  Cause : ", xcp->cr, xcp_last->cr);
	DBG_PRINT_REG_CHG("    EPC : ", xcp->epc, xcp_last->epc);
	DBG_PRINT_REG_CHG("BadAddr : ", xcp->baddr, xcp_last->baddr);
	dbg_puts("\n");
	DBG_PRINT_REG_CHG("   MDLO : ", xcp->mdlo, xcp_last->mdlo);
	DBG_PRINT_REG_CHG("   MDHI : ", xcp->mdhi, xcp_last->mdhi);

	dbg_puts("\n");

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

void set_mgmt_break(uint32_t *pAddr_break, int index)
{
	mgmt_bp_ss[index].pAddr = NULL;
	mgmt_bp_ss[index].instr = 0;

	dbg_puts_d("Set management break at ");dbg_print_word_d((long)pAddr_break);dbg_puts_d("\n");	
	// Don't overwrite user breaks
	if (IsBreak(*pAddr_break) && (BreakGetType(*pAddr_break) == BP_USER_BASE))
	{
		dbg_puts_d("Instruction at address ");dbg_print_word_d((long)pAddr_break);dbg_puts_d(" is user break\n");
	}
	else
	{
		// Save original instruction
		mgmt_bp_ss[index].pAddr = pAddr_break;
		mgmt_bp_ss[index].instr = *(mgmt_bp_ss[index].pAddr);
		// Replace instruction with managment breakpoint
		*(mgmt_bp_ss[index].pAddr) = BP_MGMT_SS(index+1);
		ICACHE_invalidate_at(mgmt_bp_ss[index].pAddr);
	}
}

void singlestep(struct xcptcontext * xcp, uint32_t *pAddr_curr, uint32_t is_branch_shadow)
{

    int junk;
 	uint32_t branch_addr, reg, bp_type;
	uint32_t *pInstr, *pAddr_prev, *pAddr_next;
	instr_info_t info;
	int set_break_at_jump_target = 0;

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

	mgmt_bp_ss[1].pAddr = NULL;
	mgmt_bp_ss[1].instr = 0;

	// We have two possible management breaks, if previous instruction was branch or jump
	if (is_branch_shadow)
	{
		int type = instr_info(*pAddr_prev, &info);

		tdisasm(g_buf, (unsigned int)pAddr_prev,  (unsigned int)*pAddr_prev, 0, &junk, (unsigned int*)&branch_addr, (unsigned int*)&reg);
		// Is jump target stored in register
		if (type == INSTR_TYPE_JUMP || type == INSTR_TYPE_BRANCH)
		{
			if (info.flags & (INSTR_FLAGS_JUMP_REGISTER|INSTR_FLAGS_BRANCH_REGISTER))
			{
				branch_addr = xcp->regs[reg&0x1F];
			}
		}
		// Don't overwrite user breaks
		if (IsBreak(branch_addr) && (BreakGetType(branch_addr) == BP_USER_BASE))
		{
			dbg_puts_d("Instruction at branch address ");dbg_print_word_d((long)branch_addr);dbg_puts_d(" is user break\n");
		}
		else
		{
			set_break_at_jump_target = 1;
		}
	}
	set_mgmt_break(pAddr_next, 0);
	if (set_break_at_jump_target)
	{
		set_mgmt_break((uint32_t*)branch_addr, 1);
	}
	
}

int dbg_handler(struct xcptcontext * xcp)
{

 	uint32_t bp_addr, bp_index = 0, branch_addr, reg, result, bp_type;
    int junk, i, leave_isr, is_branch_shadow, is_user_bp;
	uint32_t *pInstr, *pAddr_curr, *pAddr_prev, *pAddr_next;
	instr_info_t info;

	is_user_bp = 0;
	pAddr_curr = (uint32_t*)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 (g_state = STATE_INIT)
	{
		dbg_memset(&xcp_last, 0, sizeof(EXCEPTION_CONTEXT));
	}

	dbg_puts_d("\n");dbg_puts_d(g_state_names[g_state]);dbg_puts_d("\n");

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

		switch (bp_type)
		{
			case BP_MGMT_BASE_SS:

			// Restore original instructions after single-step
			dbg_puts_d("1. Restore after single-step at ");dbg_print_word_d((long)mgmt_bp_ss[0].pAddr);dbg_puts_d("\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)
			{
				dbg_puts_d("2. Restore after single-step at ");dbg_print_word_d((long)mgmt_bp_ss[1].pAddr);dbg_puts_d("\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);
			dbg_puts_d("Restore user break at ");dbg_print_word_d((long)user_bp[bp_index].pAddr);dbg_puts_d("\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);
			dbg_puts_d("1. Restore after RU at ");dbg_print_word_d((long)mgmt_bp_ru[0].pAddr);dbg_puts_d("\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);
				dbg_puts_d("2. Restore after RU at ");dbg_print_word_d((long)mgmt_bp_ru[1].pAddr);dbg_puts_d("\n");
			}
			if (!g_is_ss)
			{
				return 0;
			}

			break;

			case BP_USER_BASE:

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

			default:
			dbg_puts_d("Found ordinary break at ");dbg_print_word_d((long)pAddr_curr);dbg_puts_d("\n");
			break;

		}
	}
	dbg_puts("\n");
	print_regs(xcp, &xcp_last);

	pInstr = pAddr_prev;
	// Disassemble instructions
    for (i=0; i < SHOW_NUM_INSTR; i++)
    {
		dbg_print_word((long)pInstr);
		if (pInstr != pAddr_curr)
		{
			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);
				dbg_puts(": b("); dbg_print_byte((char)BP_GET_ID(*pInstr)); dbg_puts(")   ");
			}
			else
			{
				tdisasm(g_buf, (long)pInstr, *pInstr, 0, &junk, &junk, &junk);
				dbg_puts(":         ");
			}
		}
		else
		{
			tdisasm(g_buf, (long)pInstr, *pInstr, 0, &junk, &junk, &junk);
			if (is_user_bp)
			{
				dbg_puts(": b("); dbg_print_byte((char)bp_index); dbg_puts(")-> ");
			}
			else
			{
				dbg_puts(":      -> ");
			}
		}
		dbg_puts(g_buf);
		dbg_puts("\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].instr = 0;
		dbg_puts_d("1. Insert RU-break at ");dbg_print_word_d((long)mgmt_bp_ru[0].pAddr);dbg_puts_d("\n");

		// We have two possible management breaks, if previous instruction was branch or jump
		if (is_branch_shadow)
		{
			int type = instr_info(*pAddr_prev, &info);
			tdisasm(g_buf, (long)pAddr_prev, *pAddr_prev, 0, &junk, (unsigned int*)&branch_addr, (unsigned int*)&reg);
			// Is jump target stored in register
			if (type == INSTR_TYPE_JUMP || type == INSTR_TYPE_BRANCH)
			{
				if (info.flags & (INSTR_FLAGS_JUMP_REGISTER|INSTR_FLAGS_BRANCH_REGISTER))
				{
					branch_addr = xcp->regs[reg&0x1F];
				}
			}
			// Save original instruction
			mgmt_bp_ru[1].pAddr = (uint32_t*)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);
			dbg_puts_d("2. Insert RU-break at ");dbg_print_word_d((long)mgmt_bp_ru[1].pAddr);dbg_puts_d("\n");
		}
	}

	do
	{
		char buffer[33];
		leave_isr = 0;
		switch(dbg_getchar())
		{
			case 'c':
			g_state = STATE_RUN;
			leave_isr = 1;
			g_is_ss = 0;
			if (IsBreak(*pAddr_curr))
			{
				if (BreakGetType(*pAddr_curr) != BP_USER_BASE)
				{
					xcp->epc += 4;		// Skip ordinary program breaks
				}
			}
			break;

			case 'g':
			g_state = STATE_RUN;
			leave_isr = 1;
			g_is_ss = 0;
			dbg_puts("Enter address: ");
			dbg_gets(buffer, sizeof(buffer));
			xcp->epc = strtol(buffer, NULL, 16);
			break;

			case 'b':
			dbg_puts("Enter breakpoint: ");
			dbg_gets(buffer, sizeof(buffer));
			bp_addr = strtol(buffer, NULL, 16);
			dbg_puts("Insert breakpoint at "); dbg_print_word(bp_addr); dbg_puts("\n");

			user_bp[g_bp_count].pAddr = (uint32_t*)bp_addr;
			user_bp[g_bp_count].instr = *(user_bp[g_bp_count].pAddr);
			*(user_bp[g_bp_count].pAddr) = BP_USER(g_bp_count);

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

			case 's':
			g_state = STATE_STEP_SINGLE;
			g_is_ss = 1;
			leave_isr = 1;

			if (is_user_bp)
				break;

			if (IsBreak(*pAddr_curr))
			{
				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
				{
					break;
				}
				else
				{
					xcp->epc += 4;		// Skip ordinary program breaks
				}
			}

			singlestep(xcp, pAddr_curr, is_branch_shadow);
			break;

			case 'e':
			g_state = STATE_STEP_SINGLE;
			g_is_ss = 1;
			leave_isr = 1;

			if (is_user_bp)
				break;

			if (IsBreak(*pAddr_curr))
			{
				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
				{
					break;
				}
				else
				{
					xcp->epc += 4;		// Skip ordinary program breaks
				}
			}

			pInstr = pAddr_curr;

			do
			{
				result = instr_info(*pInstr, &info);
				if (result == INSTR_TYPE_JUMP)
				{
					if (info.rs == 0x1F)
						break;
				}
				else if (result == INSTR_TYPE_LOAD)
				{
					if (info.rs == 0x1F)
						break;
				}
				pInstr++;
			} while(1);

			dbg_puts_d("Return found at "); dbg_print_word_d((long)pInstr);dbg_puts_d("\n");
			set_mgmt_break(pInstr, 0);
			break;

			case 'p':
			g_state = STATE_STEP_PROC;
			g_is_ss = 1;
			leave_isr = 1;

			if (is_user_bp)
				break;

			if (IsBreak(*pAddr_curr))
			{
				if (BreakGetType(*pAddr_curr) == BP_USER_BASE)
				{
					break;
				}
				else
				{
					xcp->epc += 4;		// Skip ordinary program breaks
				}
			}

			result = instr_info(*pAddr_curr, &info);
			if ((result == INSTR_TYPE_JUMP) && (info.flags & INSTR_FLAGS_JUMP_LINK))
			{
				singlestep(xcp, pAddr_next, 0);
				dbg_puts_d("Skip sub-routine\n");
			}
			else
			{
				singlestep(xcp, pAddr_curr, is_branch_shadow);
			}
			break;

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

	dbg_memcpy(&xcp_last, xcp, sizeof(EXCEPTION_CONTEXT));
	
	return 0;
}