Rewrite CThunk so it does not execute from ram

Modify the implementation of CThunk so it does not execute from ram.
Instead is uses an array of functions in flash which can be allocated
as a thunk. The number of CThunks that can be used by an application
can be configured with MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX.

Author: c1728p9

platform/CThunk.h

@@ -38,53 +38,15 @@
 #ifndef __CTHUNK_H__
 #define __CTHUNK_H__
 
-#define CTHUNK_ADDRESS 1
-#define CTHUNK_VARIABLES volatile uint32_t code[2]
-
-#if (defined(__CORTEX_M3) || defined(__CORTEX_M4) || defined(__CORTEX_M7) || defined(__CORTEX_A9) \
-    || defined(__CORTEX_M33))
-/**
-* CTHUNK disassembly for Cortex-M3/M4/M7/A9 (thumb2):
-* * adr  r0, #4
-* * ldm  r0, {r0, r1, r2, pc}
-*
-* This instruction loads the arguments for the static thunking function to r0-r2, and
-* branches to that function by loading its address into PC.
-*
-* This is safe for both regular calling and interrupt calling, since it only touches scratch registers
-* which should be saved by the caller, and are automatically saved as part of the IRQ context switch.
-*/
-#define CTHUNK_ASSIGMENT do {                              \
-                             m_thunk.code[0] = 0xE890A001; \
-                             m_thunk.code[1] = 0x00008007; \
-                         } while (0)
-
-#elif (defined(__CORTEX_M0PLUS) || defined(__CORTEX_M0) || defined(__CORTEX_M23))
-/*
-* CTHUNK disassembly for Cortex M0/M0+ (thumb):
-* * adr  r0, #4
-* * ldm  r0, {r0, r1, r2, r3}
-* * bx   r3
-*/
-#define CTHUNK_ASSIGMENT do {                              \
-                             m_thunk.code[0] = 0xC80FA001; \
-                             m_thunk.code[1] = 0x00004718; \
-                         } while (0)
-
-#else
-#error "Target is not currently suported."
-#endif
-
-/* IRQ/Exception compatible thunk entry function */
-typedef void (*CThunkEntry)(void);
+#include "CThunkBase.h"
 
 /**
  * Class for created a pointer with data bound to it
  *
  * @note Synchronization level: Not protected
  */
 template<class T>
