Index: trunk/hal/tsar_mips32/core/hal_context.c
===================================================================
--- trunk/hal/tsar_mips32/core/hal_context.c	(revision 311)
+++ trunk/hal/tsar_mips32/core/hal_context.c	(revision 317)
@@ -23,4 +23,5 @@
 
 #include <hal_types.h>
+#include <hal_switch.h>
 #include <memcpy.h>
 #include <thread.h>
@@ -31,5 +32,4 @@
 #include <core.h>
 #include <cluster.h>
-
 #include <hal_context.h>
 
@@ -177,5 +177,5 @@
     hal_cpu_context_t * ctx = (hal_cpu_context_t *)thread->cpu_context;
 
-    printk("\n*** cpu_context for thread %x in cluster %x / ctx = %x ***\n" 
+    printk("\n***** cpu_context for thread %x in cluster %x / ctx = %x\n" 
            " gp_28   = %X    sp_29   = %X    ra_31   = %X\n" 
            " c0_sr   = %X    c0_epc  = %X    c0_th = %X\n"
@@ -185,161 +185,155 @@
            ctx->c0_sr   , ctx->c0_epc  , ctx->c0_th,
            ctx->c2_ptpr , ctx->c2_mode );
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////
-// These registers are saved to CPU context defined by <ctx> argument.
+
+}  // end hal_context_display()
+
+/*
+////////////////////////////////////////////////////////////////////////////////////////
+// This static function makes the actual context switch.    
+////////////////////////////////////////////////////////////////////////////////////////
+static void hal_do_switch( hal_cpu_context_t * ctx_old,
+                           hal_cpu_context_t * ctx_new )
+{
+    asm volatile(
+    ".set noat                       \n"
+    ".set noreorder                  \n"
+    "move    $26,   %0               \n"
+
+    "mfc0    $27,   $14              \n"
+    "sw      $27,   0*4($26)         \n"
+ 
+    "sw      $1,    1*4($26)         \n"
+    "sw      $2,    2*4($26)         \n"
+    "sw      $3,    3*4($26)         \n"
+    "sw      $4,    4*4($26)         \n"
+    "sw      $5,    5*4($26)         \n"
+    "sw      $6,    6*4($26)         \n"
+    "sw      $7,    7*4($26)         \n"
+
+    "sw      $8,    8*4($26)         \n"
+    "sw      $9,    9*4($26)         \n"
+    "sw      $10,  10*4($26)         \n"
+    "sw      $11,  11*4($26)         \n"
+    "sw      $12,  12*4($26)         \n"
+    "sw      $13,  13*4($26)         \n"
+    "sw      $14,  14*4($26)         \n"
+    "sw      $15,  15*4($26)         \n"
+
+    "sw      $16,  16*4($26)         \n"
+    "sw      $17,  17*4($26)         \n"
+    "sw      $18,  18*4($26)         \n"
+    "sw      $19,  19*4($26)         \n"
+    "sw      $20,  20*4($26)         \n"
+    "sw      $21,  21*4($26)         \n"
+    "sw      $22,  22*4($26)         \n"
+    "sw      $23,  23*4($26)         \n"
+
+    "sw      $24,  24*4($26)         \n"
+    "sw      $25,  25*4($26)         \n"
+
+    "mfhi    $27                     \n"
+    "sw      $27,  26*4($26)         \n"
+    "mflo    $27                     \n"
+    "sw      $27,  27*4($26)         \n"
+
+    "sw      $28,  28*4($26)         \n"
+    "sw      $29,  29*4($26)         \n"
+    "sw      $30,  30*4($26)         \n"
+    "sw      $31,  31*4($26)         \n"
+
+	"mfc2    $27,  $0                \n"
+	"sw      $27,  32*4($26)         \n"
+	"mfc2    $27,  $1                \n"
+	"sw      $27,  33*4($26)         \n"
+
+    "mfc0	 $27,  $12               \n"
+	"sw      $27,  34*4($26)         \n"
+    "mfc0	 $27,  $4, 2             \n"
+	"sw      $27,  35*4($26)         \n"
+
+    "sync                            \n"
+
+    "move    $26,   %1               \n"
+
+    "lw      $27,   0*4($26)         \n"
+    "mtc0    $27,   $14              \n"
+
+    "lw      $1,    1*4($26)         \n"
+    "lw      $2,    2*4($26)         \n"
+    "lw      $3,    3*4($26)         \n"
+    "lw      $4,    4*4($26)         \n"
+    "lw      $5,    5*4($26)         \n"
+    "lw      $6,    6*4($26)         \n"
+    "lw      $7,    7*4($26)         \n"
+
+    "lw      $8,    8*4($26)         \n"
+    "lw      $9,    9*4($26)         \n"
+    "lw      $10,  10*4($26)         \n"
+    "lw      $11,  11*4($26)         \n"
+    "lw      $12,  12*4($26)         \n"
+    "lw      $13,  13*4($26)         \n"
+    "lw      $14,  14*4($26)         \n"
+    "lw      $15,  15*4($26)         \n"
+
+	"lw      $16,  16*4($26)         \n"
+	"lw      $17,  17*4($26)         \n"
+    "lw      $18,  18*4($26)         \n"
+    "lw      $19,  19*4($26)         \n"
+    "lw      $20,  20*4($26)         \n"
+    "lw      $21,  21*4($26)         \n"
+    "lw      $22,  22*4($26)         \n"
+    "lw      $23,  23*4($26)         \n"
+
+    "lw      $24,  24*4($26)         \n"
+    "lw      $25,  25*4($26)         \n"
+
+    "lw      $27,  26*4($26)         \n"
+    "mthi    $27                     \n"
+    "lw      $27,  27*4($26)         \n"
+    "mtlo    $27                     \n"
+
+	"lw      $28,  28*4($26)         \n"
+	"lw      $29,  29*4($26)         \n"
+	"lw      $30,  30*4($26)         \n"
+	"lw      $31,  31*4($26)         \n"
+
+	"lw      $27,  32*4($26)         \n"
+	"mtc2    $27,  $0                \n"
+	"lw      $27,  33*4($26)         \n"
+	"mtc2    $27,  $1                \n"
+
+	"lw      $27,  34*4($26)         \n"
+    "mtc0	 $27,  $12               \n"
+	"lw      $27,  35*4($26)         \n"
+    "mtc0	 $27,  $4, 2             \n"
+
+    "jr      $31                     \n"
+
+	".set reorder                    \n"
+    ".set at                         \n"
+    : : "r"(ctx_old) , "r"(ctx_new) : "$26" , "$27" , "memory" ); 
+
+}  // hal_context_switch()
+
+*/
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// These registers are saved/restored to/from CPU context defined by <ctx> argument.
 // - GPR : all, but (zero, k0, k1), plus (hi, lo)
 // - CP0 : c0_th , c0_sr 
 // - CP2 : c2_ptpr , C2_mode, C2_epc
 /////////////////////////////////////////////////////////////////////////////////////////
