From: John Hodge <tpg@mutabah.net>
Date: Wed, 12 Oct 2011 00:34:57 +0000 (+0800)
Subject: Modules/GIC - Fixed interrupt handling
X-Git-Tag: rel0.12~5
X-Git-Url: https://git.ucc.asn.au/?a=commitdiff_plain;h=2bad2a51c08d52a675ab2e27eafab83bf0ae567a;p=tpg%2Facess2.git

Modules/GIC - Fixed interrupt handling
---

diff --git a/Modules/armv7/GIC/gic.c b/Modules/armv7/GIC/gic.c
index 190b9953..4ace4005 100644
--- a/Modules/armv7/GIC/gic.c
+++ b/Modules/armv7/GIC/gic.c
@@ -11,9 +11,14 @@
 #include <modules.h>
 #include "gic.h"
 
+#define N_IRQS	1024
+
 // === IMPORTS ===
 extern void	*gpIRQHandler;
 
+// === TYPES ===
+typedef void (*tIRQ_Handler)(int, void*);
+
 // === PROTOTYPES ===
  int	GIC_Install(char **Arguments);
 void	GIC_IRQHandler(void);
@@ -24,6 +29,8 @@ Uint32	*gpGIC_DistributorBase;
 Uint32	*gpGIC_InterfaceBase;
 tPAddr	gGIC_DistributorAddr;
 tPAddr	gGIC_InterfaceAddr;
+tIRQ_Handler	gaIRQ_Handlers[N_IRQS];
+void	*gaIRQ_HandlerData[N_IRQS];
 
 // === CODE ===
 int GIC_Install(char **Arguments)
@@ -51,18 +58,31 @@ void GIC_IRQHandler(void)
 {
 	Uint32	num = gpGIC_InterfaceBase[GICC_IAR];
 	Log_Debug("GIC", "IRQ 0x%x", num);
-	gpGIC_InterfaceBase[GICC_EOIR] = 1;
+	gaIRQ_Handlers[num]( num, gaIRQ_HandlerData[num] );
+	gpGIC_InterfaceBase[GICC_EOIR] = num;
 }
 
-int IRQ_AddHandler(int IRQ, void (*Handler)(int, void*), void *Ptr)
+int IRQ_AddHandler(int IRQ, tIRQ_Handler Handler, void *Ptr)
 {
+	if( IRQ < 0 || IRQ >= N_IRQS-32 ) {
+		return 1;
+	}
+	
 	LOG("IRQ = %i", IRQ);
 	IRQ += 32;	// 32 internal IRQs
 	LOG("IRQ = %i (after adjust)", IRQ);
 	LOG("mask = 0x%x", 1 << (IRQ & (31-1)));
 	gpGIC_DistributorBase[GICD_ISENABLER0+IRQ/32] = 1 << (IRQ & (32-1));
 	((Uint8*)&gpGIC_DistributorBase[GICD_ITARGETSR0])[IRQ] = 1;
-	Log_Warning("GIC", "TODO: Implement IRQ_AddHandler");
+	
+//	Log_Warning("GIC", "TODO: Implement IRQ_AddHandler");
+	
+	if( gaIRQ_Handlers[IRQ] )
+		return 2;
+	
+	gaIRQ_Handlers[IRQ] = Handler;
+	gaIRQ_HandlerData[IRQ] = Ptr;
+	
 	return 0;
 }