#include "picorv32.h"
#include "fw.h"

static irq_handler_t irq_handlers[IRQ_VECTORS];

/* Main IRQ dispatch; the .init.irq section puts it at the IRQ vector */
void __attribute__((interrupt,section(".init.irq"))) _irq(void)
{
    unsigned int mask = p_getq(1);
    unsigned int nirq = 0;

    while (mask) {
	bool handled;
	irq_handler_t handler;

	if (!(uint16_t)mask) {
	    mask >>= 16;
	    nirq += 16;
	}
	if (!(uint8_t)mask) {
	    mask >>= 8;
	    nirq += 8;
	}
	if (!(mask & 15)) {
	    mask >>= 4;
	    nirq += 4;
	}
	if (!(mask & 3)) {
	    mask >>= 2;
	    nirq += 2;
	}
	if (!(mask & 1)) {
	    mask >>= 1;
	    nirq += 1;
	}

	/* Now mask[0] is known to be 1 and nirq contains an active irq */
	handled = false;
	handler = irq_handlers[nirq];

	if (likely(handler))
	    handled = handler(nirq);

	if (unlikely(!handled)) {
	    uint32_t thisirq = (uint32_t)1 << nirq;

	    /* Mask this IRQ but don't touch others */
	    p_maskirq(thisirq, ~thisirq);
	}
    }
}

irq_handler_t register_irq(unsigned int vector, irq_handler_t handler)
{
    irq_handler_t old_handler;

    if (vector >= IRQ_VECTORS)
	return NULL;		/* Invalid vector */

    old_handler = irq_handlers[vector];
    irq_handlers[vector] = handler;

    return old_handler;
}