[tyndur-devel] [PATCH 2/3] cdi/core: Mehrere IRQ-Handler pro Treiber

[Antoine Kaufmann <toni@xxxxxxxxxx>]

! cdi/core: Jetzt funktioniert das mit den IRQs auch, wenn man mal auf
            absurde Idee kommt, mehrere IRQs in einem Treiber benutzen
            zu wollen. ;-)
---
 src/modules/cdi/lib/misc.c |   34 +++++++++++++++++++++++++---------
 1 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/src/modules/cdi/lib/misc.c b/src/modules/cdi/lib/misc.c
index 4a22f78..ae66a9e 100644
--- a/src/modules/cdi/lib/misc.c
+++ b/src/modules/cdi/lib/misc.c
@@ -16,16 +16,26 @@
 #include "cdi.h"
 #include "cdi/misc.h"
 
-// TODO Bei mehr als einem registrierten IRQ geht das so nicht mehr
-static void (*driver_irq_handler)(struct cdi_device*) = NULL;
-static struct cdi_device* driver_irq_device = NULL;
+#define LOST_IRQ_OFFSET 0x20
+#define IRQ_COUNT       0x10
+
+static void (*driver_irq_handler[IRQ_COUNT])(struct cdi_device*) = { NULL };
+static struct cdi_device* driver_irq_device[IRQ_COUNT] = { NULL };
 
 /**
  * Interner IRQ-Handler, der den IRQ-Handler des Treibers aufruft
  */
-static void irq_handler(byte irq)
+static void irq_handler(uint8_t irq)
 {
-    driver_irq_handler(driver_irq_device);
+    // Oops, das war wohl kein IRQ, den wollen wir nicht.
+    if ((irq < LOST_IRQ_OFFSET) || (irq > LOST_IRQ_OFFSET + IRQ_COUNT)) {
+        return;
+    }
+
+    irq -= LOST_IRQ_OFFSET;
+    if (driver_irq_handler[irq]) {
+        driver_irq_handler[irq](driver_irq_device[irq]);
+    }
 }
 
 /**
@@ -35,13 +45,19 @@ static void irq_handler(byte irq)
  * @param handler Handlerfunktion
  * @param device Geraet, das dem Handler als Parameter uebergeben werden soll
  */
-void cdi_register_irq(uint8_t irq, void (*handler)(struct cdi_device*), 
+void cdi_register_irq(uint8_t irq, void (*handler)(struct cdi_device*),
     struct cdi_device* device)
 {
-    driver_irq_handler = handler;
-    driver_irq_device = device;
+    if (irq > IRQ_COUNT) {
+        // FIXME: Eigentlich sollte diese Funktion etwas weniger optimistisch
+        // sein, und einen Rueckgabewert haben.
+        return;
+    }
+
+    driver_irq_handler[irq] = handler;
+    driver_irq_device[irq] = device;
 
-    register_intr_handler(0x20 + irq, irq_handler);
+    register_intr_handler(LOST_IRQ_OFFSET + irq, irq_handler);
 }
     
 /**
-- 
1.6.0.6