stk_arch_arm-cortex-m (5).cpp

Go to the documentation of this file.

/*
 * SuperTinyKernel™ (STK): Lightweight High-Performance Deterministic C++ RTOS for Embedded Systems.
 *
 * Source: https://github.com/SuperTinyKernel-RTOS
 *
 * Copyright (c) 2022-2026 Neutron Code Limited <stk@neutroncode.com>. All Rights Reserved.
 * License: MIT License, see LICENSE for a full text.
 */
 
// note: If missing, this header must be customized (get it in the root of the source folder) and
//       copied to the /include folder manually.
#include "stk_config.h"
 
#ifdef _STK_ARCH_ARM_CORTEX_M
 
//#define STK_CORTEX_M_TRUSTZONE
 
#ifdef STK_CORTEX_M_TRUSTZONE
#include <arm_cmse.h>
#endif
 
#include "stk.h"
#include "stk_arch.h"
#include "arch/stk_arch_common.h"
 
using namespace stk;

#ifndef __CORTEX_M
#error Expecting __CORTEX_M with value corresponding to Cortex-M model (0, 3, 4, ...)!
#endif

#if !defined(SysTick)
    #error "SysTick peripheral definition is missing"
#endif

#if (__CORTEX_M > 1U) && STK_TICKLESS_USE_ARM_DWT && !defined(DWT)
    #error "DWT peripheral definition is missing"
#endif

#define STK_CORTEX_M_FPU ((__FPU_PRESENT == 1U) && (__FPU_USED == 1U))

#define STK_CORTEX_M_MANAGE_LR (__CORTEX_M >= 3U)
 
// Non-Secure (and pre-ARMv8-M) EXC_RETURN values -- S bit (bit 6) = 0.
#define STK_CORTEX_M_EXC_RETURN_HANDLR_MSP 0xFFFFFFF1U // Handler mode, MSP stack (Non-Secure)
#define STK_CORTEX_M_EXC_RETURN_THREAD_MSP 0xFFFFFFF9U // Thread mode, MSP stack (Non-Secure)
#define STK_CORTEX_M_EXC_RETURN_THREAD_PSP 0xFFFFFFFDU // Thread mode, PSP stack (Non-Secure)
 
// ARMv8-M TrustZone Secure EXC_RETURN values -- S bit (bit 6) = 1.
// These are only meaningful when STK_CORTEX_M_TRUSTZONE is defined on M23/M33+.
// Reference: ARMv8-M Architecture Reference Manual, section B1.5.8.
#define STK_CORTEX_M_EXC_RETURN_S_HANDLR_MSP  0xFFFFFFE1U // Secure, Handler mode, MSP
#define STK_CORTEX_M_EXC_RETURN_S_THREAD_MSP  0xFFFFFFE9U // Secure, Thread mode, MSP
#define STK_CORTEX_M_EXC_RETURN_S_THREAD_PSP  0xFFFFFFEDU // Secure, Thread mode, PSP
// Non-Secure return with Non-Secure FPU state saved (used for NS tasks on M33 with FPU).
#define STK_CORTEX_M_EXC_RETURN_NS_THREAD_PSP 0xFFFFFFBCU // Non-Secure, Thread, PSP, NS-FPU

#define STK_CORTEX_M_EXC_RETURN_S_BIT         0x00000040U
 
#define STK_CORTEX_M_ISR_PRIORITY_HIGHEST  0
#define STK_CORTEX_M_ISR_PRIORITY_LOWEST   0xFF

#if STK_CORTEX_M_MANAGE_LR
    #define STK_CORTEX_M_REGISTER_COUNT 17
#else
    #define STK_CORTEX_M_REGISTER_COUNT 16
#endif

#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    #define STK_CORTEX_M_TZ_REGISTER_COUNT 2
#else
    #define STK_CORTEX_M_TZ_REGISTER_COUNT 0
#endif

#define STK_CORTEX_M_TOTAL_REGISTER_COUNT \
    (STK_CORTEX_M_REGISTER_COUNT + STK_CORTEX_M_TZ_REGISTER_COUNT)

#ifndef STK_SYSTICK_HANDLER
    #define STK_SYSTICK_HANDLER SysTick_Handler
#endif

#ifndef STK_PENDSV_HANDLER
    #define STK_PENDSV_HANDLER PendSV_Handler
#endif

#ifndef STK_SVC_HANDLER
    #define STK_SVC_HANDLER SVC_Handler
#endif

enum ESvcCommandId : uint8_t
{
    SVC_START_SCHEDULING = 0,
    SVC_ENTER_CRITICAL,
    SVC_EXIT_CRITICAL,
    //SVC_FORCE_SWITCH
};

#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
struct TrustZoneFrame
{
    Word PSPLIM;    
    Word PSPLIM_NS; 
};
#endif

struct ExceptionFrame
{
    Word R0;
    Word R1;
    Word R2;
    Word R3;
    Word R12;
    Word LR;
    Word PC;
    Word PSR;
};

struct TaskFrame
{
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    TrustZoneFrame tz;          
#endif
#if STK_CORTEX_M_MANAGE_LR
    Word           EXC_RETURN; 
#endif
    ExceptionFrame exc;        
};
 
// Shortcuts:
#define STK_ASM_EXIT_FROM_HANDLER  "BX LR"  // use in naked exception/ISR handlers
#define STK_ASM_EXIT_FROM_FUNCTION "BX LR"  // use in naked C-callable wrapper functions
#define STK_ASM_DISABLE_INTERRUPTS "CPSID i"
#define STK_ASM_ENABLE_INTERRUPTS  "CPSIE i"

static __stk_forceinline void HW_StartScheduler()
{
    __asm volatile("SVC %0"
    : /* output: none */
    : "I"(SVC_START_SCHEDULING)
    : "memory" /* protect against compiler reordering */ );
}

#ifdef SVC_FORCE_SWITCH
static __stk_forceinline void HW_ForceContextSwitch()
{
    __asm volatile("SVC %0"
    : /* output: none */
    : "I"(SVC_FORCE_SWITCH)
    : "memory" /* protect against compiler reordering */ );
}
#endif

__stk_attr_naked Word HW_SVCEnterCritical()
{
    __asm volatile(
    "SVC %0                      \n"
    STK_ASM_EXIT_FROM_FUNCTION " \n"
    : /* output: none */
    : "I"(SVC_ENTER_CRITICAL)
    : "memory" /* protect against compiler reordering */ );
}

__stk_attr_naked void HW_SVCExitCritical(Word /*prev*/)
{
    __asm volatile(
    "SVC %0                      \n"
    STK_ASM_EXIT_FROM_FUNCTION " \n"
    : /* output: none */
    : "I"(SVC_EXIT_CRITICAL)
    : "memory" /* protect against compiler reordering */);
}

static __stk_forceinline void HW_DisableInterrupts()
{
#if (defined(__clang__) && defined(__ARMCOMPILER_VERSION)) || defined(__ICCARM__)
    __asm volatile(STK_ASM_DISABLE_INTERRUPTS ::: "memory");
#else
    __disable_irq();
#endif
}

static __stk_forceinline void HW_EnableInterrupts()
{
#if (defined(__clang__) && defined(__ARMCOMPILER_VERSION)) || defined(__ICCARM__)
    __asm volatile(STK_ASM_ENABLE_INTERRUPTS ::: "memory");
#else
    __enable_irq();
#endif
}

static __stk_forceinline bool HW_InterruptsDisabled()
{
#if (defined(__clang__) && defined(__ARMCOMPILER_VERSION)) || defined(__ICCARM__)
    Word primask;
    __asm volatile("MRS %0, primask" : "=r"(primask));
    return (primask & 1U);
#else
    return (__get_PRIMASK() & 1U);
#endif
}

static __stk_forceinline void HW_CriticalSectionStart(uint32_t &SES)
{
    SES = __get_PRIMASK();
    HW_DisableInterrupts();
 
    // ensure the disable is recognized before subsequent code
    __DSB();
    __ISB();
}

