From 8429df68917eb18bb64b38fa73243282013f57bc Mon Sep 17 00:00:00 2001 From: gdisirio Date: Fri, 26 Mar 2010 12:55:23 +0000 Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1780 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ports/GCC/ARMCMx/chcore.c | 81 +++++++++++++++++++------------------------- 1 file changed, 34 insertions(+), 47 deletions(-) (limited to 'os') diff --git a/os/ports/GCC/ARMCMx/chcore.c b/os/ports/GCC/ARMCMx/chcore.c index 1015f030d..37a31b756 100644 --- a/os/ports/GCC/ARMCMx/chcore.c +++ b/os/ports/GCC/ARMCMx/chcore.c @@ -69,50 +69,11 @@ void SysTickVector(void) { chSysUnlockFromIsr(); } -/** - * @brief SVC vector. - * @details The SVC vector is used for commanded context switch. Structures - * @p intctx are saved and restored from the process stacks of the - * switched threads. - * - * @param[in] ntp the thread to be switched it - * @param[in] otp the thread to be switched out - */ -#if !defined(__DOXYGEN__) -__attribute__((naked)) -#endif -void SVCallVector(Thread *ntp, Thread *otp) { - (void)otp; - (void)ntp; -#ifdef CH_CURRP_REGISTER_CACHE - asm volatile ("mrs r3, BASEPRI \n\t" \ - "mrs r12, PSP \n\t" \ - "stmdb r12!, {r3-r6,r8-r11, lr} \n\t" \ - "str r12, [r1, #12] \n\t" \ - "ldr r12, [r0, #12] \n\t" \ - "ldmia r12!, {r3-r6,r8-r11, lr} \n\t" \ - "msr PSP, r12 \n\t" \ - "msr BASEPRI, r3 \n\t" \ - "bx lr "); -#else - asm volatile ("mrs r3, BASEPRI \n\t" \ - "mrs r12, PSP \n\t" \ - "stmdb r12!, {r3-r11, lr} \n\t" \ - "str r12, [r1, #12] \n\t" \ - "ldr r12, [r0, #12] \n\t" \ - "ldmia r12!, {r3-r11, lr} \n\t" \ - "msr PSP, r12 \n\t" \ - "msr BASEPRI, r3 \n\t" \ - "bx lr "); -#endif -} - #ifdef CH_CURRP_REGISTER_CACHE -#define PUSH_CONTEXT(sp) { \ - register uint32_t tmp asm ("r3") = CORTEX_BASEPRI_USER; \ +#define PUSH_CONTEXT(sp, prio) { \ asm volatile ("mrs %0, PSP \n\t" \ "stmdb %0!, {r3-r6,r8-r11, lr}" : \ - "=r" (sp) : "r" (sp), "r" (tmp)); \ + "=r" (sp) : "r" (sp), "r" (prio)); \ } #define POP_CONTEXT(sp) { \ @@ -122,21 +83,45 @@ void SVCallVector(Thread *ntp, Thread *otp) { "bx lr" : "=r" (sp) : "r" (sp)); \ } #else -#define PUSH_CONTEXT(sp) { \ - register uint32_t tmp asm ("r3") = CORTEX_BASEPRI_USER; \ +#define PUSH_CONTEXT(sp, prio) { \ asm volatile ("mrs %0, PSP \n\t" \ "stmdb %0!, {r3-r11,lr}" : \ - "=r" (sp) : "r" (sp), "r" (tmp)); \ + "=r" (sp) : "r" (sp), "r" (prio)); \ } #define POP_CONTEXT(sp) { \ - asm volatile ("ldmia %0!, {r3-r11, lr} \n\t" \ + asm volatile ("ldmia r12!, {r3-r11, lr} \n\t" \ "msr PSP, %0 \n\t" \ "msr BASEPRI, r3 \n\t" \ "bx lr" : "=r" (sp) : "r" (sp)); \ } #endif +/** + * @brief SVC vector. + * @details The SVC vector is used for commanded context switch. Structures + * @p intctx are saved and restored from the process stacks of the + * switched threads. + * + * @param[in] ntp the thread to be switched it + * @param[in] otp the thread to be switched out + */ +#if !defined(__DOXYGEN__) +__attribute__((naked)) +#endif +void SVCallVector(Thread *ntp, Thread *otp) { + register struct intctx *sp_thd asm("r12"); + register uint32_t prio asm ("r3"); + + asm volatile ("mrs r3, BASEPRI" : "=r" (prio) : ); + PUSH_CONTEXT(sp_thd, prio); + + otp->p_ctx.r13 = sp_thd; + sp_thd = ntp->p_ctx.r13; + + POP_CONTEXT(sp_thd); +} + /** * @brief Preemption code. */ @@ -144,12 +129,14 @@ void SVCallVector(Thread *ntp, Thread *otp) { __attribute__((naked)) #endif void PendSVVector(void) { - Thread *otp, *ntp; register struct intctx *sp_thd asm("r12"); + register uint32_t prio asm ("r3"); + Thread *otp, *ntp; chSysLockFromIsr(); - PUSH_CONTEXT(sp_thd); + prio = CORTEX_BASEPRI_USER; + PUSH_CONTEXT(sp_thd, prio); (otp = currp)->p_ctx.r13 = sp_thd; ntp = fifo_remove(&rlist.r_queue); -- cgit v1.2.3