#include "common.h"
#include "io.h"
#include "sys.h"
#include "console.h"
#include "esp.h"
#include "irq.h"

/* Don't mark no_return or gcc moves it to SDRAM */
static void __hot __text_hot killed(const char *how)
{
    /* Cannot use con_printf() here */
    const uint16_t *pcp;
    size_t mtval;
    size_t mepc, meinfo, task;
    size_t pc;

    /* Try to move back to the previous instruction (wrong for jumps...) */
    asm volatile("csrr %0,mepc" : "=r" (mepc));
    asm volatile("csrr %0,%1" : "=r" (meinfo) : "i" (MEINFO_CSR));
    pc = mepc - (((meinfo & 3) + 1) << 4);
    pcp = (const uint16_t *)pc;

    con_puts(hotstr("ERROR: "));
    con_puts(how);
    con_puts(hotstr(" at 0x"));
    con_print_hex(pc);
    con_puts(hotstr(" (0x"));
    con_print_hex((pcp[1] << 16) + pcp[0]);
    con_puts(hotstr(")\nBad address: 0x"));
    asm volatile("csrr %0,mtval" : "=r" (mtval));
    con_print_hex(mtval);
    con_puts(hotstr(" task "));
    asm volatile("csrr %0,user_context" : "=r" (task));
    con_putc('0' + task);
    con_putc('\n');

    for (int i = 0; i < 32; i += 8) {
	for (int j = 0; j < 8; j++) {
	    uint32_t v = rdxreg(i+j);
	    con_print_hex(v);
	    con_putc((j == 7) ? '\n' : ' ');
	}
    }

    con_flush();
    udelay(5000000);
    reset(SYS_RESET_SOFT);
}

IRQHANDLER(buserr,0)
{
    killed(hotstr("misaligned"));
}

IRQHANDLER(ebreak,0)
{
    killed(hotstr("invalid instruction"));
}