Index: trunk/hal/tsar_mips32/core/hal_switch.S
===================================================================
--- trunk/hal/tsar_mips32/core/hal_switch.S	(revision 406)
+++ trunk/hal/tsar_mips32/core/hal_switch.S	(revision 407)
@@ -22,21 +22,18 @@
  */
 
-#---------------------------------------------------------------------------------
-# This code makes the following assumptions:
+    .section   .switch , "ax" , @progbits
+
+    .global  hal_do_cpu_switch
+    .global  hal_do_cpu_save
+
+    .set     noat 
+    .set     noreorder 
+
+#---------------------------------------------------------------------------------
+# The hal_do_cpu_switch() function makes the following assumptions:
 # - register $4 contains a pointer on the old thread context.
 # - register $5 contains a pointer on the new thread context.
-# When the switch is completed, it jumps to address contained in register $31
-# of the new context.
-#---------------------------------------------------------------------------------
-
-    .section   .switch , "ax" , @progbits
-
-    .ent     hal_do_switch
-    .global  hal_do_switch
-
-    .set     noat 
-    .set     noreorder 
-
-hal_do_switch:
+#---------------------------------------------------------------------------------
+hal_do_cpu_switch:
 
     move    $26,   $4                  /* $26 <= ctx_old */
@@ -133,19 +130,19 @@
     mthi    $27                        /* restore hi from slot 26 */
     lw      $27,  27*4($26)         
-    mtlo    $27                        /* restote lo from slot 27 */
+    mtlo    $27                        /* restore lo from slot 27 */
 
 	lw      $28,  28*4($26)         
 	lw      $29,  29*4($26)         
 	lw      $30,  30*4($26)         
-	lw      $31,  31*4($26)         
-
-	lw      $27,  32*4($26)            /* $27 <= c2_ptpr */
+	lw      $31,  31*4($26)            /* restore ra from slot 31 */
+
+	lw      $27,  32*4($26)
 	mtc2    $27,  $0                   /* restore c2_ptpr from slot 32 */
 
-	lw      $27,  35*4($26)            /* $27 <= c0_th */      
+	lw      $27,  35*4($26)      
     mtc0	$27,  $4, 2                /* restore c0_th from slot 35 */
 
-	lw      $27,  33*4($26)            /* $27 <= c2_mode */
-	lw      $26,  34*4($26)            /* $26 <= c0_sr */
+	lw      $27,  33*4($26)
+	lw      $26,  34*4($26)
 
 	mtc2    $27,  $1                   /* restore c2_mode from slot 33 */
@@ -155,7 +152,78 @@
     nop
 
+#---------------------------------------------------------------------------------
+# The hal_do_cpu_save()function makes the following assumptions:
+# - register $4 contains a pointer on the target thread context.
+# - register $5 contains the target thread descriptor pointer.
+# - register $6 contains the offset to add to stack pointer.
+#---------------------------------------------------------------------------------
+hal_do_cpu_save:
+
+    move    $26,  $4                  /* $26 <= context */
+
+    move    $27,  $5
+	sw      $27,  35*4($26)           /* save child thread to slot 35 */
+
+    add     $27,  $6,  $29
+    sw      $27,  29*4($26)           /* save (sp_29 + offset) to slot 29 */
+
+    mfc0    $27,  $12               
+	sw      $27,  34*4($26)           /* save c0_sr to slot 34 */
+
+	mfc2    $27,  $0                
+	sw      $27,  32*4($26)           /* save c2_ptpr to slot 32 */
+
+	mfc2    $27,  $1                
+	sw      $27,  33*4($26)           /* save c2_mode to slot 33 */
+
+    mfc0    $27,   $14              
+    sw      $27,   0*4($26)           /* save c0_epc to slot 0 */
+ 
+    sw      $1,    1*4($26)         
+    sw      $2,    2*4($26)         
+    sw      $3,    3*4($26)         
+    sw      $4,    4*4($26)         
+    sw      $5,    5*4($26)         
+    sw      $6,    6*4($26)         
+    sw      $7,    7*4($26)         
+
+    sw      $8,    8*4($26)         
+    sw      $9,    9*4($26)         
+    sw      $10,  10*4($26)         
+    sw      $11,  11*4($26)         
+    sw      $12,  12*4($26)         
+    sw      $13,  13*4($26)         
+    sw      $14,  14*4($26)         
+    sw      $15,  15*4($26)         
+
+    sw      $16,  16*4($26)         
+    sw      $17,  17*4($26)         
+    sw      $18,  18*4($26)         
+    sw      $19,  19*4($26)         
+    sw      $20,  20*4($26)         
+    sw      $21,  21*4($26)         
+    sw      $22,  22*4($26)         
+    sw      $23,  23*4($26)         
+
+    sw      $24,  24*4($26)         
+    sw      $25,  25*4($26)         
+
+    mfhi    $27                     
+    sw      $27,  26*4($26)           /* save hi to slot 26 */
+
+    mflo    $27                     
+    sw      $27,  27*4($26)           /* save lo to slot 27 */
+
+    sw      $28,  28*4($26)           /* save gp to slot 28 */
+
+    sw      $30,  30*4($26)           /* save s8 to slot 30 */
+    sw      $31,  31*4($26)           /* save ra to slot 31 */
+
+    sync
+
+    jr      $31                       /* return to caller */
+    nop
+
 	.set reorder                    
     .set at                         
 
-    .end hal_do_switch
-