-void hal_cpu_context_save( void * ctx )
-{
-    asm volatile(
-    ".set noat                     \n"
-    ".set noreorder                \n"
-    "move    $26,   $4             \n"   /* $26 <= &ctx */
-
-    "mfc0    $27,   $14            \n"
-    "sw      $27,   0*4($26)       \n"   /* save c0_epc to slot 0 */
- 
-    "sw      $1,    1*4($26)       \n"
-    "sw      $2,    2*4($26)       \n"
-    "sw      $3,    3*4($26)       \n"
-    "sw      $4,    4*4($26)       \n"
-    "sw      $5,    5*4($26)       \n"
-    "sw      $6,    6*4($26)       \n"
-    "sw      $7,    7*4($26)       \n"
-
-    "sw      $8,    8*4($26)       \n"
-    "sw      $9,    9*4($26)       \n"
-    "sw      $10,  10*4($26)       \n"
-    "sw      $11,  11*4($26)       \n"
-    "sw      $12,  12*4($26)       \n"
-    "sw      $13,  13*4($26)       \n"
-    "sw      $14,  14*4($26)       \n"
-    "sw      $15,  15*4($26)       \n"
-
-    "sw      $16,  16*4($26)       \n"
-    "sw      $17,  17*4($26)       \n"
-    "sw      $18,  18*4($26)       \n"
-    "sw      $19,  19*4($26)       \n"
-    "sw      $20,  20*4($26)       \n"
-    "sw      $21,  21*4($26)       \n"
-    "sw      $22,  22*4($26)       \n"
-    "sw      $23,  23*4($26)       \n"
-
-    "sw      $24,  24*4($26)       \n"
-    "sw      $25,  25*4($26)       \n"
-
-    "mfhi    $27                   \n"
-    "sw      $27,  26*4($26)       \n"   /* save hi to slot 26 */
-    "mflo    $27                   \n"
-    "sw      $27,  27*4($26)       \n"   /* save lo to slot 27 */
-
-    "sw      $28,  28*4($26)       \n"
-    "sw      $29,  29*4($26)       \n"
-    "sw      $30,  30*4($26)       \n"
-    "sw      $31,  31*4($26)       \n"
-
-	"mfc2    $27,  $0              \n"
-	"sw      $27,  32*4($26)       \n"   /* save c2_ptpr to slot 32 */
-	"mfc2    $27,  $1              \n"
-	"sw      $27,  33*4($26)       \n"   /* save c2_mode to slot 33 */
-
-    "mfc0	 $27,  $12             \n"
-	"sw      $27,  34*4($26)       \n"   /* save c0_sr to slot 34 */
-    "mfc0	 $27,  $4, 2           \n"
-	"sw      $27,  35*4($26)       \n"   /* save c0_th to slot 35 */
-
-    "sync                          \n"
-
-    "move    $4,   $27               \n"
-    "jal     hal_cpu_context_display \n"    /* display context */
-    "nop                             \n"
-
-	".set reorder                  \n"
-    ".set at                       \n"
-    : : : "$26" , "$27" , "memory" ); 
-}
-
-/////////////////////////////////////////////////////////////////////////////////////////
-// These registers are restored from cpu context defined by <ctx> argument:
-// - GPR : all, but (zero, k0, k1), plus (hi, lo)
-// - CP0 : c0_th , c0_sr
-// - CP2 : c2_ptpr , C2_mode
-/////////////////////////////////////////////////////////////////////////////////////////
-void hal_cpu_context_restore( void * ctx )
-{
-    asm volatile(
-    ".set noat                     \n"
-    ".set noreorder                \n"
-    "move    $26,   $4             \n"   /* $26 <= &ctx */
-
-    "lw      $4,   35*4($26)         \n"
-    "jal     hal_cpu_context_display \n"    /* display context */ 
-    "nop                             \n"
-
-    "lw      $27,   0*4($26)       \n"
-    "mtc0    $27,   $14            \n"   /* restore C0_epc from slot 0 */
-
-    "lw      $1,    1*4($26)       \n"
-    "lw      $2,    2*4($26)       \n"
-    "lw      $3,    3*4($26)       \n"
-    "lw      $4,    4*4($26)       \n"
-    "lw      $5,    5*4($26)       \n"
-    "lw      $6,    6*4($26)       \n"
-    "lw      $7,    7*4($26)       \n"
-
-    "lw      $8,    8*4($26)       \n"
-    "lw      $9,    9*4($26)       \n"
-    "lw      $10,  10*4($26)       \n"
-    "lw      $11,  11*4($26)       \n"
-    "lw      $12,  12*4($26)       \n"
-    "lw      $13,  13*4($26)       \n"
-    "lw      $14,  14*4($26)       \n"
-    "lw      $15,  15*4($26)       \n"
-
-	"lw      $16,  16*4($26)       \n"
-	"lw      $17,  17*4($26)       \n"
-    "lw      $18,  18*4($26)       \n"
-    "lw      $19,  19*4($26)       \n"
-    "lw      $20,  20*4($26)       \n"
-    "lw      $21,  21*4($26)       \n"
-    "lw      $22,  22*4($26)       \n"
-    "lw      $23,  23*4($26)       \n"
-
-    "lw      $24,  24*4($26)       \n"
-    "lw      $25,  25*4($26)       \n"
-
-    "lw      $27,  26*4($26)       \n"
-    "mthi    $27                   \n"   /* restore hi from slot 26 */
-    "lw      $27,  27*4($26)       \n"
-    "mtlo    $27                   \n"   /* restote lo from slot 27 */
-
-	"lw      $28,  28*4($26)       \n"
-	"lw      $29,  29*4($26)       \n"
-	"lw      $30,  30*4($26)       \n"
-	"lw      $31,  31*4($26)       \n"
-
-	"lw      $27,  32*4($26)       \n"
-	"mtc2    $27,  $0              \n"   /* restore c2_ptpr from slot 32 */
-	"lw      $27,  33*4($26)       \n"
-	"mtc2    $27,  $1              \n"   /* restore c2_mode from slot 33 */
-
-	"lw      $27,  34*4($26)       \n"
-    "mtc0	 $27,  $12             \n"   /* restore c0_sr from slot 34 */
-	"lw      $27,  35*4($26)       \n"
-    "mtc0	 $27,  $4, 2           \n"   /* restore co_th from slot 35 */
-
-    ".set reorder                  \n"
-    ".set at                       \n"
-    : : : "$26" , "$27" ); 
-}
-
-void hal_cpu_context_switch( thread_t * old , thread_t * new )
-{
-    // XXX XXX: TODO
-    hal_cpu_context_save( old->cpu_context );
-    hal_cpu_context_restore( new->cpu_context );
+void hal_cpu_context_switch( thread_t * old,
+                             thread_t * new )
+{
+    hal_cpu_context_t * ctx_old = old->cpu_context;
+    hal_cpu_context_t * ctx_new = new->cpu_context;
+
+    #if CONFIG_CONTEXT_DEBUG
+    hal_cpu_context_display( old );
+    hal_cpu_context_display( new );
+    #endif
+
+    hal_do_switch( ctx_old , ctx_new );
 }
 
@@ -368,4 +362,5 @@
 
     return 0;
+
 }  // end hal_cpu_context_copy()
 
@@ -380,4 +375,8 @@
 
 }  // end hal_cpu_context_destroy()
+
+
+
+
 
 ///////////////////////////////////////////////////
