Merge pull request #8488 from c1728p9/cthunk_overhaul

Rewrite CThunk so it does not execute from ram

Author: Cruz Monrreal

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");
+    }
+
+}