static __stk_forceinline void HW_CriticalSectionEnd(uint32_t SES)
{
    // ensure all memory work is finished before re-enabling
    __DSB();
 
    __set_PRIMASK(SES);
 
    // synchronization point: any pending interrupt can be serviced immediately at this boundary
    __ISB();
}
 
#ifdef CONTROL_nPRIV_Msk
static __stk_forceinline bool HW_SpinLockTryLock(volatile bool &lock)
{
    return !__atomic_test_and_set(&lock, __ATOMIC_ACQUIRE);
}

static __stk_forceinline void HW_SpinLockUnlock(volatile bool &lock)
{
    if (!lock)
        STK_KERNEL_PANIC(KERNEL_PANIC_SPINLOCK_DEADLOCK); // release attempt of unowned lock
 
    // ensure all data writes (like scheduling metadata) are flushed before the lock is released:
    // __atomic_clear with __ATOMIC_RELEASE provides the required store-release barrier,
    // the explicit dmb ishst is retained for toolchains that do not lower __ATOMIC_RELEASE
    // to a full DMB on ARMv7-M (e.g. older GCC versions with -mcpu=cortex-m4)
    __asm volatile("dmb ishst" ::: "memory");
 
    __atomic_clear(&lock, __ATOMIC_RELEASE);
}
#elif defined(RP2040_H)
// Raspberry RP2040 dual-core M0+ implementation, using Hardware Spinlock 0 (SIO base 0xd0000000 + offset)
#define STK_SIO_SPINLOCK SIO->SPINLOCK31

static __stk_forceinline bool HW_SpinLockTryLock(volatile bool &lock)
{
    bool success = (STK_SIO_SPINLOCK == 0 ? false : ((lock) = true, true));
    __DMB();
 
    return success;
}

static __stk_forceinline void HW_SpinLockUnlock(volatile bool &lock)
{
    if (!lock)
        STK_KERNEL_PANIC(KERNEL_PANIC_SPINLOCK_DEADLOCK); // release attempt of unowned lock
 
    __DMB();
    (lock) = false;
    STK_SIO_SPINLOCK = 1; // writing any value releases the hardware lock
}
 
#undef STK_SIO_SPINLOCK
#else // !RP2040_H
// Standard single-core Cortex-M0 implementation:

static __stk_forceinline bool HW_SpinLockTryLock(volatile bool &lock)
{
    uint32_t ses;
    HW_CriticalSectionStart(ses);
 
    if (lock)
    {
        HW_CriticalSectionEnd(ses);
        return false;
    }
 
    lock = true;
    __DMB();
 
    HW_CriticalSectionEnd(ses);
    return true;
}

static __stk_forceinline void HW_SpinLockUnlock(volatile bool &lock)
{
    if (!lock)
        STK_KERNEL_PANIC(KERNEL_PANIC_SPINLOCK_DEADLOCK); // release attempt of unowned lock
 
    __DMB();
    lock = false;
}
#endif // CONTROL_nPRIV_Msk

static __stk_forceinline void HW_SpinLockLock(volatile bool &lock)
{
    uint32_t timeout = 0xFFFFFF;
    while (!HW_SpinLockTryLock(lock))
    {
        if (--timeout == 0)
        {
            // invariant violated: the lock owner exited without releasing
            STK_KERNEL_PANIC(KERNEL_PANIC_SPINLOCK_DEADLOCK);
        }
        __stk_relax_cpu();
    }
}


struct JmpFrame
{
    Word R4, R5, R6, R7, R8, R9, R10, R11; 
    Word SP;    
    Word LR;    
#if STK_CORTEX_M_FPU
    Word FPSCR; 
#endif
};

__stk_attr_naked
int32_t SaveJmp(JmpFrame &/*f*/)
{
    __asm volatile(
    ".syntax unified                \n"
#if (__CORTEX_M >= 3U)
    // Cortex-M3/M4/M7: STMIA stores r4-r11 at r0+0 .. r0+28
    "STMIA r0,  {r4-r11}            \n" // store r4-r11 at offsets 0-28, no writeback
    "STR   sp,  [r0, #32]           \n" // SP at offset 32
    "STR   lr,  [r0, #36]           \n" // LR at offset 36
#else
    // Cortex-M0/M0+/M1: Thumb-1 only
    "STR  r4,  [r0, #0]             \n"
    "STR  r5,  [r0, #4]             \n"
    "STR  r6,  [r0, #8]             \n"
    "STR  r7,  [r0, #12]            \n"
    "MOV  r1,  r8                   \n"
    "STR  r1,  [r0, #16]            \n"
    "MOV  r1,  r9                   \n"
    "STR  r1,  [r0, #20]            \n"
    "MOV  r1,  r10                  \n"
    "STR  r1,  [r0, #24]            \n"
    "MOV  r1,  r11                  \n"
    "STR  r1,  [r0, #28]            \n"
    "MOV  r1,  sp                   \n"
    "STR  r1,  [r0, #32]            \n"
    "MOV  r1,  lr                   \n"
    "STR  r1,  [r0, #36]            \n"
#endif
 
#if STK_CORTEX_M_FPU
    "VMRS r1,  FPSCR                \n"
    "STR  r1,  [r0, #40]            \n"
#endif
    "MOVS r0,  #0                   \n"
    "BX   lr                        \n");
}

__stk_attr_naked
__stk_attr_noreturn
void RestoreJmp(JmpFrame &/*f*/, int32_t /*val*/)
{
    __asm volatile(
    ".syntax unified                \n"
#if (__CORTEX_M >= 3U)
    // Cortex-M3/M4/M7: LDMIA loads r4-r11 from offsets 0-28
    "LDR   sp,  [r0, #32]           \n" // restore SP
#if STK_CORTEX_M_FPU
    "LDR   r2,  [r0, #40]           \n" // load saved FPSCR
    "VMSR  FPSCR, r2                \n" // restore rounding mode + flags
#endif
    "LDR   r2,  [r0, #36]           \n" // load saved LR into r2
    "LDMIA r0,  {r4-r11}            \n" // restore r4-r11, no writeback
    "MOV   r0,  r1                  \n" // return val
    "BX    r2                       \n"
#else
    // Cortex-M0/M0+/M1: Thumb-1 only
    "LDR  r2,  [r0, #36]            \n"
    "MOV  lr,  r2                   \n"
    "LDR  r2,  [r0, #32]            \n"
    "MOV  sp,  r2                   \n"
    "LDR  r2,  [r0, #28]            \n"
    "MOV  r11, r2                   \n"
    "LDR  r2,  [r0, #24]            \n"
    "MOV  r10, r2                   \n"
    "LDR  r2,  [r0, #20]            \n"
    "MOV  r9,  r2                   \n"
    "LDR  r2,  [r0, #16]            \n"
    "MOV  r8,  r2                   \n"
    "LDR  r4,  [r0, #0]             \n"
    "LDR  r5,  [r0, #4]             \n"
    "LDR  r6,  [r0, #8]             \n"
    "LDR  r7,  [r0, #12]            \n"
    "MOV  r0,  r1                   \n"  // return val
    "BX   lr                        \n"
#endif
    );
}
 
// -----------------------------------------------------------------------------

static __stk_forceinline bool HW_IsHandlerMode()
{
    return (__get_IPSR() != 0U);
}

static __stk_forceinline bool HW_IsPrivilegedThreadMode()
{
#ifdef CONTROL_nPRIV_Msk
    return ((__get_CONTROL() & CONTROL_nPRIV_Msk) == 0U);
#else
    return true;
#endif
}

static __stk_forceinline Word HW_GetCallerSP()
{
    // use SP (R13) directly: __get_PSP() returns 0 in unprivileged thread mode
    Word sp;
    __asm volatile("MOV %0, sp" : "=r" (sp));
    return sp;
}

static __stk_forceinline void HW_ScheduleContextSwitch()
{
    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
}

static __stk_forceinline void HW_ClearFpuState()
{
#if STK_CORTEX_M_FPU
    __set_CONTROL(__get_CONTROL() & ~CONTROL_FPCA_Msk);
#endif
}

