X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Modules%2Farmv7%2FGIC%2Fgic.c;h=9eb720fdb7ffb21f5b347c1201652ce14c5e94ad;hb=f42e912b8bc615c2009b795ce12d4c54cef4ee8b;hp=190b9953890dc3b43d646537c25988b87b5eb6ee;hpb=be74abde2c01112d53e0fd86b47a4874ec940663;p=tpg%2Facess2.git

diff --git a/Modules/armv7/GIC/gic.c b/Modules/armv7/GIC/gic.c
index 190b9953..9eb720fd 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)
@@ -34,7 +41,9 @@ int GIC_Install(char **Arguments)
 
 	// Initialise
 	gpGIC_InterfaceBase = (void*)MM_MapHWPages(gGIC_InterfaceAddr, 1);
+	LOG("gpGIC_InterfaceBase = %p", gpGIC_InterfaceBase);
 	gpGIC_DistributorBase = (void*)MM_MapHWPages(gGIC_DistributorAddr, 1);
+	LOG("gpGIC_DistributorBase = %p", gpGIC_DistributorBase);
 
 	gpGIC_InterfaceBase[GICC_PMR] = 0xFF;	
 	gpGIC_InterfaceBase[GICC_CTLR] = 1;	// Enable CPU
@@ -50,19 +59,32 @@ int GIC_Install(char **Arguments)
 void GIC_IRQHandler(void)
 {
 	Uint32	num = gpGIC_InterfaceBase[GICC_IAR];
-	Log_Debug("GIC", "IRQ 0x%x", num);
-	gpGIC_InterfaceBase[GICC_EOIR] = 1;
+//	Log_Debug("GIC", "IRQ 0x%x", num);
+	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;
 }