cortex.h

#pragma once
 
#include <stdbool.h>
#include <stdint.h>
#include <RTE_Components.h>
#include CMSIS_device_header
 
typedef struct {
    uint32_t r0123[4];
/*  uint32_t r1;
    uint32_t r2;
    uint32_t r3;*/
    uint32_t r12;
    void (*lr)(void);
    void *pc;
    uint32_t xpsr;
/*  uint32_t arg4;
    uint32_t arg5;
    uint32_t arg6;
    uint32_t arg7;*/
} ExceptionFrame;
 
_Static_assert(sizeof(ExceptionFrame) % 8 == 0, "The size of ExceptionFrame structure is not divisible by 8!");
 
typedef struct {
    uint32_t r0123[4];
    uint32_t r12;
    void (*lr)(void);
    void *pc;
    uint32_t xpsr;
    uint32_t fpu_regs[16];
    uint32_t fpscr;
    uint32_t __spacer;
} ExceptionFrameFP;
 
_Static_assert(sizeof(ExceptionFrameFP) % 8 == 0, "The size of ExceptionFrameFP structure is not divisible by 8!");
 
#define EXCEPTION_FRAME_ENTRIES         (sizeof(ExceptionFrame) / sizeof(uint32_t))
#define EXCEPTION_FRAME_FP_ENTRIES      (sizeof(ExceptionFrameFP) / sizeof(uint32_t))
 
#define ALWAYS_INLINE __STATIC_FORCEINLINE
 
#if defined(__ARM_ARCH_6M__) || defined(__ARM_ARCH_8M_BASE__)
 
/* ARMv6M and ARMv8M-Baseline: limited register access */
ALWAYS_INLINE void __ISR_return()
{
        asm volatile(
                "POP {R0, R1, R2, R3}\n\t"
                "POP {R6, R7}\n\t"
                "MOV R12, R6\n\t"
                "MOV LR, R7\n\t"
                "POP {R6, R7}\n\t"
                "BX R6\n\t"
                );
}
 
#else
 
/* ARMv7M, ARMv7EM, ARMv8M-Mainline: full register access */
// TODO: Test this
ALWAYS_INLINE void __ISR_return()
{
        asm volatile(
                "POP {R0, R1, R2, R3}\n\t"
                "POP {R12, LR}\n\t"
                "POP {R6, R7}\n\t"
                "BX R6\n\t"
                );
}
 
#endif
 
#define SAVE_CONTEXT() \
asm volatile( \
    ".syntax unified\n\t" \
    "MRS r0, PSP\n\t" \
    "SUBS r0, #16\n\t" \
    "STMEA r0!, {r4 - r7}\n\t" \
    "SUBS r0, #32\n\t" \
    "MOV r4, r8\n\t" \
    "MOV r5, r9\n\t" \
    "MOV r6, r10\n\t" \
    "MOV r7, r11\n\t" \
    "STMEA r0!, {r4 - r7}\n\t" \
    "SUBS r0, #16\n\t" \
    "MSR PSP, r0\n\t" \
    : : : "r0", "r4", "r5", "r6", "r7", "memory" \
)
 
 
#define LOAD_CONTEXT() \
asm ( \
    ".syntax unified\n\t" \
    "MRS r0, PSP\n\t" \
    "LDMFD r0!, {r4 - r7}\n\t" \
    "MOV r8, r4\n\t" \
    "MOV r9, r5\n\t" \
    "MOV r10, r6\n\t" \
    "MOV r11, r7\n\t" \
    "LDMFD r0!, {r4 - r7}\n\t" \
    "MSR PSP, r0\n\t" \
    : : : "r0", "r4", "r5", "r6", "r7", "memory" \
)
 
uint32_t * get_exception_arg_addr(ExceptionFrame * frame, unsigned argno, bool fp_active);
 
static inline unsigned get_exception_argument(ExceptionFrame * frame, unsigned argno, bool fp_active)
{
    uint32_t * arg_addr = get_exception_arg_addr(frame, argno, fp_active);
    return *arg_addr;
}
 
static inline void set_exception_argument(ExceptionFrame * frame, unsigned argno, unsigned value, bool fp_active)
{
    uint32_t * arg_addr = get_exception_arg_addr(frame, argno, fp_active);
    *arg_addr = value;
}
 