static __stk_forceinline void HW_EnableFullFpuAccess()
{
#if STK_CORTEX_M_FPU
    // enable FPU CP10/CP11 Secure and Non-secure register access
    SCB->CPACR |= (0b11 << 20) | (0b11 << 22);
#endif
}

static __stk_forceinline uint32_t HW_CoreClockFrequency()
{
    return SystemCoreClock;
}

static __stk_forceinline void HW_SysTickStart(uint32_t period_ticks)
{
    uint32_t result = SysTick_Config(static_cast<uint32_t>(ConvertTimeUsToClockCycles(HW_CoreClockFrequency(), period_ticks)));
    STK_ASSERT(result == 0U);
    (void)result;
 
    // QEMU workaround (Launchpad Bug #1872237):
    // SysTick_Config() writes VAL=0 before setting ENABLE=1. On QEMU,
    // systick_reload() silently discards VAL writes while ENABLE=0, leaving
    // the internal tick accumulator stale from the previous period.
    // Writing VAL=0 here, after ENABLE=1 is already set, forces a correct reload.
    // This is a no-op on real Cortex-M hardware (spec: ARMv7-M ARM B3.3.1).
    SysTick->VAL = 0U;
}

static __stk_forceinline void HW_SysTickStop()
{
    SysTick->CTRL = 0U;
    SCB->ICSR = SCB_ICSR_PENDSTCLR_Msk;
}

static __stk_forceinline void HW_SysTickDisable()
{
    SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
}

static __stk_forceinline uint32_t HW_SysTickValueAfterDisable()
{
    // is required for QEMU which then resets SysTick->VAL and thus we can't calculate elapsed time correctly
    __DSB();
 
    // check for a QEMU case and discard elapsed result
    uint32_t VAL = SysTick->VAL;
    if (VAL == 0)
        VAL = SysTick->LOAD;
 
    return VAL;
}

static __stk_forceinline uint32_t HW_SysTickGetElapsed(uint32_t VAL)
{
    return SysTick->LOAD - VAL;
}

static __stk_forceinline void HW_SysTickRearm(uint32_t ticks)
{
    SysTick->LOAD  = ticks - 1U;
    SysTick->VAL   = 0U;
    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;
}

static __stk_forceinline void HW_DWTEnableCounter()
{
#if defined(CoreDebug)
    // enable Trace and Debug blocks (DWT, ITM, ETM, TPIU)
    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
    __DSB();
#endif
 
    // LAR (Lock Access Register) is mandatory for Cortex-M7 to allow register writes,
    // it is typically not implemented or deprecated on M0, M3, M4, and M33
#if (__CORTEX_M == 7U)
    DWT->LAR = 0xC5ACCE55; // unlock DWT unit using standard CoreSight magic key
    __DSB();
#endif
 
#if defined(DWT)
    // disable counter briefly to safely reset the value to zero
    DWT->CTRL &= ~DWT_CTRL_CYCCNTENA_Msk;
    DWT->CYCCNT = 0;
 
    // enable
    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
#endif
}

static __stk_forceinline uint32_t HW_DWTGetCounter()
{
#if defined(DWT)
    return DWT->CYCCNT;
#else
    return 0U;
#endif
}

static volatile bool s_StkCortexmCsuLock = false;

static struct Context : public PlatformContext
{
    Context() : PlatformContext(), m_exit_buf(), m_overrider(nullptr),
    #if STK_TICKLESS_IDLE
        m_sleep_ticks(0), m_sleep_error(0U),
    #endif
        m_csu(0U), m_csu_nesting(0U), m_started(false), m_exiting(false)
    {}

    ~Context()
    {}
 
    void Initialize(IPlatform::IEventHandler *handler, IKernelService *service, Stack *exit_trap,
        uint32_t resolution_us)
    {
        PlatformContext::Initialize(handler, service, exit_trap, resolution_us);
 
        STK_STATIC_ASSERT_DESC_N(SP, offsetof(Stack, SP) == 0U,
            "expect Stack::mode member at offset of 0 (first member)");
        STK_STATIC_ASSERT_DESC_N(mode, offsetof(Stack, mode) == 4U,
            "expect Stack::mode member at offset of 4 (second member)");
 
        m_csu         = 0U;
        m_csu_nesting = 0U;
        m_overrider   = nullptr;
        m_started     = false;
        m_exiting     = false;
 
    #if (__CORTEX_M > 1) && STK_TICKLESS_USE_ARM_DWT
        HW_DWTEnableCounter();
    #endif
 
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_Init(
                HW_CoreClockFrequency(),
                HW_CoreClockFrequency(),
                nullptr,
                SendSysDesc);
    #endif
    }
 
    __stk_forceinline void OnTick()
    {
        HW_DisableInterrupts();
 
    #if STK_TICKLESS_IDLE
        Timeout ticks = m_sleep_ticks;
    #endif
 
        if (m_handler->OnTick(m_stack_idle, m_stack_active
        #if STK_TICKLESS_IDLE
                , ticks
        #endif
        ))
        {
            #if STK_SEGGER_SYSVIEW
            SEGGER_SYSVIEW_OnTaskStopExec();
            if (GetContext().m_stack_active->tid != SYS_TASK_ID_SLEEP)
            SEGGER_SYSVIEW_OnTaskStartExec(GetContext().m_stack_active->tid);
            #endif
 
            HW_ScheduleContextSwitch();
        }
 
        // rearm SysTick only if tick period changed
    #if STK_TICKLESS_IDLE
        if (ticks != m_sleep_ticks)
        {
            ReloadTickPeriod(ticks);
            m_sleep_ticks = ticks;
        }
    #endif
 
        HW_EnableInterrupts();
    }
 
    __stk_forceinline void EnterCriticalSection()
    {
        // disable local interrupts first to prevent self-deadlock
        uint32_t current_ses;
        HW_CriticalSectionStart(current_ses);
 
        if (m_csu_nesting == 0U)
        {
            // ONLY attempt the global spinlock if we aren't already nested
            HW_SpinLockLock(s_StkCortexmCsuLock);
 
            // store the hardware interrupt state to restore later
            m_csu = current_ses;
        }
 
        // increase nesting count within a limit
        if (++m_csu_nesting > STK_CRITICAL_SECTION_NESTINGS_MAX)
        {
            // invariant violated: exceeded max allowed number of recursions
            STK_KERNEL_PANIC(KERNEL_PANIC_CS_NESTING_OVERFLOW);
        }
    }
 
    __stk_forceinline void ExitCriticalSection()
    {
        STK_ASSERT(m_csu_nesting != 0U);
        --m_csu_nesting;
 
        if (m_csu_nesting == 0U)
        {
            // capture the state before releasing lock
            uint32_t ses_to_restore = m_csu;
 
            // release global lock
            HW_SpinLockUnlock(s_StkCortexmCsuLock);
 
            // restore hardware interrupts
            HW_CriticalSectionEnd(ses_to_restore);
        }
    }
 
    // Unprivileged
    __stk_forceinline void UnprivEnterCriticalSection()
    {
        // elevate to privileged/disabled state via SVC
        Word current_ses = HW_SVCEnterCritical();
 
        if (m_csu_nesting == 0U)
        {
            // ONLY attempt the global spinlock if we aren't already nested
            HW_SpinLockLock(s_StkCortexmCsuLock);
 
            // store the hardware interrupt state to restore later
            m_csu = current_ses;
        }
 
        // increase nesting count within a limit
        if (++m_csu_nesting > STK_CRITICAL_SECTION_NESTINGS_MAX)
        {
            // invariant violated: exceeded max allowed number of recursions
            STK_KERNEL_PANIC(KERNEL_PANIC_CS_NESTING_OVERFLOW);
        }
    }
 
