X-Git-Url: https://codewiz.org/gitweb?a=blobdiff_plain;f=bertos%2Fcpu%2Fframe.h;fp=bertos%2Fcpu%2Fframe.h;h=b65aaed710dc9aeafe1a13e50f9f5ddeca96a67b;hb=77958b8e9d27594aa22e76132b4bbb8d0335bfc2;hp=e8d0f898973458fe214c4ad1e5272b86ccda1ad1;hpb=ffc9d84f819c7480515ece4417329f4972510462;p=bertos.git

diff --git a/bertos/cpu/frame.h b/bertos/cpu/frame.h
index e8d0f898..b65aaed7 100644
--- 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
 	/*