From 24359085b9ed5368480ef7a21af26b83879a351e Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Mon, 17 Mar 2008 15:43:03 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@239
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 ports/ARMCM3/chcore.c | 20 ++++++++++++++++++++
 ports/ARMCM3/chcore.h |  5 +++--
 2 files changed, 23 insertions(+), 2 deletions(-)

(limited to 'ports/ARMCM3')

diff --git a/ports/ARMCM3/chcore.c b/ports/ARMCM3/chcore.c
index a98da808b..7ef8f8daf 100644
--- a/ports/ARMCM3/chcore.c
+++ b/ports/ARMCM3/chcore.c
@@ -66,5 +66,25 @@ void threadstart(void) {
   asm volatile ("mov     r0, r5                                 \n\t" \
                 "blx     r4                                     \n\t" \
                 "bl      chThdExit                              ");
+}
+
+void *retaddr;
+
+__attribute__((naked, weak))
+void threadswitch(void) {
+
+  asm volatile ("sub     sp, sp, #4                             \n\t" \
+                "push    {r0-r3, r12, lr}                       \n\t" \
+                "mrs     r0, XPSR                               \n\t" \
+                "push    {r0}                                   \n\t" \
+                "ldr     r0, =retaddr                           \n\t" \
+                "str     r0, [sp, #28]                          ");
+
+  chSchDoRescheduleI();
 
+  asm volatile ("pop     {r0}                                   \n\t" \
+                "msr     XPSR, r0                               \n\t" \
+                "pop     {r0-r3, r12, lr}                       \n\t" \
+                "cpsie   i                                      \n\t" \
+                "pop     {pc}                                   ");
 }
diff --git a/ports/ARMCM3/chcore.h b/ports/ARMCM3/chcore.h
index 706f49c28..9fee0c61d 100644
--- a/ports/ARMCM3/chcore.h
+++ b/ports/ARMCM3/chcore.h
@@ -26,13 +26,14 @@ typedef void *regarm;
  * Interrupt saved context.
  */
 struct extctx {
-  regarm  spsr_irq;
-  regarm  lr_irq;
+  regarm  pc;
+  regarm  xpsr;
   regarm  r0;
   regarm  r1;
   regarm  r2;
   regarm  r3;
   regarm  r12;
+  regarm  lr;
 };
 
 /*
-- 
cgit v1.2.3