    // Unprivileged
    __stk_forceinline void UnprivExitCriticalSection()
    {
        STK_ASSERT(m_csu_nesting != 0U);
        --m_csu_nesting;
 
        if (m_csu_nesting == 0U)
        {
            // capture the state before releasing lock
            Word ses_to_restore = m_csu;
 
            // release global lock
            HW_SpinLockUnlock(s_StkCortexmCsuLock);
 
            // restore hardware interrupts via SVC
            HW_SVCExitCritical(ses_to_restore);
        }
    }
 
    void Start();
    void OnStart();
    void OnStop();
#if STK_TICKLESS_IDLE
    void ReloadTickPeriod(Timeout ticks_requested);
#endif
 
    typedef IPlatform::IEventOverrider eovrd_t;
 
    JmpFrame      m_exit_buf;    
    eovrd_t      *m_overrider;   
#if STK_TICKLESS_IDLE
    Timeout       m_sleep_ticks; 
    uint32_t      m_sleep_error; 
#endif
    Word          m_csu;         
    uint8_t       m_csu_nesting; 
    volatile bool m_started;     
    bool          m_exiting;     
}
s_StkPlatformContext[STK_ARCH_CPU_COUNT];
 
// ---------------------------------------------------------------------------
// TrustZone TLS helpers (Change ⑤ / Fix B)
//
// Placed here — after the Context struct is fully defined — so no forward
// declaration is needed.  GetContext() is a macro (stk_arch_common.h) that
// expands to s_StkPlatformContext[cpu_id], which is now visible.
//
// When STK_CORTEX_M_TRUSTZONE is active on ARMv8-M, r9 is the Non-Secure
// base register (SB) in the FDPIC ABI and must not be used as a TLS register
// across world transitions.  The TLS word is instead kept in Stack::tls so it
// is saved and restored automatically on every context switch.
//
// GetTls() / SetTls() in the header forward to these two C-linkage functions
// to avoid a circular include dependency (the header cannot see Context).
// ---------------------------------------------------------------------------
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
 
extern "C" stk::Word PlatformArmCortexM_GetTlsFromContext()
{
    return GetContext().m_stack_active->tls;
}
 
extern "C" void PlatformArmCortexM_SetTlsInContext(stk::Word tp)
{
    GetContext().m_stack_active->tls = tp;
}
 
#endif // STK_CORTEX_M_TRUSTZONE && __CORTEX_M >= 33U
// ---------------------------------------------------------------------------

class HiResClockDWT
{
    Cycles   m_acc;
    uint32_t m_prev;
 
public:
    HiResClockDWT() : m_acc(0), m_prev(0U)
    {
        HW_DWTEnableCounter();
 
        m_prev = HW_DWTGetCounter();
        m_acc  = 0;
    }
 
    static HiResClockDWT *GetInstance()
    {
        // keep declaration function-local to allow compiler stripping it from the binary if
        // it is unused by the user code
        static HiResClockDWT clock;
        return &clock;
    }
 
    void Update()
    {
        const uint32_t current = HW_DWTGetCounter();
 
        // unsigned subtraction handles the wrap-around perfectly
        uint32_t delta = current - m_prev;
        m_acc += delta;
 
        m_prev = current;
    }
 
    Cycles GetCycles()
    {
        Update();
        return m_acc;
    }
 
    uint32_t GetFrequency()
    {
        return HW_CoreClockFrequency();
    }
};

class HiResClockM0
{
public:
    static HiResClockM0 *GetInstance()
    {
        // keep declaration function-local to allow compiler stripping it from the binary if
        // it is unused by the user code
        static HiResClockM0 clock;
        return &clock;
    }
 
    Cycles GetCycles()
    {
        // On M0, combine the coarse OS ticks with the fine-grained SysTick counter
        Cycles cycles = ConvertTimeUsToClockCycles(HW_CoreClockFrequency(), stk::GetTicks() * GetContext().m_tick_resolution);
 
        uint32_t val  = SysTick->VAL;  // down-counter (cycles remaining in current tick)
        uint32_t load = SysTick->LOAD; // current reload value
 
        // total elapsed cycles
        return cycles + static_cast<Cycles>(load - val);
    }
 
    uint32_t GetFrequency()
    {
        return HW_CoreClockFrequency();
    }
};
 
#if (__CORTEX_M >= 3U)
    typedef HiResClockDWT HiResClockImpl;
#else
    typedef HiResClockM0 HiResClockImpl;
#endif

static volatile EKernelPanicId g_LastPanicId = KERNEL_PANIC_NONE;
 
__stk_attr_noinline  // keep out of inlining to preserve stack frame
__stk_attr_noreturn  // never returns - a trap
void STK_PANIC_HANDLER_DEFAULT(EKernelPanicId id)
{
    g_LastPanicId = id;
 
    // disable all maskable interrupts: this prevents scheduler from running again and corrupting state further
    HW_DisableInterrupts();
 
    // spin forever: with a watchdog active this produces a clean reset, without a watchdog,
    // a debugger can attach and inspect 'id'
    for (;;)
    {
        __stk_relax_cpu();
    }
}
 
void PlatformArmCortexM::ProcessTick()
{
    GetContext().OnTick();
}
 
#if STK_TICKLESS_IDLE
void Context::ReloadTickPeriod(Timeout ticks_requested)
{
    uint32_t reload = static_cast<uint32_t>(ConvertTimeUsToClockCycles(HW_CoreClockFrequency(), m_tick_resolution));
 
    // guard against uint32_t overflow in the reload calculation
    STK_ASSERT(static_cast<uint64_t>(ticks_requested) * reload <= UINT32_MAX);
 
    // start counting how many CPU cycles further instructions take until SysTick timer is enabled again;
    // without DWT we will have tick error of around 80 cycles depending on CPU model and compiler optimization
#if (__CORTEX_M > 1) && STK_TICKLESS_USE_ARM_DWT
    const uint32_t error = HW_DWTGetCounter();
    __stk_compiler_barrier(); // prevent reordering, we measure all cycles of instructions below this point
#endif
 
    // pause SysTick
    HW_SysTickDisable();
 
    // get already elapsed CPU cycles since SysTick ISR invocation up to SysTick timer stop (see above)
    // to account for them for a new period value
    const uint32_t elapsed_till_stop = HW_SysTickGetElapsed(HW_SysTickValueAfterDisable());
 
    // OnTick() should not consume more than next period
    STK_ASSERT(static_cast<Timeout>(elapsed_till_stop / reload) <= static_cast<Timeout>(ticks_requested));
 
    const uint32_t cpu_ticks_requested = static_cast<uint32_t>(ticks_requested) * reload;
 
    // substract number of cycles elapsed till SysTick stop + error from previous round
    uint32_t new_load = cpu_ticks_requested - elapsed_till_stop - m_sleep_error;
 
    // clamp: rearm overhead must never push new_load into underflow
    if (new_load > cpu_ticks_requested)
        new_load = cpu_ticks_requested;
 
    // reload with elapsed ticks accounted
    HW_SysTickRearm(new_load);
 
#if (__CORTEX_M > 1) && STK_TICKLESS_USE_ARM_DWT
    // calculate error: subtract cycles consumed by the rearm sequence itself in the next round
    m_sleep_error = HW_DWTGetCounter() - error;
#endif
}
#endif
 
