diff options
Diffstat (limited to 'ports/ARMCM3/chcore.c')
-rw-r--r-- | ports/ARMCM3/chcore.c | 52 |
1 files changed, 32 insertions, 20 deletions
diff --git a/ports/ARMCM3/chcore.c b/ports/ARMCM3/chcore.c index bb9bc1d83..16560842a 100644 --- a/ports/ARMCM3/chcore.c +++ b/ports/ARMCM3/chcore.c @@ -118,6 +118,34 @@ void SVCallVector(Thread *otp, Thread *ntp) { #endif } +#ifdef CH_CURRP_REGISTER_CACHE +#define PUSH_CONTEXT(sp) { \ + register uint32_t tmp asm ("r3") = BASEPRI_USER; \ + asm volatile ("mrs %0, PSP \n\t" \ + "stmdb %0!, {r3-r6,r8-r11, lr}" : \ + "=r" (sp) : "r" (sp), "r" (tmp)); \ +} + +#define POP_CONTEXT(sp) { \ + asm volatile ("ldmia %0!, {r3-r6,r8-r11, lr} \n\t" \ + "msr PSP, %0 \n\t" \ + "msr BASEPRI, r3" : "=r" (sp) : "r" (sp)); \ +} +#else +#define PUSH_CONTEXT(sp) { \ + register uint32_t tmp asm ("r3") = BASEPRI_USER; \ + asm volatile ("mrs %0, PSP \n\t" \ + "stmdb %0!, {r3-r11,lr}" : \ + "=r" (sp) : "r" (sp), "r" (tmp)); \ +} + +#define POP_CONTEXT(sp) { \ + asm volatile ("ldmia %0!, {r3-r11, lr} \n\t" \ + "msr PSP, %0 \n\t" \ + "msr BASEPRI, r3" : "=r" (sp) : "r" (sp)); \ +} +#endif + /* * Preemption invoked context switch. */ @@ -134,18 +162,7 @@ void PendSVVector(void) { } asm volatile ("pop {lr}"); - register uint32_t tmp asm ("r3") = BASEPRI_USER; -#ifdef CH_CURRP_REGISTER_CACHE - asm volatile ("mrs %0, PSP \n\t" \ - "stmdb %0!, {r3-r6,r8-r11, lr}" : - "=r" (sp_thd) : - "r" (sp_thd), "r" (tmp)); -#else - asm volatile ("mrs %0, PSP \n\t" \ - "stmdb %0!, {r3-r11,lr}" : - "=r" (sp_thd) : - "r" (sp_thd), "r" (tmp)); -#endif + PUSH_CONTEXT(sp_thd); (otp = currp)->p_ctx.r13 = sp_thd; chSchReadyI(otp); @@ -159,12 +176,7 @@ void PendSVVector(void) { #endif sp_thd = currp->p_ctx.r13; -#ifdef CH_CURRP_REGISTER_CACHE - asm volatile ("ldmia %0!, {r3-r6,r8-r11, lr}" : : "r" (sp_thd)); -#else - asm volatile ("ldmia %0!, {r3-r11, lr}" : : "r" (sp_thd)); -#endif - asm volatile ("msr PSP, %0 \n\t" \ - "msr BASEPRI, r3 \n\t" \ - "bx lr" : : "r" (sp_thd)); + POP_CONTEXT(sp_thd); + + asm volatile ("bx lr"); } |