@@ -75,6 +75,7 @@ bool memfault_arch_is_inside_isr(void) {
7575 // Reference Manual section "B1.3.1 ARM processor modes" for these values.
7676 #define CPSR_MODE_msk 0x1f
7777 #define CPSR_USER_msk 0x10
78+ #define CPSR_SUPERVISOR_msk 0x13
7879 #define CPSR_SYSTEM_msk 0x1f
7980
8081 uint32_t cpsr ;
@@ -84,8 +85,9 @@ bool memfault_arch_is_inside_isr(void) {
8485
8586 const bool in_user_mode = (mode == CPSR_USER_msk );
8687 const bool in_system_mode = (mode == CPSR_SYSTEM_msk );
88+ const bool in_supervisor_mode = (mode == CPSR_SUPERVISOR_msk );
8789
88- return !(in_user_mode || in_system_mode );
90+ return !(in_user_mode || in_system_mode || in_supervisor_mode );
8991}
9092
9193#if defined(__GNUC__ )
@@ -237,6 +239,9 @@ void memfault_fault_handler(const sMfltRegState *regs, eMemfaultRebootReason rea
237239// Processor mode values, used when saving LR and SPSR to the appropriate stack.
238240// From https://developer.arm.com/documentation/dui0801/a/CHDEDCCD
239241// Defined as strings for macro concatenation below
242+ #define CPU_MODE_FIQ_STR "0x11"
243+ #define CPU_MODE_IRQ_STR "0x12"
244+ #define CPU_MODE_SUPERVISOR_STR "0x13"
240245#define CPU_MODE_ABORT_STR "0x17"
241246#define CPU_MODE_UNDEFINED_STR "0x1b"
242247#define CPU_MODE_SYSTEM_STR "0x1f"
@@ -281,7 +286,28 @@ void memfault_fault_handler(const sMfltRegState *regs, eMemfaultRebootReason rea
281286 /* push user mode regs at point of fault, including sp + lr */ \
282287 "mov r8, r1 \n" \
283288 "mov r1, sp \n" \
289+ /* Save SPSR and mask mode bits */ \
290+ "mrs r9, spsr \n" \
291+ "and r9, #0x1f \n" \
292+ /* Check each applicable mode and set current mode on a match */ \
293+ "cmp r9, #" CPU_MODE_IRQ_STR " \n" \
294+ "bne fiq_mode_%= \n" \
295+ "cps #" CPU_MODE_IRQ_STR " \n" \
296+ "b store_regs_%= \n" \
297+ "fiq_mode_%=: \n" \
298+ "cmp r9, #" CPU_MODE_FIQ_STR " \n" \
299+ "bne supervisor_mode_%= \n" \
300+ "cps #" CPU_MODE_FIQ_STR " \n" \
301+ "b store_regs_%= \n" \
302+ "supervisor_mode_%=: \n" \
303+ "cmp r9, #" CPU_MODE_SUPERVISOR_STR " \n" \
304+ "bne system_mode_%= \n" \
305+ "cps #" CPU_MODE_SUPERVISOR_STR " \n" \
306+ "b store_regs_%= \n" \
307+ /* Fall back to system mode if no match */ \
308+ "system_mode_%=: \n" \
284309 "cps #" CPU_MODE_SYSTEM_STR " \n" \
310+ "store_regs_%=: \n" \
285311 "stmfd r1!, {r8-r12, sp, lr} \n" \
286312 "cps #" _mode_string " \n" \
287313 /* save active registers in exception frame */ \
0 commit comments