extern "C" void STK_SYSTICK_HANDLER()
{
#if STK_SEGGER_SYSVIEW
    SEGGER_SYSVIEW_RecordEnterISR();
#endif
 
    Context &ctx = GetContext();
 
#ifdef HAL_MODULE_ENABLED // STM32 HAL
    // make sure STM32 HAL gets timing information as it depends on SysTick in delaying procedures
#if STK_TICKLESS_IDLE
    uwTick += ctx.m_sleep_ticks * ctx.m_tick_resolution;
#else
    HAL_IncTick();
#endif
 
    // STM32 HAL starts SysTick on its initialization that will cause a crash on NULL,
    // therefore use additional check if HAL_MODULE_ENABLED is defined
    if (ctx.m_started)
    {
#else
    {
        // make sure SysTick is enabled by the Kernel::Start(), disable its start anywhere else
        STK_ASSERT(ctx.m_started);
        STK_ASSERT(ctx.m_handler != nullptr);
#endif
        ctx.OnTick();
    }
 
#if STK_SEGGER_SYSVIEW
    SEGGER_SYSVIEW_RecordExitISR();
    SEGGER_SYSVIEW_IsStarted();
#endif
}

#define STK_ASM_BLOCK_PRIVILEGE_MODE\
    "LDR        r1, %[st_active]\n" /* r1 = Stack* (m_stack_active) */ \
    "LDR        r1, [r1, #4]    \n" /* r1 = m_stack_active->mode (offset 4), reuse r1 */ \
    "CMP        r1, %[priv_val] \n" /* compare with ACCESS_PRIVILEGED */ \
    "BEQ        1f              \n" /* if equal, privileged mode */\
    /* unprivileged mode: set CONTROL.nPRIV = 1 */\
    "MRS        r0, CONTROL     \n"\
    "ORR        r0, r0, #1      \n"\
    "MSR        CONTROL, r0     \n"\
    "B          2f              \n"\
    "1:                         \n"\
    /* privileged mode: set CONTROL.nPRIV = 0 */\
    "MRS        r0, CONTROL     \n"\
    "BIC        r0, r0, #1      \n"\
    "MSR        CONTROL, r0     \n"\
    "2:                         \n"
 
extern "C" __stk_attr_naked void STK_PENDSV_HANDLER()
{
    __asm volatile(
    ".syntax unified             \n"
 
    STK_ASM_DISABLE_INTERRUPTS " \n"
 
    // save stack to inactive (idle) task
    "MRS        r0, psp          \n"
 
#if STK_CORTEX_M_FPU
    // save FP registers
    "TST        LR, #16          \n" /* test LR for 0xffffffe_, e.g. Thread mode with FP data */
    "IT         EQ               \n"
    "VSTMDBEQ   r0!, {s16-s31}   \n" /* store 16 SP registers */
#endif
 
    // save general registers
#if STK_CORTEX_M_MANAGE_LR
    // save r4-r11 and LR
    // note: for Cortex-M3 and higher save LR to keep correct Thread state of the task when it is restored
    "STMDB      r0!, {r4-r11, LR}\n"
#else
    // note: STMIA is limited to r0-r7 range, therefore save via stack memory
    "SUBS       r0, r0, #16      \n" /* decrement for r4-r7 */
    "STMIA      r0!, {r4-r7}     \n"
    "MOV        r4, r8           \n"
    "MOV        r5, r9           \n"
    "MOV        r6, r10          \n"
    "MOV        r7, r11          \n"
    "SUBS       r0, r0, #32      \n" /* decrement for r8-r11 and pointer reset */
    "STMIA      r0!, {r4-r7}     \n"
    "SUBS       r0, r0, #16      \n" /* final pointer adjustment */
#endif
 
    // store in GetContext().m_stack_idle
    "LDR        r1, %[st_idle]   \n"
    "STR        r0, [r1]         \n" /* store the first member (SP) from r0 */
 
    // ARMv8-M TrustZone: save PSPLIM (Secure) and PSPLIM_NS (Non-Secure) stack
    // limit registers into the idle stack frame so overflow detection stays
    // correct after the switch.  The two words are pushed BELOW the callee-saved
    // registers (r0 already points past them), matching TrustZoneFrame layout.
    // Reference: ARMv8-M ARM B3.29 / B3.30.
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    "MRS        r2, PSPLIM       \n" /* Secure PSPLIM */
    "MRS        r3, PSPLIM_NS    \n" /* Non-Secure PSPLIM */
    "STMDB      r0!, {r2, r3}    \n" /* push below saved general regs */
    "STR        r0, [r1]         \n" /* update idle stack SP to include TZ words */
#endif
 
    // set privileged/unprivileged mode for the active stack
#ifdef CONTROL_nPRIV_Msk
    STK_ASM_BLOCK_PRIVILEGE_MODE
#endif
 
    // load stack of the active task from GetContext().m_stack_active (note: keep in sync with OnTaskStart)
    "LDR        r1, %[st_active] \n"
    "LDR        r0, [r1]         \n" /* load the first member of Stack (Stack::SP) into r0 */
 
    // ARMv8-M TrustZone: restore PSPLIM (Secure) and PSPLIM_NS (Non-Secure)
    // stack limit registers from the active task frame.  The two words were
    // pushed beneath the callee-saved registers by the save path above.
    // They are popped first so that r0 advances to the callee-saved region.
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    "LDMIA      r0!, {r2, r3}    \n" /* pop PSPLIM, PSPLIM_NS */
    "MSR        PSPLIM, r2       \n" /* restore Secure PSPLIM */
    "MSR        PSPLIM_NS, r3    \n" /* restore Non-Secure PSPLIM */
#endif
 
    // restore general registers
#if STK_CORTEX_M_MANAGE_LR
    // restore r4-r11 and LR
    "LDMIA      r0!, {r4-r11, LR}\n"
#else
    // note: LDMIA is limited to r0-r7 range, therefore load via stack memory
    "LDMIA      r0!, {r4-r7}     \n"
    "MOV        r8, r4           \n"
    "MOV        r9, r5           \n"
    "MOV        r10, r6          \n"
    "MOV        r11, r7          \n"
    "LDMIA      r0!, {r4-r7}     \n"
#endif
 
#if STK_CORTEX_M_FPU
    // restore FP registers
    "TST        LR, #16         \n" /* test LR for 0xffffffe_, e.g. Thread mode with FP data */
    "IT         EQ              \n" /* if result is positive */
    "VLDMIAEQ   r0!, {s16-s31}  \n" /* restore FP registers */
#endif
 
    "MSR        psp, r0         \n" /* restore psp */
 
    STK_ASM_ENABLE_INTERRUPTS " \n"
 
    STK_ASM_EXIT_FROM_HANDLER " \n"
 
    : /* output: none */
    : [st_idle]   "m" (GetContext().m_stack_idle),
      [st_active] "m" (GetContext().m_stack_active),
      [priv_val]  "i" (ACCESS_PRIVILEGED)
    : "r0", "r1" /* scratchpad; r2, r3 additionally used in the TrustZone PSPLIM path */ );
}
 
__stk_attr_naked void OnTaskStart()
{
    // note: HW_DisableInterrupts() must be called prior calling this function
 
    __asm volatile(
    ".syntax unified             \n"
 
    // set privileged/unprivileged mode for the active stack
#ifdef CONTROL_nPRIV_Msk
    STK_ASM_BLOCK_PRIVILEGE_MODE
#endif
 
    // load stack of the active task from GetContext().m_stack_active (note: keep in sync with OnTaskStart)
    "LDR        r1, %[st_active] \n"
    "LDR        r0, [r1]         \n" /* load the first member of Stack (Stack::SP) into r0 */
 
    // restore general registers
#if STK_CORTEX_M_MANAGE_LR
    // restore r4-r11 and LR
    "LDMIA      r0!, {r4-r11, LR}\n"
#else
    // note: LDMIA is limited to r0-r7 range, therefore load via stack memory
    "LDMIA      r0!, {r4-r7}     \n"
    "MOV        r8, r4           \n"
    "MOV        r9, r5           \n"
    "MOV        r10, r6          \n"
    "MOV        r11, r7          \n"
    "LDMIA      r0!, {r4-r7}     \n"
#endif
 
#if STK_CORTEX_M_FPU
    // restore FP registers
    "TST        LR, #16         \n" /* test LR for 0xffffffe_, e.g. Thread mode with FP data */
    "IT         EQ              \n" /* if result is positive */
    "VLDMIAEQ   r0!, {s16-s31}  \n" /* restore FP registers */
#endif
 
    // restore psp
    "MSR        psp, r0         \n"
 
#if !STK_CORTEX_M_MANAGE_LR
    // M0: set LR to Thread mode, use PSP state and stack
    "LDR        r0, =%[exc_ret] \n"
    "MOV        LR, r0          \n"
#endif
 
    STK_ASM_ENABLE_INTERRUPTS " \n"
 
    STK_ASM_EXIT_FROM_HANDLER " \n"
 
    : /* output: none */
    : [st_active] "m" (GetContext().m_stack_active),
      [priv_val]  "i" (ACCESS_PRIVILEGED),
      [exc_ret]   "i" (STK_CORTEX_M_EXC_RETURN_THREAD_PSP)
    : "r0", "r1" /* only r0, r1 are used as a scratchpad */ );
}
 
void Context::Start()
{
    m_exiting = false;
 
    // save jump location of the Exit trap
    SaveJmp(m_exit_buf);
    if (m_exiting)
    {
        // notify kernel about a full stop
        m_handler->OnStop();
        return;
    }
 
    HW_StartScheduler();
}
 
void Context::OnStart()
{
    // interrupts must be disabled at this point
    STK_ASSERT(HW_InterruptsDisabled());
 
    // FPU
    HW_EnableFullFpuAccess();
 
    // clear FPU usage status if FPU was used before kernel start
    HW_ClearFpuState();
 
    // notify kernel
    m_handler->OnStart(m_stack_active);
 
#if STK_TICKLESS_IDLE
    // reset sleep ticks if kernel was restarted
    m_sleep_ticks = 1;
#endif
 
    // start SysTick timer (it is yet can't fire an interrupt due to HW_DisableInterrupts)
    HW_SysTickStart(m_tick_resolution);
 
    // set priority
    // note: after SysTick_Config because it may change SysTick priority, PendSV and SysTick peripherals
    // have equal priority to avoid race
    NVIC_SetPriority(PendSV_IRQn, STK_CORTEX_M_ISR_PRIORITY_LOWEST);
    NVIC_SetPriority(SysTick_IRQn, STK_CORTEX_M_ISR_PRIORITY_LOWEST);
    // set highest priority for SVC interrupts to support critical section for unprivileged tasks
#ifdef CONTROL_nPRIV_Msk
    NVIC_SetPriority(SVCall_IRQn, STK_CORTEX_M_ISR_PRIORITY_HIGHEST);
#endif
 
    m_started = true;
}
 
// __stk_attr_used required for Link-Time Optimization (-flto)
extern "C" __stk_attr_used void SVC_Handler_Main(Word *svc_args)
{
    // Word is typedef uintptr_t (stk_common.h) - the only integer type the Standard
    // blesses for lossless pointer round-trips (MISRA C++ 5-2-8, CERT INT36-C)
    STK_STATIC_ASSERT_DESC_N(PTR, sizeof(Word) == sizeof(void *),
        "Word must be uintptr_t width for safe pointer round-trip via frame->PC");
 
    // priority 0 (NMI, HardFault) unaffected: SVC (priority 0 per OnStart()) remains
    // reachable so SVC_EXIT_CRITICAL can always unwind
    STK_STATIC_ASSERT_DESC_N(NVIC, __NVIC_PRIO_BITS < 32u,
        "NVIC priority bit width exceeds safe shift range");
 
    // 'volatile': R0 is written back to stacked memory, compiler must not eliminate the store
    volatile ExceptionFrame * const frame = reinterpret_cast<volatile ExceptionFrame *>(svc_args);
 
    // details: https://developer.arm.com/documentation/ka004005/latest
    // Thumb SVC encoding: [15:8] = 0xDF, [7:0] = imm8
    // ---
    // opcode lives two bytes (one Thumb halfword) before the stacked PC:
    // do explicit subtraction instead of negative indexing (MISRA C++ 5-0-16),
    // uint8_t extracted before ESvcCommandId cast to avoid impl-defined enum conversion (MISRA 5-2-6)
    const uint8_t       *insn_ptr = hw::WordToPtr<const uint8_t>(frame->PC) - 2;
    const ESvcCommandId  command  = static_cast<ESvcCommandId>(*insn_ptr);
 
    switch (command)
    {
    case SVC_START_SCHEDULING: {
        // disallow any duplicate attempt
        STK_ASSERT(!GetContext().m_started);
        if (GetContext().m_started)
            return;
 
        // make sure interrupts do not interfere, OnStart expects interrupts disabled
        HW_DisableInterrupts();
 
        GetContext().OnStart();
 
        // start first task
        OnTaskStart();
        break; }
 
    /*case SVC_FORCE_SWITCH: {
        HW_ScheduleContextSwitch();
        break; }*/
 
#ifdef CONTROL_nPRIV_Msk
    case SVC_ENTER_CRITICAL: {
        const Word saved_basepri = __get_BASEPRI();
        __set_BASEPRI(static_cast<uint32_t>(1U) << __NVIC_PRIO_BITS); // mask all configurable-priority interrupts
        __DSB();                   // BASEPRI write visible to bus before SVC return
        __ISB();                   // pipeline flush: mask in effect at first caller instruction
        frame->R0 = saved_basepri; // return saved value via stacked R0
        break; }
 
    case SVC_EXIT_CRITICAL: {
        __DSB();                   // drain pending stores before widening interrupt window
        __set_BASEPRI(frame->R0);  // restore saved BASEPRI (passed in R0)
        __ISB();                   // pending interrupts at restored priority may fire now
        break; }
#endif // CONTROL_nPRIV_Msk
 
    default: {
        // any SVC number not in ESvcCommandId is a defect, panic unconditionally
        STK_KERNEL_PANIC(KERNEL_PANIC_UNKNOWN_SVC);
        break; }
    }
}
 
// details: "How to Write an SVC Function", https://developer.arm.com/documentation/ka004005/latest
extern "C" __stk_attr_naked void STK_SVC_HANDLER()
{
    __asm volatile(
    ".syntax unified            \n"
    ".global SVC_Handler_Main   \n"
    ".align 2                   \n" // ensure the entry point is aligned
 
#ifdef STK_CORTEX_M_TRUSTZONE
    // ARMv8-M TrustZone (Cortex-M33 / Mainline)
    // EXC_RETURN bit layout (ARMv8-M ARM B1.5.8):
    //   bit 6 (0x40): 1 = Secure stack, 0 = Non-Secure stack.  <-- S-bit
    //   bit 2 (0x04): 1 = PSP,          0 = MSP.
    // We test bit 6 first to select the correct world's stack pointer,
    // then bit 2 to choose MSP vs PSP within that world.
    #if (__CORTEX_M >= 33U)
    // -----------------------------------------------------------------
    // Cortex-M33 and later (ARMv8-M Mainline) -- IT/ITE available.
    // -----------------------------------------------------------------
    "TST    LR, #4              \n" // bit 2: 0 = MSP, 1 = PSP
    "BNE    .use_psp            \n"
    // --- MSP branch ---
    "TST    LR, #64             \n" // bit 6 (S-bit): 1 = Secure, 0 = Non-Secure
    "ITE    NE                  \n"
    "MRSNE  r0, MSP             \n" // r0 = Secure MSP
    "MRSEQ  r0, MSP_NS          \n" // r0 = Non-Secure MSP
    "B      .call_main          \n"
 
    ".use_psp:                  \n"
    "TST    LR, #64             \n" // bit 6 (S-bit): 1 = Secure, 0 = Non-Secure
    "ITE    NE                  \n"
    "MRSNE  r0, PSP             \n" // r0 = Secure PSP
    "MRSEQ  r0, PSP_NS          \n" // r0 = Non-Secure PSP
    #else
    // -----------------------------------------------------------------
    // Cortex-M23 (ARMv8-M Baseline) -- no IT instructions, limited ISA.
    // Shift bits into the sign position and use BMI (Branch if Minus).
    //   bit 2 -> sign: LSLS r1, #29  (32 - 3 = 29)
    //   bit 6 -> sign: LSLS r1, #25  (32 - 7 = 25)
    // -----------------------------------------------------------------
    "MOV    r1, LR              \n"
    "LSLS   r1, r1, #29         \n" // shift bit 2 into sign
    "BMI    .use_psp_v8m_b      \n" // bit 2 set  -> PSP branch
    // --- MSP branch ---
    "MOV    r1, LR              \n"
    "LSLS   r1, r1, #25         \n" // shift bit 6 (S-bit) into sign
    "BMI    .use_msp_s          \n" // S=1 -> Secure MSP
    "MRS    r0, MSP_NS          \n" // S=0 -> Non-Secure MSP
    "B      .call_main          \n"
 
    ".use_msp_s:                \n"
    "MRS    r0, MSP             \n" // r0 = Secure MSP
    "B      .call_main          \n"
 
    ".use_psp_v8m_b:            \n"
    "MOV    r1, LR              \n"
    "LSLS   r1, r1, #25         \n" // shift bit 6 (S-bit) into sign
    "BMI    .use_psp_s          \n" // S=1 -> Secure PSP
    "MRS    r0, PSP_NS          \n" // S=0 -> Non-Secure PSP
    "B      .call_main          \n"
 
    ".use_psp_s:                \n"
    "MRS    r0, PSP             \n" // r0 = Secure PSP
    #endif
 
    ".call_main:                \n"
#elif (__CORTEX_M >= 3U)
    // Cortex-M3/M4/M7
    "TST    LR, #4              \n" // check EXC_RETURN bit 2
    "ITE    EQ                  \n"
    "MRSEQ  r0, MSP             \n" // r0 = MSP
    "MRSNE  r0, PSP             \n" // else r0 = PSP
#else
    // Cortex-M0/M0+ (limited ISA)
    "MOV    r0, LR              \n" // r0 = LR
    "LSLS   r0, r0, #29         \n" // if (r0 & 4)
    "BMI    .use_psp_m0         \n" // else
    "MRS    r0, MSP             \n" // r0 = MSP
    "B      .call_main_m0       \n"
 
    ".use_psp_m0:               \n"
    "MRS    r0, PSP             \n" // else r0 = PSP
 
    ".call_main_m0:             \n"
#endif
 
    // even on Cortex-M3+, a long jump is safer when using LTO, we load address into register to allow far jump (>2KB)
    "LDR    r1, =SVC_Handler_Main \n"
    "BX     r1                  \n"
 
    ".align 2                   \n"   // ensure literal pool is aligned
    ".pool                      \n"); // ensure literal pool is reachable
}
 
static void OnTaskRun(ITask *task)
{
    task->Run();
}
 
static void OnTaskExit()
{
    uint32_t cs;
    HW_CriticalSectionStart(cs);
 
    GetContext().m_handler->OnTaskExit(GetContext().m_stack_active);
 
    HW_CriticalSectionEnd(cs);
 
    for (;;)
    {
        __DSB();
        __WFI(); // enter standby mode until time slot expires
    }
}
 
static void OnSchedulerSleep()
{
    for (;;)
    {
    #if STK_SEGGER_SYSVIEW
        SEGGER_SYSVIEW_OnIdle();
    #endif
 
        SCB->SCR &= ~SCB_SCR_SLEEPDEEP_Msk; // disable deep-sleep, go into a WAIT mode (sleep)
        __DSB();                            // ensure store takes effect (see ARM info)
        __WFI();                            // enter sleep mode
    }
}
 
static void OnSchedulerSleepOverride()
{
    if (!GetContext().m_overrider->OnSleep())
        OnSchedulerSleep();
}
 
static void OnSchedulerExit()
{
    __set_CONTROL(0U); // switch to MSP
    __set_PSP(0U);     // clear PSP (for a clean register state)
 
    // jump back to SaveJmp's return site with m_exiting already set to true
    RestoreJmp(GetContext().m_exit_buf, 0);
}
 
#if STK_SEGGER_SYSVIEW
static void SendSysDesc()
{
    SEGGER_SYSVIEW_SendSysDesc("SuperTinyKernel (STK)");
}
#endif
 
void PlatformArmCortexM::Initialize(IEventHandler *event_handler, IKernelService *service, uint32_t resolution_us,
    Stack *exit_trap)
{
    GetContext().Initialize(event_handler, service, exit_trap, resolution_us);
}
 
void PlatformArmCortexM::Start()
{
    GetContext().Start();
}
 
bool PlatformArmCortexM::InitStack(EStackType stack_type, Stack *stack, IStackMemory *stack_memory, ITask *user_task)
{
    // Precondition (TrustZone builds): stack->mode must be set by the caller
    // before this function is invoked.  On TZ builds the EXC_RETURN selection
    // below reads stack->mode to decide between Secure (0xED) and Non-Secure
    // (0xBC) EXC_RETURN values.  In the Kernel class, KernelTask::Bind() sets
    // stack->mode from user_task->GetAccessMode() immediately before calling
    // InitStack(), satisfying this requirement.
    STK_STATIC_ASSERT_DESC_N(ExceptionFrame, (sizeof(ExceptionFrame) == (8 * sizeof(Word))),
        "ExceptionFrame layout must match the ARMv7-M hardware exception frame exactly");
    STK_STATIC_ASSERT_DESC_N(TaskFrame, sizeof(TaskFrame) == (8 + STK_CORTEX_M_MANAGE_LR + STK_CORTEX_M_TZ_REGISTER_COUNT) * sizeof(Word),
        "TaskFrame size must equal ExceptionFrame plus optional EXC_RETURN word plus optional TrustZone words");
 
    STK_ASSERT(stack_memory->GetStackSize() > STK_CORTEX_M_TOTAL_REGISTER_COUNT);
 
    // initialize stack memory
    Word *stack_top = Context::InitStackMemory(stack_memory);
 
    // initialize Stack Pointer (SP)
    stack->SP = hw::PtrToWord(stack_top - STK_CORTEX_M_TOTAL_REGISTER_COUNT);
 
    // place the initial task frame flush against the top of the stack:
    // TaskFrame::exc (ExceptionFrame) occupies the top 8 words, TaskFrame::EXC_RETURN
    // (when present) sits immediately below it, and TrustZoneFrame (when present) sits
    // below that.
    TaskFrame * const task_frame = reinterpret_cast<TaskFrame *>(stack_top) - 1;
 
    // initialize registers for the user task's first start
    switch (stack_type)
    {
    case STACK_USER_TASK: {
        task_frame->exc.PC = hw::PtrToWord(&OnTaskRun) & ~0x1UL;
        task_frame->exc.LR = hw::PtrToWord(&OnTaskExit);
        task_frame->exc.R0 = hw::PtrToWord(user_task);
        break; }
 
    case STACK_SLEEP_TRAP: {
        task_frame->exc.PC = hw::PtrToWord(GetContext().m_overrider != nullptr ? &OnSchedulerSleepOverride : &OnSchedulerSleep);
        task_frame->exc.LR = STK_STACK_MEMORY_FILLER; // should not attempt to exit
        task_frame->exc.R0 = 0U;
        break; }
 
    case STACK_EXIT_TRAP: {
        task_frame->exc.PC = hw::PtrToWord(&OnSchedulerExit);
        task_frame->exc.LR = STK_STACK_MEMORY_FILLER; // should not attempt to exit
        task_frame->exc.R0 = 0U;
        break; }
 
    default:
        return false;
    }
 
    // details: "Program counter: Bit [0] is always 0, so instructions are always aligned to halfword boundaries",
    // https://developer.arm.com/documentation/ddi0413/c/programmer-s-model/registers/general-purpose-registers
    task_frame->exc.PC &= ~0x1UL;
 
    // set T bit of EPSR sub-register to enable execution of instructions
    // details: "Special-purpose program status registers (xPSR)": Execution PSR, https://developer.arm.com/documentation/ddi0413/c/programmer-s-model/registers/special-purpose-program-status-registers--xpsr-
    task_frame->exc.PSR = static_cast<Word>(1U) << 24;
 
#if STK_CORTEX_M_MANAGE_LR
    /*
     * Choose the correct EXC_RETURN for the new task.
     *
     * ARMv8-M TrustZone (Cortex-M33+):
     *   - Secure tasks must use 0xFFFFFFED (S-bit = 1, Thread, PSP).
     *   - Non-Secure tasks (and all tasks on plain ARMv7-M) use 0xFFFFFFFD.
     *
     * The Secure/Non-Secure classification is derived from the stack's
     * access mode.  ACCESS_PRIVILEGED tasks running in the Secure partition
     * are given the Secure EXC_RETURN; all others get the Non-Secure value.
     *
     * Reference: ARMv8-M Architecture Reference Manual B1.5.8.
     */
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    if (stack_type == STACK_USER_TASK && stack->mode == ACCESS_PRIVILEGED)
        task_frame->EXC_RETURN = STK_CORTEX_M_EXC_RETURN_S_THREAD_PSP;
    else
        task_frame->EXC_RETURN = STK_CORTEX_M_EXC_RETURN_NS_THREAD_PSP;
#else
    task_frame->EXC_RETURN = STK_CORTEX_M_EXC_RETURN_THREAD_PSP;
#endif
#endif // STK_CORTEX_M_MANAGE_LR
 
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)
    // Initialize PSPLIM registers to 0 (no limit enforced until the application
    // sets them explicitly via CMSE APIs).  They will be saved/restored per-task
    // by the PendSV handler from this point forward.
    task_frame->tz.PSPLIM    = 0U;
    task_frame->tz.PSPLIM_NS = 0U;
#endif
 
    return true;
}
 
