Implemented irq handler on irq 2

Signed-off-by: Damien Zammit <damien@zamaudio.com>

3 files changed, 27 insertions, 36 deletions

diff --git a/firmware/servicereq.c b/firmware/servicereq.c
index 7f8ce57..949146d 100644
--- a/firmware/servicereq.c
+++ b/firmware/servicereq.c
@@ -5553,7 +5553,7 @@ end:
 	return;
 }
 
-void smu_service_request(void)
+void smu_service_request(unsigned int level, void* dummy)
 {
 
 	static ddiphy_t ddiphy = {{0}};
@@ -5652,11 +5652,11 @@ void smu_service_request(void)
 	default:
 		break;
 	}
+	write32(0xe0003004, 3);
 }
 
 void MicoISRHandler(void)
 {
-/*
 	unsigned int ip, im, Mask, IntLevel;
 	asm volatile ("rcsr %0,im":"=r"(im));
 
@@ -5669,23 +5669,23 @@ void MicoISRHandler(void)
 		IntLevel = 0;
 
 		if( ip!=0 ){
-		do {
-			if(Mask & ip) {
-				if(ISREntryTable[IntLevel].Callback != 0){
-					(ISREntryTable[IntLevel].Callback)(IntLevel, ISREntryTable[IntLevel].Context);
-					asm volatile ("wcsr ip, %0"::"r"(Mask));
-					break;
-				} else {
-					asm volatile ("rcsr %0,im":"=r"(im));
-					im &= ~Mask;
-					asm volatile ("wcsr im, %0"::"r"(im));
-					asm volatile ("wcsr ip, %0"::"r"(Mask));
-					break;
+			do {
+				if(Mask & ip) {
+					if(ISREntryTable[IntLevel].Callback != 0){
+						(ISREntryTable[IntLevel].Callback)(IntLevel, ISREntryTable[IntLevel].Context);
+						asm volatile ("wcsr ip, %0"::"r"(Mask));
+						break;
+					} else {
+						asm volatile ("rcsr %0,im":"=r"(im));
+						im &= ~Mask;
+						asm volatile ("wcsr im, %0"::"r"(im));
+						asm volatile ("wcsr ip, %0"::"r"(Mask));
+						break;
+					}
 				}
-			}
-			Mask = Mask << 0x1;
-			++IntLevel;
-		} while(1);
+				Mask = Mask << 0x1;
+				++IntLevel;
+			} while(1);
 
 		} else {
 			break;
@@ -5694,6 +5694,5 @@ void MicoISRHandler(void)
 	} while(1);
 
 	//OSIntExit();
-*/	
 	return;
 }
diff --git a/firmware/servicereq.h b/firmware/servicereq.h
index 9c6dbd1..c67233b 100644
--- a/firmware/servicereq.h
+++ b/firmware/servicereq.h
@@ -20,7 +20,7 @@ typedef struct ISREntry{
 	void *Context;
 } ISREntry_t;
 
-//static ISREntry_t ISREntryTable[32];
+static ISREntry_t ISREntryTable[32];
 
 void MicoISRHandler(void);
-void smu_service_request(void);
+void smu_service_request(unsigned int, void*);
diff --git a/firmware/smu.c b/firmware/smu.c
index 8a2de21..b29c7fb 100644
--- a/firmware/smu.c
+++ b/firmware/smu.c
@@ -20,36 +20,28 @@
 void main(void)
 {
 	int ie;
-//	int mask, im;
-//	int irq = 1;
-//	mask = 0x1 << irq; // << irq
+	int mask, im;
+	int irq = 2;
+	mask = 0x1 << irq;
 
 	/* disable interrupts */
 	asm volatile ("rcsr %0,ie":"=r"(ie));
 	ie &= (~0x1);
 	asm volatile ("wcsr ie, %0"::"r"(ie));
 
-/**** Enable interrupts and handler
 	ISREntryTable[irq].Callback = &smu_service_request;
-	ISREntryTable[irq].Context = 0; //?
+	ISREntryTable[irq].Context = 0;
 
+	/* enable interrupt handler 2 */
 	asm volatile ("rcsr %0, im":"=r"(im));
 	im |= mask;
 	asm volatile ("wcsr im, %0"::"r"(im));
 
+	/* enable interrupts */
 	ie |= 0x1;
 	asm volatile ("wcsr ie, %0"::"r"(ie));
-*/
-	write32(0xe0003004, 3); // Assume interrupt is done and acked
-	
+
 	while (1) {
 		mdelay(10);
-
-		if (read32(0xe0003000)) {
-			write32(0xe0003004, 1); // Interrupt ack
-			smu_service_request();  // Do service request
-			write32(0xe0003000, 0); // Clear service request
-			write32(0xe0003004, 3); // Interrupt done + ack
-		}
 	}
 }