diff --git a/src/irq.c b/src/irq.c index 62c8a18..6a9d3a1 100644 --- a/src/irq.c +++ b/src/irq.c @@ -1,4 +1,4 @@ -#include +#include #include "platform.h" #undef IRQ_VECTOR_MAX @@ -97,11 +97,19 @@ static void __do_irq_vector(int index) void irq_lowlevel_init() { + int i; + struct irq_descr *irqd; + IRQ_resetAll(); IRQ_globalDisable(); IRQ_nmiDisable(); - memset(&__irq_descr, 0, sizeof __irq_descr); + for (i = 0; i < sizeof __irq_descr / sizeof __irq_descr[0]; ++i) { + irqd = &__irq_descr[i]; + irqd->irq_handler = NULL; + irqd->args = NULL; + irqd->event_id = -1; + } } unsigned int irq_global_disable() @@ -119,7 +127,7 @@ void irq_global_enable() IRQ_globalEnable(); } -#if 0 +#ifdef IRQ_CONFIG_HOOKUP static int __hookup_irq(int vector_id, int event_id, void (*handler)(void *args), void *args) { void (*irqh)() = NULL; @@ -183,16 +191,18 @@ int irq_attach(int vector_id, int event_id, void (*handler)(void *args), void *a { int r = 0; Uint32 gie; +#if !defined(IRQ_CONFIG_HOOKUP) + struct irq_descr *irqd; -#if 1 if (vector_id < IRQ_VECTOR_START || vector_id >= (IRQ_VECTOR_MAX + IRQ_VECTOR_START)) { return -1; } gie = irq_global_disable(); r = vector_id - IRQ_VECTOR_START; - __irq_descr[r].irq_handler = handler; - __irq_descr[r].args = args; - __irq_descr[r].event_id = event_id; + irqd = &__irq_descr[r]; + irqd->irq_handler = handler; + irqd->args = args; + irqd->event_id = event_id; #else gie = irq_global_disable(); if ((r = __hookup_irq(vector_id, event_id, handler, args))) { @@ -215,15 +225,18 @@ int irq_dettach(int event_id, void (*handler)(void *args), void *args) int i; int found = 0; Uint32 gie = irq_global_disable(); + struct irq_descr *irqd; for (i = 0; i < IRQ_VECTOR_MAX; ++i) { - if (handler && (__irq_descr[i].irq_handler == handler)) { - if (args && (args != __irq_descr[i].args)) { + irqd = &__irq_descr[i]; + if (handler && (irqd->irq_handler == handler) && (event_id == irqd->event_id)) { + if (args && (args != irqd->args)) { continue; } - irq_disable(__irq_descr[i].event_id); - __irq_descr[i].irq_handler = NULL; - __irq_descr[i].args = NULL; + irq_disable(event_id); + irqd->irq_handler = NULL; + irqd->args = NULL; + irqd->event_id = -1; found = 1; break; }