// ---------------------------------------------------------------------------
// ARMv8-M TrustZone Non-Secure callable (NSC) gateway veneers  (Change ④)
//
// These functions are placed in the NSC-attributed linker section.  The
// compiler inserts an SG (Secure Gateway) instruction and a BXNS return so
// that Non-Secure code can call into Secure-world kernel services through a
// hardened, auditable entry point.
//
// Calling convention: the Non-Secure caller passes arguments in r0-r3 per
// AAPCS.  CMSE clears all other registers on entry to prevent information
// leakage from the Secure side.
//
// To use: declare the matching cmse_nonsecure_call function pointer in the
// Non-Secure image (see ITaskNonSecure_t in stk_arch_arm-cortex-m.h) and
// place these symbols in a region mapped as NSC in the SAU/IDAU configuration.
//
// Only compiled when STK_CORTEX_M_TRUSTZONE is defined on an ARMv8-M
// Mainline target (Cortex-M33 or later).
// ---------------------------------------------------------------------------
#if defined(STK_CORTEX_M_TRUSTZONE) && (__CORTEX_M >= 33U)

__stk_nsc_entry void NSC_SwitchToNext()
{
    // Non-Secure Handler mode passes MSP_NS as the caller SP, which the
    // scheduler cannot use to locate a task context.  Guard and return safely.
    if (HW_IsHandlerMode())
        return;
 
    GetContext().m_handler->OnTaskSwitch(HW_GetCallerSP());
}

