diff --git a/firmware/kernel.c b/firmware/kernel.c
index 012ff94c73..da3ddf5ca7 100644
--- a/firmware/kernel.c
+++ b/firmware/kernel.c
@@ -17,6 +17,7 @@
  *
  ****************************************************************************/
 #include "kernel.h"
+#include "thread.h"
 
 long current_tick = 0;
 
@@ -32,4 +33,5 @@ void sleep(int ticks)
 
 void yield(void)
 {
+    switch_thread();
 }
diff --git a/firmware/test/kernel/Makefile b/firmware/test/kernel/Makefile
index f632b3f6de..0740f6809c 100644
--- a/firmware/test/kernel/Makefile
+++ b/firmware/test/kernel/Makefile
@@ -14,7 +14,8 @@ TARGET = -DARCHOS_PLAYER_OLD=1
 CFLAGS = -g -O -Wall -m1 -save-temps -nostdlib -Wstrict-prototypes -fomit-frame-pointer -fschedule-insns -fno-builtin $(INCLUDES) $(TARGET)
 AFLAGS += -small -relax
 
-OBJS= ../../crt0.o ../../system.o main.o timer.o ../../thread.o ../../debug.o
+OBJS= ../../crt0.o ../../system.o main.o timer.o \
+	../../thread.o ../../kernel.o ../../debug.o
 
 %.o: %.S
 	$(CC) -o $@ $(CFLAGS) $(INCLUDES) $(DEFS) -c $<
diff --git a/firmware/test/kernel/main.c b/firmware/test/kernel/main.c
index 6ce99c8065..b52ecff21d 100644
--- a/firmware/test/kernel/main.c
+++ b/firmware/test/kernel/main.c
@@ -17,6 +17,7 @@
  *
  ****************************************************************************/
 #include "thread.h"
+#include "kernel.h"
 #include "sh7034.h"
 #include "debug.h"
 
@@ -30,11 +31,11 @@ void t2(void);
 
 int main(void)
 {
-   char buf[40];
-   char str[32];
-   int i=0;
+    char buf[40];
+    char str[32];
+    int i=0;
 
-   /* Clear it all! */
+    /* Clear it all! */
     SSR1 &= ~(SCI_RDRF | SCI_ORER | SCI_PER | SCI_FER);
 
     /* This enables the serial Rx interrupt, to be able to exit into the
@@ -46,31 +47,31 @@ int main(void)
     debugf("OK. Let's go\n");
 
     tick_start(40);
-    
+
     create_thread(t1, s1, 1024);
     create_thread(t2, s2, 1024);
 
     while(1)
     {
-       debugf("t0\n");
-       switch_thread();
+	debugf("t0\n");
+	sleep(100);
     }
 }
 
 void t1(void)
 {
-   while(1)
-   {
-      debugf("t1\n");
-      switch_thread();
-   }
+    while(1)
+    {
+	debugf("t1\n");
+	sleep(200);
+    }
 }
 
 void t2(void)
 {
-   while(1)
-   {
-      debugf("t2\n");
-      switch_thread();
-   }
+    while(1)
+    {
+	debugf("t2\n");
+	sleep(300);
+    }
 }
diff --git a/firmware/test/kernel/timer.c b/firmware/test/kernel/timer.c
index be253db2b5..31c6f82baa 100644
--- a/firmware/test/kernel/timer.c
+++ b/firmware/test/kernel/timer.c
@@ -19,6 +19,7 @@
 #include "sh7034.h"
 #include "system.h"
 #include "debug.h"
+#include "kernel.h"
 
 void tick_start(unsigned int interval_in_ms)
 {
@@ -26,6 +27,7 @@ void tick_start(unsigned int interval_in_ms)
 
     count = FREQ / 1000 / 8 * interval_in_ms;
 
+    debugf("count = %d\n", count);
     if(count > 0xffff)
     {
 	debugf("Error! The tick interval is too long (%d ms)\n",
@@ -40,24 +42,22 @@ void tick_start(unsigned int interval_in_ms)
     TMDR &= ~0x01; /* Operate normally */
 
     TCNT0 = 0;   /* Start counting at 0 */
-    GRA0 = 0xfff0;
+    GRA0 = count;
     TCR0 = 0x23; /* Clear at GRA match, sysclock/8 */
 
-    TSTR |= 0x01; /* Start timer 1 */
-
     /* Enable interrupt on level 1 */
     IPRC = (IPRC & ~0x00f0) | 0x0010;
-
-    TIER0 |= 0x01; /* Enable GRA match interrupt */
     
-    while(1)
-    {
-    }
+    TSR0 &= ~0x01;
+    TIER0 |= 0x01; /* Enable GRA match interrupt */
+
+    TSTR |= 0x01; /* Start timer 1 */
 }
 
 #pragma interrupt
 void IMIA0(void)
 {
+    current_tick++;
+
     TSR0 &= ~0x01;
-    debugf("Yes\n");
 }