-class CThunk {
+class CThunk: public CThunkBase {
 public:
     typedef void (T::*CCallbackSimple)(void);
     typedef void (T::*CCallback)(void *context);
@@ -101,7 +63,8 @@ class CThunk {
 
     ~CThunk()
     {
-
+        cthunk_free(_entry);
+        _entry = NULL;
     }
 
     inline CThunk(T *instance, CCallbackSimple callback)
@@ -126,27 +89,30 @@ class CThunk {
 
     inline void callback(CCallback callback)
     {
-        m_callback = callback;
+        _callback = callback;
     }
 
     inline void callback(CCallbackSimple callback)
     {
-        m_callback = (CCallback)callback;
+        _callback_simple = callback;
     }
 
     inline void context(void *context)
     {
-        m_thunk.context = (uint32_t)context;
+        _context = context;
     }
 
     inline void context(uint32_t context)
     {
-        m_thunk.context = context;
+        _context = (void*)context;
     }
 
     inline uint32_t entry(void)
     {
-        return (((uint32_t)&m_thunk) | CTHUNK_ADDRESS);
+        if (_entry == NULL) {
+            _entry = cthunk_alloc(this);
+        }
+        return (uint32_t)_entry;
     }
 
     /* get thunk entry point for connecting rhunk to an IRQ table */
@@ -168,78 +134,34 @@ class CThunk {
     }
 
 private:
-    T *m_instance;
-    volatile CCallback m_callback;
-
-// TODO: this needs proper fix, to refactor toolchain header file and all its use
-// PACKED there is not defined properly for IAR
-#if defined (__ICCARM__)
-    typedef __packed struct {
-        CTHUNK_VARIABLES;
-        volatile uint32_t instance;
-        volatile uint32_t context;
-        volatile uint32_t callback;
-        volatile uint32_t trampoline;
-    }  CThunkTrampoline;
-#else
-    typedef struct {
-        CTHUNK_VARIABLES;
-        volatile uint32_t instance;
-        volatile uint32_t context;
-        volatile uint32_t callback;
-        volatile uint32_t trampoline;
-    } __attribute__((__packed__)) CThunkTrampoline;
-#endif
-
-    static void trampoline(T *instance, void *context, CCallback *callback)
-    {
-        if (instance && *callback) {
-            (static_cast<T *>(instance)->**callback)(context);
+    T *_instance;
+    void *_context;
+    union {
+        CCallbackSimple _callback_simple;
+        CCallback _callback;
+    };
+
+    CThunkEntry _entry;
+
+    static void trampoline(CThunkBase *base)
+    {
+        CThunk<T> *self = static_cast<CThunk<T>*>(base);
+        T *instance = self->_instance;
+        void *context = self->_context;
+        CCallback callback = self->_callback;
+
+        if (instance && callback) {
+            (instance->*callback)(context);
         }
     }
 
-    volatile CThunkTrampoline m_thunk;
-
     inline void init(T *instance, CCallback callback, void *context)
     {
-        /* remember callback - need to add this level of redirection
-           as pointer size for member functions differs between platforms */
-        m_callback = callback;
-
-        /* populate thunking trampoline */
-        CTHUNK_ASSIGMENT;
-        m_thunk.context = (uint32_t)context;
-        m_thunk.instance = (uint32_t)instance;
-        m_thunk.callback = (uint32_t)&m_callback;
-        m_thunk.trampoline = (uint32_t)&trampoline;
-
-#if defined(__CORTEX_A9)
-        /* Data cache clean */
-        /* Cache control */
-        {
-            uint32_t start_addr = (uint32_t)&m_thunk & 0xFFFFFFE0;
-            uint32_t end_addr   = (uint32_t)&m_thunk + sizeof(m_thunk);
-            uint32_t addr;
-
-            /* Data cache clean and invalid */
-            for (addr = start_addr; addr < end_addr; addr += 0x20) {
-                L1C_CleanInvalidateDCacheMVA((void *)addr);
-            }
-            /* Instruction cache invalid */
-            L1C_InvalidateICacheAll();
-            MMU_InvalidateTLB();
-            L1C_InvalidateBTAC();
-        }
-#endif
-#if defined(__CORTEX_M7)
-        /* Data cache clean and invalid */
-        SCB_CleanInvalidateDCache();
-
-        /* Instruction cache invalid */
-        SCB_InvalidateICache();
-#endif
-        __ISB();
-        __DSB();
+        _instance = instance;
+        _context = context;
+        _callback = callback;
+        _trampoline = &trampoline;
+        _entry = 0;
     }
 };
 

platform/CThunkBase.cpp

@@ -0,0 +1,144 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2018-2018 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "platform/platform.h"
+#include "platform/mbed_critical.h"
+#include "platform/mbed_assert.h"
+#include "platform/mbed_error.h"
+
+
+#include "CThunkBase.h"
+
+MBED_STATIC_ASSERT(MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX < 256, "MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX must be less than 256");
+MBED_STATIC_ASSERT(MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX > 0, "MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX must be greater than 0");
+
+#define ENABLE_N(N) ((MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX & N) ? 1 : 0)
+
+#define START_128   0
+#define START_64    (START_128 + ENABLE_N(128) * 128)
+#define START_32    (START_64  + ENABLE_N(64)  * 64)
+#define START_16    (START_32  + ENABLE_N(32)  * 32)
+#define START_8     (START_16  + ENABLE_N(16)  * 16)
+#define START_4     (START_8   + ENABLE_N(8)   * 8)
+#define START_2     (START_4   + ENABLE_N(4)   * 4)
+#define START_1     (START_2   + ENABLE_N(2)   * 2)
+
+#define DECLARE_THUNK128(start) \
+    DECLARE_THUNK64(start),     \
+    DECLARE_THUNK64(start + 64)
+#define DECLARE_THUNK64(start) \
+    DECLARE_THUNK32(start),     \
+    DECLARE_THUNK32(start + 32)
+#define DECLARE_THUNK32(start) \
+    DECLARE_THUNK16(start),     \
+    DECLARE_THUNK16(start + 16)
+#define DECLARE_THUNK16(start) \
+    DECLARE_THUNK8(start),      \
+    DECLARE_THUNK8(start + 8)
+#define DECLARE_THUNK8(start)  \
+    DECLARE_THUNK4(start),      \
+    DECLARE_THUNK4(start + 4)
+#define DECLARE_THUNK4(start)  \
+    DECLARE_THUNK2(start),      \
+    DECLARE_THUNK2(start + 2)
+#define DECLARE_THUNK2(start)  \
+    DECLARE_THUNK1(start),       \
+    DECLARE_THUNK1(start + 1)
+#define DECLARE_THUNK1(index)  &CThunkBase::thunk_entry<index>
+
+const CThunkEntry CThunkBase::_thunk_table[MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX] = {
+#if ENABLE_N(128)
+        DECLARE_THUNK128(START_128),
+#endif
+#if ENABLE_N(64)
+        DECLARE_THUNK64(START_64),
+#endif
+#if ENABLE_N(32)
+        DECLARE_THUNK32(START_32),
+#endif
+#if ENABLE_N(16)
+        DECLARE_THUNK16(START_16),
+#endif
+#if ENABLE_N(8)
+        DECLARE_THUNK8(START_8),
+#endif
+#if ENABLE_N(4)
+        DECLARE_THUNK4(START_4),
+#endif
+#if ENABLE_N(2)
+        DECLARE_THUNK2(START_2),
+#endif
+#if ENABLE_N(1)
+        DECLARE_THUNK1(START_1)
+#endif
+};
+
+CThunkBase *CThunkBase::_thunk_storage[MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX];
+
+CThunkBase::CthunkFree CThunkBase::_cthunk_free_real = NULL;
+
+CThunkEntry CThunkBase::cthunk_alloc(CThunkBase *cthunk)
+{
+    // Atomically allocate one entry
+    core_util_critical_section_enter();
+    CThunkEntry entry = NULL;
+    for (int i = 0; i < MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX; i++) {
+        if (_thunk_storage[i] == NULL) {
+            _thunk_storage[i] = cthunk;
+            entry = _thunk_table[i];
+            break;
+        }
+    }
+    core_util_critical_section_exit();
+
+    if (entry == NULL) {
+        MBED_ERROR(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_OUT_OF_RESOURCES), "Ran out of CThunk entries. Increase MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX to fix this error");
+    }
+
+    // Set function pointer on first use. This allows _thunk_table
+    // and _thunk_storage to get removed by the linker if
+    // cthunk_alloc is never used.
+    _cthunk_free_real = &cthunk_free_real;
+
+    return entry;
+}
+
+void CThunkBase::cthunk_free(CThunkEntry item)
+{
+    if (_cthunk_free_real) {
+        _cthunk_free_real(item);
+    }
+}
+
+void CThunkBase::cthunk_free_real(CThunkEntry item)
+{
+    bool found = false;
+
+    core_util_critical_section_enter();
+    for (int i = 0; i < MBED_CONF_PLATFORM_CTHUNK_COUNT_MAX; i++) {
+        if (_thunk_table[i] == item) {
+            _thunk_storage[i] = NULL;
+            found = true;
+            break;
+        }
+    }
+    core_util_critical_section_exit();
+
+    if (!found) {
+        MBED_ERROR(MBED_MAKE_ERROR(MBED_MODULE_PLATFORM, MBED_ERROR_CODE_INVALID_ARGUMENT), "Tried to free invalid CThunkEntry");
+    }
+
+}