diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/stm/ao_arch_funcs.h | 16 |
1 files changed, 15 insertions, 1 deletions
diff --git a/src/stm/ao_arch_funcs.h b/src/stm/ao_arch_funcs.h index 42f1a2e5..02b344b1 100644 --- a/src/stm/ao_arch_funcs.h +++ b/src/stm/ao_arch_funcs.h @@ -335,6 +335,13 @@ static inline void ao_arch_save_stack(void) { static inline void ao_arch_restore_stack(void) { uint32_t sp; + uint32_t control; + + asm("mrs %0,control" : "=&r" (control)); + control |= (1 << 1); + asm("msr control,%0" : : "r" (control)); + asm("isb"); + sp = (uint32_t) ao_cur_task->sp; /* Switch stacks */ @@ -375,7 +382,14 @@ static inline void ao_arch_start_scheduler(void) { } #endif -#define ao_arch_isr_stack() +static inline void ao_arch_isr_stack(void) { + uint32_t control; + + asm("mrs %0,control" : "=&r" (control)); + control &= ~(1 << 1); + asm("msr control,%0" : : "r" (control)); + asm("isb"); +} #endif |