From 7c165d29b989a15e2fd23a6d17c08d1981411bc3 Mon Sep 17 00:00:00 2001
From: Vegard Storheil Eriksen <zyp@jvnv.net>
Date: Sun, 4 Dec 2011 02:05:02 +0100
Subject: Created member function interrupt gate.

---
 SConstruct        |  2 +-
 hal/interrupt.cpp | 14 +++++++++++++-
 hal/interrupt.h   | 23 ++++++++++++++++++++++-
 3 files changed, 36 insertions(+), 3 deletions(-)

diff --git a/SConstruct b/SConstruct
index b596946..133ec8f 100644
--- a/SConstruct
+++ b/SConstruct
@@ -6,7 +6,7 @@ env = Environment(
 	CC = 'arm-none-eabi-gcc',
 	CXX = 'arm-none-eabi-g++',
 	AS = 'arm-none-eabi-gcc',
-	CCFLAGS = '-O2 -Wall -ggdb -mcpu=cortex-m4 -mthumb -mhard-float -ffunction-sections',
+	CCFLAGS = '-O2 -Wall -ggdb -mcpu=cortex-m4 -mthumb -mhard-float -ffunction-sections -Wno-pmf-conversions',
 	CXXFLAGS = '-fno-exceptions -fno-rtti',
 	ASFLAGS = '-c -x assembler-with-cpp -mcpu=cortex-m4 -mthumb -mhard-float',
 	
diff --git a/hal/interrupt.cpp b/hal/interrupt.cpp
index e9e7b24..611b09c 100644
--- a/hal/interrupt.cpp
+++ b/hal/interrupt.cpp
@@ -1,9 +1,21 @@
 #include "interrupt.h"
 
+namespace Interrupt {
+	MFP mf_vectors[16 + NUM_IRQs];
+};
+
 void entry();
 
+void member_function_interrupt_gate() {
+	uint32_t interrupt_num;
+	asm ("mrs %0, ipsr" : "=r" (interrupt_num));
+	
+	Interrupt::mf_vectors[interrupt_num].func_p(Interrupt::mf_vectors[interrupt_num].instance_p);
+}
+
 extern "C" void unused_interrupt() {
-	while(1) {}
+	member_function_interrupt_gate();
+	//while(1);
 }
 
 template<> void interrupt<Interrupt::NMI>()             __attribute__ ((weak, alias ("unused_interrupt")));
diff --git a/hal/interrupt.h b/hal/interrupt.h
index f817103..34651f2 100644
--- a/hal/interrupt.h
+++ b/hal/interrupt.h
@@ -75,7 +75,8 @@ namespace Interrupt {
 		DMA2_Channel1,
 		DMA2_Channel2,
 		DMA2_Channel3,
-		DMA2_Channel4_5
+		DMA2_Channel4_5,
+		NUM_IRQs
 	};
 	
 	inline void enable(IRQ n) {
@@ -89,6 +90,26 @@ namespace Interrupt {
 	inline void set_priority(IRQ n, uint8_t priority) {
 		NVIC.IPR[n] = priority;
 	}
+	
+	struct MFP {
+		void (*func_p)(void*);
+		void* instance_p;
+	};
+	
+	extern MFP mf_vectors[];
+	
+	template<class T>
+	inline void set_handler(IRQ n, void (T::*f)(), T* i) {
+		MFP& mfp = mf_vectors[16 + n];
+		mfp.func_p = reinterpret_cast<void (*)(void*)>(f);
+		mfp.instance_p = i;
+	}
+	
+	template<class T>
+	inline void enable(IRQ n, void (T::*f)(), T* i) {
+		set_handler(n, f, i);
+		enable(n);
+	}
 };
 
 template<Interrupt::Exception>
-- 
cgit v1.2.3