From 7237fbce6c86dd095b371295738afd518b1afade Mon Sep 17 00:00:00 2001
From: "(no author)" <(no author)@6dc7ffe9-61d6-0310-9af1-9938baff3ed1>
Date: Sun, 10 Sep 2006 16:59:08 +0000
Subject: Further PIO IRQ fixes

git-svn-id: https://svn.openpcd.org:2342/trunk@176 6dc7ffe9-61d6-0310-9af1-9938baff3ed1
---
 openpcd/firmware/src/os/pio_irq.c | 22 ++++++++++++++++++----
 openpcd/firmware/src/os/pio_irq.h | 10 ++++------
 2 files changed, 22 insertions(+), 10 deletions(-)

diff --git a/openpcd/firmware/src/os/pio_irq.c b/openpcd/firmware/src/os/pio_irq.c
index 3e65561..266512e 100644
--- a/openpcd/firmware/src/os/pio_irq.c
+++ b/openpcd/firmware/src/os/pio_irq.c
@@ -1,22 +1,28 @@
+#include <errno.h>
+#include <sys/types.h>
+#include <lib_AT91SAM7.h>
+#include <os/pio_irq.h>
 
-#include <interrupt_helper.h>
-#define NR_PIO 32
+static irq_handler_t *pio_handlers[NR_PIO];
 
+#if 0
 static u_int8_t ffs(u_int32_t in)
 {
 	int i;
 
 	for (i = sizeof(in)*8; i > 0; i++) {
-		if (in & (1 << i-1))
+		if (in & (1 << (i-1)))
 			return i;
 	}
 
 	return 0;
 }
+#endif
 
 static void pio_irq_demux(void)
 {
 	u_int32_t pio = AT91F_PIO_GetInterruptStatus(AT91C_BASE_PIOA);
+	int i;
 
 	for (i = 0; i < NR_PIO; i++) {
 		if (pio & (1 << i) && pio_handlers[i])
@@ -35,7 +41,7 @@ void pio_irq_disable(u_int32_t pio)
 	AT91F_PIO_InterruptDisable(AT91C_BASE_PIOA, pio);
 }
 
-int pio_irq_register(u_int32_t pio, void (*handler)(void))
+int pio_irq_register(u_int32_t pio, irq_handler_t *handler)
 {
 	u_int8_t num = ffs(pio);
 
@@ -64,3 +70,11 @@ void pio_irq_unregister(u_int32_t pio)
 	pio_irq_disable(pio);
 	pio_handlers[num] = NULL;
 }
+
+void pio_irq_init(void)
+{
+	AT91F_AIC_ConfigureIt(AT91C_BASE_AIC, AT91C_ID_PIOA,
+			      AT91C_AIC_PRIOR_LOWEST,
+			      AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL, &pio_irq_demux);
+	AT91F_AIC_EnableIt(AT91C_BASE_AIC, AT91C_ID_PIOA);
+}
diff --git a/openpcd/firmware/src/os/pio_irq.h b/openpcd/firmware/src/os/pio_irq.h
index 86021df..33f4656 100644
--- a/openpcd/firmware/src/os/pio_irq.h
+++ b/openpcd/firmware/src/os/pio_irq.h
@@ -1,15 +1,13 @@
 #ifndef _PIO_IRQ_H
 #define _PIO_IRQ_H
 
-#include <include/AT91SAM7S64.h>
-
-typedef irq_handler_t (void)(u_int32_t pio);
-
-static irq_handler_t pio_handlers[NR_PIO];
+#define NR_PIO 32
+typedef void irq_handler_t(u_int32_t pio);
 
 extern void pio_irq_enable(u_int32_t pio);
 extern void pio_irq_disable(u_int32_t pio);
-extern int pio_irq_register(u_int32_t pio, irq_handler_t func);
+extern int pio_irq_register(u_int32_t pio, irq_handler_t *func);
 extern void pio_irq_unregister(u_int32_t pio);
+extern void pio_irq_init(void);
 
 #endif
-- 
cgit v1.2.3