__stk_nsc_entry void NSC_Sleep(Timeout ticks)
{
    if (HW_IsHandlerMode())
        return;
 
    GetContext().m_handler->OnTaskSleep(HW_GetCallerSP(), ticks);
}

__stk_nsc_entry IWaitObject *NSC_Wait(ISyncObject *sync_obj, IMutex *mutex, Timeout timeout)
{
    // Handler-mode guard: Non-Secure ISRs pass MSP_NS which cannot locate a task context.
    if (HW_IsHandlerMode())
        return nullptr;
 
    // Validate that the pointers supplied by the Non-Secure caller actually
    // refer to Non-Secure accessible memory.  If either check fails the call
    // is treated as an invalid request and we return nullptr immediately.
    if (sync_obj != nullptr &&
        cmse_check_pointed_object(sync_obj, CMSE_NONSECURE) == nullptr)
    {
        return nullptr;
    }
    if (mutex != nullptr &&
        cmse_check_pointed_object(mutex, CMSE_NONSECURE) == nullptr)
    {
        return nullptr;
    }
 
    return GetContext().m_handler->OnTaskWait(HW_GetCallerSP(), sync_obj, mutex, timeout);
}

__stk_nsc_entry TId NSC_GetTid()
{
    if (HW_IsHandlerMode())
        return TID_ISR;
 
    return GetContext().m_handler->OnGetTid(HW_GetCallerSP());
}