static inline void set_exception_pc_lr(ExceptionFrame * frame, void * pc, void (* lr)(void))
{
    frame->pc = pc;
    frame->lr = lr;
}
 
ExceptionFrame * push_exception_frame(ExceptionFrame * frame, unsigned args, bool fp_active);
 
ExceptionFrame * shim_exception_frame(ExceptionFrame * frame, unsigned args, bool fp_active);
 
ExceptionFrame * pop_exception_frame(ExceptionFrame * frame, unsigned args, bool fp_active);
 
ALWAYS_INLINE void * __get_LR(void)
{
    void * lr;
    asm volatile(
            ".syntax unified\n\t"
            "MOV %0, LR\n\t"
            : "=r" (lr)
            );
 
    return lr;
}
 
ALWAYS_INLINE void __set_LR(void * lr)
{
    asm volatile(
        ".syntax unified\n\t"
        "MOV LR, %0\n\t"
        :
        : "r" (lr)
    );
}
 
#if defined(__ARM_ARCH_6M__) || defined(__ARM_ARCH_8M_BASE__)
 
/* ARMv6M and ARMv8M-Baseline: limited immediate support, use LDR */
ALWAYS_INLINE  void __forge_shutdown_exception_frame(void (*continue_here)(void))
{
    asm volatile(
        ".syntax unified\n\t"
        "LDR R0, =0xFFFFFFF9\n\t" // return to thread mode with MSP
        "MOV LR, R0\n\t"
        "LDR R0, =0x01000000\n\t"
        "MOV R1, %0\n\t"
        "LDR R2, =0\n\t"
        "PUSH {R0}\n\t" // xPSR
        "PUSH {R1}\n\t" // PC
        "PUSH {R2}\n\t" // LR - NULL, point of no return
        "PUSH {R2}\n\t" // R12
        "PUSH {R2}\n\t" // R3
        "PUSH {R2}\n\t" // R2
        "PUSH {R2}\n\t" // R1
        "PUSH {R2}\n\t" // R0
        "BX LR"
        :
        : "r" (continue_here)
    );
}
 
#else
 
/* ARMv7M, ARMv7EM, ARMv8M-Mainline: modified immediate support */
ALWAYS_INLINE  __attribute__((noreturn)) void __forge_shutdown_exception_frame(void (*continue_here)(void))
{
    asm volatile(
        ".syntax unified\n\t"
        "MOV R0, #0x01000000\n\t"
        "MOV R1, %0\n\t"
        "MOV R2, #0\n\t"
        "MOV LR, #0xFFFFFFF9\n\t" // return to thread mode with MSP
        "PUSH {R0}\n\t" // xPSR
        "PUSH {R1}\n\t" // PC
        "PUSH {R2}\n\t" // LR - NULL, point of no return
        "PUSH {R2}\n\t" // R12
        "PUSH {R2}\n\t" // R3
        "PUSH {R2}\n\t" // R2
        "PUSH {R2}\n\t" // R1
        "PUSH {R2}\n\t" // R0
        "BX LR"
        :
        : "r" (continue_here)
    );
}
 
#endif
 
static inline bool cortex_is_fpu_used(uint32_t return_addr)
{
#if defined(__ARM_ARCH_8M_BASE__) || defined(__ARM_ARCH_8M_MAIN__)
    return (return_addr & EXC_RETURN_FTYPE) == 0;
#elif defined(__ARM_ARCH_6M__)
    return false;
#else
    return return_addr == EXC_RETURN_THREAD_PSP_FPU;
#endif
}
 
static inline bool cortex_is_thread_psp_used(uint32_t return_addr)
{
#if defined(__ARM_ARCH_8M_BASE__) || defined(__ARM_ARCH_8M_MAIN__)
    return (return_addr & (EXC_RETURN_MODE | EXC_RETURN_SPSEL)) == (EXC_RETURN_MODE | EXC_RETURN_SPSEL);
#elif defined(__ARM_ARCH_6M__)
    return return_addr == EXC_RETURN_THREAD_PSP;
#else
    return return_addr == EXC_RETURN_THREAD_PSP || return_addr == EXC_RETURN_THREAD_PSP_FPU;
#endif
}