diff --git a/ports/ARM7-LPC214x/GCC/chcore.c b/ports/ARM7-LPC214x/GCC/chcore.c
index fc4c01942..d562a1527 100644
--- a/ports/ARM7-LPC214x/GCC/chcore.c
+++ b/ports/ARM7-LPC214x/GCC/chcore.c
@@ -69,7 +69,7 @@ void chSysUnlock(void) {
 }
 #endif
 
-void chSysSwitchI(Context *oldp, Context *newp) {
+void chSysSwitchI(struct Thread *otp, struct Thread *ntp) {
 
 #ifdef THUMB
   asm(".p2align 2,,                                             \n\t" \
@@ -80,8 +80,8 @@ void chSysSwitchI(Context *oldp, Context *newp) {
 
 #ifdef CH_CURRP_REGISTER_CACHE
   asm("stmfd    sp!, {r4, r5, r6, r8, r9, r10, r11, lr}         \n\t" \
-      "str      sp, [r0, #0]                                    \n\t" \
-      "ldr      sp, [r1, #0]                                    \n\t");
+      "str      sp, [r0, #16]                                   \n\t" \
+      "ldr      sp, [r1, #16]                                   \n\t");
 #ifdef THUMB_PRESENT
   asm("ldmfd    sp!, {r4, r5, r6, r8, r9, r10, r11, lr}         \n\t" \
       "bx       lr                                              \n\t");
@@ -90,8 +90,8 @@ void chSysSwitchI(Context *oldp, Context *newp) {
 #endif /* !THUMB_PRESENT */
 #else /* !CH_CURRP_REGISTER_CACHE */
   asm("stmfd    sp!, {r4, r5, r6, r7, r8, r9, r10, r11, lr}     \n\t" \
-      "str      sp, [r0, #0]                                    \n\t" \
-      "ldr      sp, [r1, #0]                                    \n\t");
+      "str      sp, [r0, #16]                                   \n\t" \
+      "ldr      sp, [r1, #16]                                   \n\t");
 #ifdef THUMB_PRESENT
   asm("ldmfd    sp!, {r4, r5, r6, r7, r8, r9, r10, r11, lr}     \n\t" \
       "bx       lr                                              \n\t");
diff --git a/ports/ARM7-LPC214x/GCC/chcore.h b/ports/ARM7-LPC214x/GCC/chcore.h
index 05223170d..624386f15 100644
--- a/ports/ARM7-LPC214x/GCC/chcore.h
+++ b/ports/ARM7-LPC214x/GCC/chcore.h
@@ -122,7 +122,7 @@ extern void chSysUnlock(void);
 void _IdleThread(void *p) __attribute__((noreturn));
 
 void chSysHalt(void);
-void chSysSwitchI(Context *oldp, Context *newp);
+void chSysSwitchI(struct Thread *otp, struct Thread *ntp);
 void chSysPuts(char *msg);
 void threadstart(void);
 
diff --git a/src/chschd.c b/src/chschd.c
index 4c3b05760..cbd1cfdd3 100644
--- a/src/chschd.c
+++ b/src/chschd.c
@@ -87,7 +87,7 @@ void chSchGoSleepS(t_tstate newstate) {
 #ifdef CH_USE_TRACE
   chDbgTrace(otp, currp);
 #endif
-  chSysSwitchI(&otp->p_ctx, &currp->p_ctx);
+  chSysSwitchI(otp, currp);
 }
 
 /**
@@ -114,7 +114,7 @@ void chSchWakeupS(Thread *ntp, t_msg msg) {
 #ifdef CH_USE_TRACE
     chDbgTrace(otp, ntp);
 #endif
-    chSysSwitchI(&otp->p_ctx, &ntp->p_ctx);
+    chSysSwitchI(otp, ntp);
   }
 }
 
@@ -131,7 +131,7 @@ void chSchDoRescheduleI(void) {
 #ifdef CH_USE_TRACE
   chDbgTrace(otp, currp);
 #endif
-  chSysSwitchI(&otp->p_ctx, &currp->p_ctx);
+  chSysSwitchI(otp, currp);
 }
 
 /**
diff --git a/src/include/ch.h b/src/include/ch.h
index 2c2bb2f7c..dcb486626 100644
--- a/src/include/ch.h
+++ b/src/include/ch.h
@@ -27,6 +27,8 @@
 
 #define _CHIBIOS_RT_
 
+struct Thread;
+
 #ifndef __DOXIGEN__
 #ifndef _CHCONF_H_
 #include <chconf.h>
diff --git a/src/include/scheduler.h b/src/include/scheduler.h
index 183e3af79..67fae50a4 100644
--- a/src/include/scheduler.h
+++ b/src/include/scheduler.h
@@ -61,8 +61,8 @@ extern "C" {
   void chSchReadyI(Thread *tp, t_msg msg);
   void chSchGoSleepS(t_tstate newstate);
   void chSchWakeupS(Thread *tp, t_msg msg);
-  void chSchRescheduleS(void);
   void chSchDoRescheduleI(void);
+  void chSchRescheduleS(void);
   BOOL chSchRescRequiredI(void);
 #ifdef __cplusplus
 }
diff --git a/src/include/threads.h b/src/include/threads.h
index ba53e6158..a0c2189da 100644
--- a/src/include/threads.h
+++ b/src/include/threads.h
@@ -47,6 +47,8 @@ struct Thread {
   t_tstate          p_state;
   /** Mode flags.*/
   t_tmode           p_flags;
+  /** Machine dependent processor context.*/
+  Context           p_ctx;
   /*
    * The following fields are merged in unions because they are all
    * state-specific fields. This trick saves some extra space for each
@@ -79,8 +81,6 @@ struct Thread {
     void            *p_wtobjp;
 #endif
   };
-  /** Machine dependent processor context.*/
-  Context           p_ctx;
   /*
    * Start of the optional fields.
    */