git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@267 35acf78f-673a-0410-8e92-d51de3d6d3f4

3 files changed, 51 insertions, 40 deletions

diff --git a/ports/ARMCM3/chcore.c b/ports/ARMCM3/chcore.c
index 25df8413f..c5424848e 100644
--- a/ports/ARMCM3/chcore.c
+++ b/ports/ARMCM3/chcore.c
@@ -43,9 +43,7 @@ void chSysPuts(char *msg) {
  */

 void chSysSwitchI(Thread *otp, Thread *ntp) {

 

-  asm volatile ("cpsie   i                                      \n\t" \

-                "svc     #0                                     \n\t" \

-                "cpsid   i                                      ");

+  asm volatile ("svc     #0");

 }

 

 /*

@@ -54,7 +52,7 @@ void chSysSwitchI(Thread *otp, Thread *ntp) {
 __attribute__((naked, weak))

 void chSysHalt(void) {

 

-  chSysLock();

+  asm volatile ("cpsid   i");

   while (TRUE) {

   }

 }

@@ -62,10 +60,11 @@ void chSysHalt(void) {
 __attribute__((naked, weak))

 void threadstart(void) {

 

-  asm volatile ("cpsie   i                                      \n\t" \

+  asm volatile ( \

                 "blx     r1                                     \n\t" \

                 "bl      chThdExit                              \n\t" \

-                "bl      chSysHalt                              ");

+                "bl      chSysHalt                              \n\t" \

+               );

 }

 

 /*

@@ -85,41 +84,30 @@ void SysTickVector(void) {
 void *retaddr;

 

 /*

- * To be invoked at the end of any interrupt handler that can trigger a

- * reschedule.

- */

-void chSysIRQExitI(void) {

-

-  chSysLock();

-

-  if ((SCB_ICSR & ICSR_RETTOBASE) && chSchRescRequiredI()) {

-    SCB_ICSR = ICSR_PENDSVSET;

-  }

-

-  chSysUnlock();

-}

-

-/*

  * System invoked context switch.

  */

 __attribute__((naked))

 void SVCallVector(Thread *otp, Thread *ntp) {

 

 #ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("mrs     r12, PSP                               \n\t" \

-                "stmdb   r12!, {r4-6,r8-r11, lr}                \n\t" \

+  asm volatile ("mrs     r3, BASEPRI                            \n\t" \

+                "mrs     r12, PSP                               \n\t" \

+                "stmdb   r12!, {r3-r6,r8-r11, lr}               \n\t" \

                 "str     r12, [r0, #16]                         \n\t" \

                 "ldr     r12, [r1, #16]                         \n\t" \

-                "ldmia   r12!, {r4-6,r8-r11, lr}                \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     r12, PSP                               \n\t" \

-                "stmdb   r12!, {r4-r11, lr}                     \n\t" \

+  asm volatile ("mrs     r3, BASEPRI                            \n\t" \

+                "mrs     r12, PSP                               \n\t" \

+                "stmdb   r12!, {r3-r11, lr}                     \n\t" \

                 "str     r12, [r0, #16]                         \n\t" \

                 "ldr     r12, [r1, #16]                         \n\t" \

-                "ldmia   r12!, {r4-r11, lr}                     \n\t" \

+                "ldmia   r12!, {r3-r11, lr}                     \n\t" \

                 "msr     PSP, r12                               \n\t" \

+                "msr     BASEPRI, r3                            \n\t" \

                 "bx      lr                                     ");

 #endif

 }

@@ -132,34 +120,41 @@ void PendSVVector(void) {
   Thread *otp;

   register struct intctx *sp_thd asm("r12");

 

-  asm volatile ("cpsid   i                                      \n\t" \

+  chSysLock();

+  asm volatile ("push    {lr}");

+  if (!chSchRescRequiredI()) {

+    chSysUnlock();

+    asm volatile ("pop     {pc}");

+  }

+

+  asm volatile ("pop     {lr}                                   \n\t" \

+                "mov     r3, #0                                 \n\t" \

                 "mrs     %0, PSP" : "=r" (sp_thd) : );

 #ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("stmdb   %0!, {r4-r6,r8-r11, lr}" :

+  asm volatile ("stmdb   %0!, {r3-r6,r8-r11, lr}" :

                 "=r" (sp_thd) :

                 "r" (sp_thd));

 #else

-  asm volatile ("stmdb   %0!, {r4-r11, lr}" :

+  asm volatile ("stmdb   %0!, {r3-r11,lr}" :

                 "=r" (sp_thd) :

                 "r" (sp_thd));

 #endif

 

-  (otp  = currp)->p_ctx.r13 = sp_thd;

+  (otp = currp)->p_ctx.r13 = sp_thd;

   chSchReadyI(otp);

   (currp = fifo_remove(&rlist.r_queue))->p_state = PRCURR;

   rlist.r_preempt = CH_TIME_QUANTUM;

 #ifdef CH_USE_TRACE

   chDbgTrace(otp, currp);

 #endif

-

   sp_thd = currp->p_ctx.r13;

 

 #ifdef CH_CURRP_REGISTER_CACHE

-  asm volatile ("ldmia   %0!, {r4-r6,r8-r11, lr}" : : "r" (sp_thd));

+  asm volatile ("ldmia   %0!, {r3-r6,r8-r11, lr}" : : "r" (sp_thd));

 #else

-  asm volatile ("ldmia   %0!, {r4-r11, lr}" : : "r" (sp_thd));

+  asm volatile ("ldmia   %0!, {r3-r11, lr}" : : "r" (sp_thd));

 #endif

   asm volatile ("msr     PSP, %0                                \n\t" \

-                "cpsie   i                                      \n\t" \

+                "msr     BASEPRI, r3                            \n\t" \

                 "bx      lr" : : "r" (sp_thd));

 }

diff --git a/ports/ARMCM3/chcore.h b/ports/ARMCM3/chcore.h
index ccb070228..e9fc693e0 100644
--- a/ports/ARMCM3/chcore.h
+++ b/ports/ARMCM3/chcore.h
@@ -32,6 +32,7 @@ struct extctx {
  * System saved context.

  */

 struct intctx {

+  regarm  basepri;

   regarm  r4;

   regarm  r5;

   regarm  r6;

@@ -68,15 +69,26 @@ typedef struct {
   tp->p_ctx.r13 = (struct intctx *)((uint8_t *)workspace +              \

                                      wsize -                            \

                                      sizeof(struct intctx));            \

+  tp->p_ctx.r13->basepri = 0;                                           \

+  tp->p_ctx.r13->lr_exc = (regarm)0xFFFFFFFD;                           \

   tp->p_ctx.r13->r0 = arg;                                              \

   tp->p_ctx.r13->r1 = pf;                                               \

-  tp->p_ctx.r13->lr_exc = (regarm)0xFFFFFFFD;                           \

   tp->p_ctx.r13->pc = threadstart;                                      \

   tp->p_ctx.r13->xpsr = (regarm)0x01000000;                             \

 }

 

-#define chSysLock() asm("cpsid   i")

-#define chSysUnlock() asm("cpsie   i")

+#define chSysLock() {                                                   \

+  asm volatile ("push    {r12}");                                       \

+  asm volatile ("mov     r12, #0x10");                                  \

+  asm volatile ("msr     BASEPRI, r12");                                \

+  asm volatile ("pop     {r12}");                                       \

+}

+#define chSysUnlock() {                                                 \

+  asm volatile ("push    {r12}");                                       \

+  asm volatile ("mov     r12, #0");                                     \

+  asm volatile ("msr     BASEPRI, r12");                                \

+  asm volatile ("pop     {r12}");                                       \

+}

 

 #define INT_REQUIRED_STACK 0

 #define StackAlign(n) ((((n) - 1) | 3) + 1)

@@ -88,6 +100,9 @@ typedef struct {
 #define WorkingArea(s, n) uint32_t s[UserStackSize(n) >> 2];

 

 #define chSysIRQEnterI()

+#define chSysIRQExitI() {                                               \

+  SCB_ICSR = ICSR_PENDSVSET;                                            \

+}

 

 /* It should be 8.*/

 #define IDLE_THREAD_STACK_SIZE 0

@@ -97,6 +112,5 @@ void chSysHalt(void);
 void chSysSwitchI(Thread *otp, Thread *ntp);

 void chSysPuts(char *msg);

 void threadstart(void);

-void chSysIRQExitI(void);

 

 #endif /* _CHCORE_H_ */

diff --git a/ports/ARMCM3/crt0.s b/ports/ARMCM3/crt0.s
index 836934c33..bd0b8b1a5 100644
--- a/ports/ARMCM3/crt0.s
+++ b/ports/ARMCM3/crt0.s
@@ -76,7 +76,9 @@ bloop:
          */

         mov     r0, #CONTROL_MODE_PRIVILEGED | CONTROL_USE_PSP

         msr     CONTROL, r0

-        cpsid   i

+	mov	r0, #0x10

+	msr	BASEPRI, r0

+        cpsie   i

         /*

          * Application-provided HW initialization routine.

          */