CM3: kernel preemption.

[bertos.git] / bertos / cpu / frame.h

diff --git a/bertos/cpu/frame.h b/bertos/cpu/frame.h
index e8d0f898973458fe214c4ad1e5272b86ccda1ad1..b65aaed710dc9aeafe1a13e50f9f5ddeca96a67b 100644 (file)
--- a/bertos/cpu/frame.h
+++ b/bertos/cpu/frame.h
@@ -157,12 +157,48 @@
 
 #elif CPU_CM3
 
-
-       #define CPU_PUSH_CALL_FRAME(sp, func) \
-               do { \
-                       CPU_PUSH_WORD((sp), 0x01000000);          /* xPSR  */   \
-                       CPU_PUSH_WORD((sp), (cpu_stack_t)(func)); /* lr  */     \
-               } while (0);
+       #if CONFIG_KERN_PREEMPT
+               INLINE void asm_switch_context(cpu_stack_t **new_sp, cpu_stack_t **old_sp)
+               {
+                       register cpu_stack_t **__new_sp asm ("r0") = new_sp;
+                       register cpu_stack_t **__old_sp asm ("r1") = old_sp;
+
+                       asm volatile ("svc #0"
+                               : : "r"(__new_sp), "r"(__old_sp) : "memory", "cc");
+               }
+               #define asm_switch_context asm_switch_context
+
+               #define CPU_PUSH_CALL_FRAME(sp, func) \
+                       do { \
+                               CPU_PUSH_WORD((sp), 0x01000000);                /* xPSR    */   \
+                               CPU_PUSH_WORD((sp), (cpu_stack_t)(func));       /* pc      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* lr      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* ip      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* r3      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* r2      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* r1      */   \
+                               CPU_PUSH_WORD((sp), 0);                         /* r0      */   \
+                               CPU_PUSH_WORD((sp), 0xfffffffd);                /* lr_exc  */   \
+                       } while (0);
+
+               #define CPU_CREATE_NEW_STACK(stack) \
+                       do { \
+                               size_t i; \
+                               /* Initialize process stack frame */ \
+                               CPU_PUSH_CALL_FRAME(stack, proc_entry); \
+                               /* Push a clean set of CPU registers for asm_switch_context() */ \
+                               for (i = 0; i < CPU_SAVED_REGS_CNT; i++) \
+                                       CPU_PUSH_WORD(stack, CPU_REG_INIT_VALUE(i)); \
+                               CPU_PUSH_WORD(stack, IRQ_PRIO_DISABLED); \
+                       } while (0)
+
+       #else /* !CONFIG_KERN_PREEMPT */
+               #define CPU_PUSH_CALL_FRAME(sp, func) \
+                       do { \
+                               CPU_PUSH_WORD((sp), 0x01000000);                /* xPSR    */   \
+                               CPU_PUSH_WORD((sp), (cpu_stack_t)(func));       /* pc      */   \
+                       } while (0);
+       #endif /* CONFIG_KERN_PREEMPT */
 
 #elif CPU_AVR
        /*