__stk_nsc_entry int32_t NSC_GetTickResolution()
{
    return static_cast<int32_t>(GetContext().m_tick_resolution);
}
 
#endif // STK_CORTEX_M_TRUSTZONE && __CORTEX_M >= 33U
// ---------------------------------------------------------------------------
 
void Context::OnStop()
{
#if STK_SEGGER_SYSVIEW
    SEGGER_SYSVIEW_Stop();
#endif
 
    // stop SysTick timer
    HW_SysTickStop();
 
    // clear pending PendSV exception
    SCB->ICSR = SCB_ICSR_PENDSVCLR_Msk;
 
    m_started = false;
    m_exiting = true;
 
    // make sure all assignments are set and executed
    __DSB();
    __ISB();
}
 
void PlatformArmCortexM::Stop()
{
    GetContext().OnStop();
 
    // load context of the Exit trap
    HW_DisableInterrupts();
    OnTaskStart();
}
 
uint32_t PlatformArmCortexM::GetTickResolution() const
{
    return GetContext().m_tick_resolution;
}
 
void PlatformArmCortexM::SwitchToNext()
{
    GetContext().m_handler->OnTaskSwitch(HW_GetCallerSP());
}
 
void PlatformArmCortexM::Sleep(Timeout ticks)
{
    GetContext().m_handler->OnTaskSleep(HW_GetCallerSP(), ticks);
}
 
IWaitObject *PlatformArmCortexM::Wait(ISyncObject *sync_obj, IMutex *mutex, Timeout timeout)
{
    return GetContext().m_handler->OnTaskWait(HW_GetCallerSP(), sync_obj, mutex, timeout);
}
 
TId PlatformArmCortexM::GetTid() const
{
    return GetContext().m_handler->OnGetTid(HW_GetCallerSP());
}
 
void PlatformArmCortexM::ProcessHardFault()
{
    if ((GetContext().m_overrider == nullptr) || !GetContext().m_overrider->OnHardFault())
    {
        STK_KERNEL_PANIC(KERNEL_PANIC_HRT_HARD_FAULT);
    }
}
 
void PlatformArmCortexM::SetEventOverrider(IEventOverrider *overrider)
{
    STK_ASSERT(!GetContext().m_started);
    GetContext().m_overrider = overrider;
}
 
Word PlatformArmCortexM::GetCallerSP() const
{
    return HW_GetCallerSP();
}
 
IKernelService *IKernelService::GetInstance()
{
    return GetContext().m_service;
}
 
void stk::hw::CriticalSection::Enter()
{
    // if we are in Handler or Privileged Thread Mode, we can skip the SVC and take the fast path
    if (HW_IsHandlerMode() || HW_IsPrivilegedThreadMode())
        GetContext().EnterCriticalSection();
    else
        GetContext().UnprivEnterCriticalSection();
}
 
void stk::hw::CriticalSection::Exit()
{
    // if we are in Handler or Privileged Thread Mode, we can skip the SVC and take the fast path
    if (HW_IsHandlerMode() || HW_IsPrivilegedThreadMode())
        GetContext().ExitCriticalSection();
    else
        GetContext().UnprivExitCriticalSection();
}
 
void stk::hw::SpinLock::Lock()
{
    HW_SpinLockLock(m_lock);
}
 
void stk::hw::SpinLock::Unlock()
{
    HW_SpinLockUnlock(m_lock);
}
 
bool stk::hw::SpinLock::TryLock()
{
    return HW_SpinLockTryLock(m_lock);
}
 
bool stk::hw::IsInsideISR()
{
    return HW_IsHandlerMode();
}
 
Cycles stk::hw::HiResClock::GetCycles()
{
    return HiResClockImpl::GetInstance()->GetCycles();
}
 
uint32_t stk::hw::HiResClock::GetFrequency()
{
    Cycles freq = HiResClockImpl::GetInstance()->GetFrequency();
    STK_ASSERT(freq != 0);
    return freq;
}
 
#endif // _STK_ARCH_ARM_CORTEX_M