Diff: CThunk.h

Revision:

4:e4a106e8f3fe

Parent:

3:51023d181133

Child:

5:df90d98292a6

--- a/CThunk.h	Wed Aug 20 07:49:00 2014 +0000
+++ b/CThunk.h	Wed Aug 20 10:44:31 2014 +0000
@@ -95,23 +95,22 @@
         }
 
     private:
+        T* m_instance;
         volatile CCallback m_callback;
+
         typedef struct
         {
             volatile uint32_t code;
             volatile uint32_t instance;
             volatile uint32_t context;
             volatile uint32_t callback;
+            volatile uint32_t trampoline;
         } __attribute__((packed)) CThunkTrampoline;
 
-        inline void callback(void* context)
+        static void trampoline(T* instance, void* context, CCallback* callback)
         {
-            pc.printf("Trampoline: context=0x%08X\n", context);
-        }
-
-        static void trampoline(void* instance, void* context)
-        {
-            ((CThunk<T>*)instance)->callback(context);
+            if(instance && *callback)
+                (static_cast<T*>(instance)->**callback)(context);
         }
 
         volatile CThunkTrampoline m_thunk;
@@ -119,14 +118,18 @@
         inline void init(T *instance, CCallback callback, void* context)
         {
 #ifdef __CORTEX_M3
-            m_thunk.code = 0x8003E89F;
+            m_thunk.code = 0x8007E89F;
 #else
 #error "TODO: add support for non-cortex-m3 trampoline, too"
 #endif
+            m_callback = callback;
+
+            /* populate thunking trampoline */
             m_thunk.context = (uint32_t)context;
             m_thunk.instance = (uint32_t)instance;
-            m_thunk.callback = (uint32_t)&trampoline;
-            m_callback = callback;
+            m_thunk.callback = (uint32_t)&m_callback;
+            m_thunk.trampoline = (uint32_t)&trampoline;
+
             __ISB();
             __DSB();
         }