diff --git a/firmware/thread.c b/firmware/thread.c
index e6bc1dd2ff..e3116fbeb5 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -52,16 +52,18 @@ static inline void stctx(void* addr)
                   "mov.l r9, @(4, %1)\n\t"
                   "mov.l r10, @(8, %1)\n\t"
                   "mov.l r11, @(12, %1)\n\t"
-                  "mov.l r12, @(16, %1)\n\t"
-                  "mov.l r13, @(20, %1)\n\t"
-                  "mov.l r14, @(24, %1)\n\t"
-                  "mov.l r15, @(28, %1)\n\t"
+                  "add #16,%1\n\t"
+                  "mov.l r12, @(0, %1)\n\t"
+                  "mov.l r13, @(4, %1)\n\t"
+                  "mov.l r14, @(8, %1)\n\t"
+                  "mov.l r15, @(12, %1)\n\t"
+                  "add #16,%1\n\t"
                   "stc sr, %0\n\t"
-                  "mov.l %0, @(32, %1)\n\t"
+                  "mov.l %0, @(0, %1)\n\t"
                   "stc gbr, %0\n\t"
-                  "mov.l %0, @(36, %1)\n\t"
+                  "mov.l %0, @(4, %1)\n\t"
                   "sts pr, %0\n\t"
-                  "mov.l %0, @(40, %1)" : "=r&" (tmp) : "r" (addr));
+                  "mov.l %0, @(8, %1)" : "=r&" (tmp) : "r" (addr));
 }
 
 /*--------------------------------------------------------------------------- 
@@ -76,15 +78,17 @@ static inline void ldctx(void* addr)
                   "mov.l @(4, %1), r9\n\t"
                   "mov.l @(8, %1), r10\n\t"
                   "mov.l @(12, %1), r11\n\t"
-                  "mov.l @(16, %1), r12\n\t"
-                  "mov.l @(20, %1), r13\n\t"
-                  "mov.l @(24, %1), r14\n\t"
-                  "mov.l @(28, %1), r15\n\t"
-                  "mov.l @(32, %1), r0\n\t"
+                  "add #16,%1\n\t"
+                  "mov.l @(0, %1), r12\n\t"
+                  "mov.l @(4, %1), r13\n\t"
+                  "mov.l @(8, %1), r14\n\t"
+                  "mov.l @(12, %1), r15\n\t"
+                  "add #16,%1\n\t"
+                  "mov.l @(0, %1), r0\n\t"
                   "ldc %0, sr\n\t"
-                  "mov.l @(36, %1), %0\n\t"
+                  "mov.l @(4, %1), %0\n\t"
                   "ldc %0, gbr\n\t"
-                  "mov.l @(40, %1), %0\n\t"
+                  "mov.l @(8, %1), %0\n\t"
                   "lds %0, pr\n\t"
                   "mov.l %0, @(0, r15)" : "=r&" (tmp) : "r" (addr));
 }