Index: trunk/hal/tsar_mips32/core/hal_context.c
===================================================================
--- trunk/hal/tsar_mips32/core/hal_context.c	(revision 317)
+++ trunk/hal/tsar_mips32/core/hal_context.c	(revision 406)
@@ -33,4 +33,5 @@
 #include <cluster.h>
 #include <hal_context.h>
+#include <hal_kentry.h>
 
 /////////////////////////////////////////////////////////////////////////////////////////
@@ -38,14 +39,13 @@
 /////////////////////////////////////////////////////////////////////////////////////////
 
-#define SR_USR_MODE       0xFC11
-#define SR_USR_MODE_FPU   0x2000FC11
-#define SR_SYS_MODE       0xFC00
-
-/////////////////////////////////////////////////////////////////////////////////////////
-// These structuree defines the cpu_context anf fpu_context for TSAR MIPS32.
+#define SR_USR_MODE       0x0000FC13
+#define SR_USR_MODE_FPU   0x2000FC13
+#define SR_SYS_MODE       0x0000FC00
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// This structuree defines the cpu_context for TSAR MIPS32.
 // These registers are saved/restored at each context switch.
-// WARNING : update the hal_***_context_save() and hal_***_context_restore()
-//           functions when modifying this structure, and check the two
-//           CONFIG_CPU_CTX_SIZE & CONFIGFPU_CTX_SIZE configuration parameterss. 
+// WARNING : check the two CONFIG_CPU_CTX_SIZE & CONFIG_FPU_CTX_SIZE configuration 
+//           parameterss when modifying this structure.
 /////////////////////////////////////////////////////////////////////////////////////////
 
@@ -80,5 +80,5 @@
 
     uint32_t t8_24;      // slot 24
-    uint32_t t8_25;      // slot 25
+    uint32_t t9_25;      // slot 25
     uint32_t hi_26;      // slot 26
     uint32_t lo_27;      // slot 27
@@ -107,14 +107,13 @@
 
 
-
 /////////////////////////////////////////////////////////////////////////////////////////
 //        CPU context access functions
 /////////////////////////////////////////////////////////////////////////////////////////
 
-
-/////////////////////////////////////////////////////////////////////////////////////////
-// Seven registers are initialised by this function:
-// GPR : sp_29 / fp_30 / ra_31
-// CP0 : c0_sr / c0_th
+/////////////////////////////////////////////////////////////////////////////////////////
+// This function allocates and initializes the cpu_context stucture in thread descriptor.
+// The following context slots are initialised by this function:
+// GPR : a0_04 / sp_29 / fp_30 / ra_31
+// CP0 : c0_sr / c0_th / c0_epc
 // CP2 : c2_ptpr / c2_mode
 /////////////////////////////////////////////////////////////////////////////////////////
@@ -123,10 +122,12 @@
     kmem_req_t  req;
 
-    context_dmsg("\n[INFO] %s : enters for thread %x in process %x\n",
+    assert( (sizeof(hal_cpu_context_t) <= CONFIG_CPU_CTX_SIZE) , __FUNCTION__ ,
+    "inconsistent CPU context size" );
+
+    context_dmsg("\n[DMSG] %s : enters for thread %x in process %x\n",
                  __FUNCTION__ , thread->trdid , thread->process->pid );
 
     // allocate memory for cpu_context
     req.type   = KMEM_CPU_CTX;
-    req.size   = sizeof(hal_cpu_context_t);
     req.flags  = AF_KERNEL | AF_ZERO;
 
@@ -158,7 +159,9 @@
 
     // initialise context
+    context->a0_04      = (uint32_t)thread->entry_args;
 	context->sp_29      = sp_29; 
-	context->fp_30      = sp_29;                          // TODO check this [AG]
-    context->ra_31      = (uint32_t)thread->entry_func;
+	context->fp_30      = sp_29;                               // TODO check this [AG]
+    context->ra_31      = (uint32_t)&hal_kentry_eret;
+    context->c0_epc     = (uint32_t)thread->entry_func;
 	context->c0_sr      = c0_sr;
 	context->c0_th      = (uint32_t)thread; 
@@ -166,8 +169,20 @@
 	context->c2_mode    = c2_mode;
 
-    context_dmsg("\n[INFO] %s : exit for thread %x in process %x / ra = %x\n",
-                 __FUNCTION__ , thread->trdid , thread->process->pid , context->ra_31 );
-
+    context_dmsg("\n[DMSG] %s : exit for thread %x in process %x\n"
+                 " - a0   = %x\n"
+                 " - sp   = %x\n"
+                 " - fp   = %x\n"
+                 " - ra   = %x\n"
+                 " - sr   = %x\n"
+                 " - th   = %x\n"   
+                 " - epc  = %x\n"   
+                 " - ptpr = %x\n"   
+                 " - mode = %x\n",  
+                 __FUNCTION__ , thread->trdid , thread->process->pid,
+                 context->a0_04, context->sp_29, context->fp_30, context->ra_31,
+                 context->c0_sr, context->c0_th, context->c0_epc,
+                 context->c2_ptpr, context->c2_mode );
     return 0;
+
 }  // end hal_cpu_context_create()
 
@@ -177,9 +192,9 @@
     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 process %x / cycle %d\n" 
            " gp_28   = %X    sp_29   = %X    ra_31   = %X\n" 
            " c0_sr   = %X    c0_epc  = %X    c0_th = %X\n"
            " c2_ptpr = %X    c2_mode = %X\n",
-           thread->trdid, local_cxy, ctx,
+           thread->trdid, thread->process->pid, hal_time_stamp(),
            ctx->gp_28   , ctx->sp_29   , ctx->ra_31,
            ctx->c0_sr   , ctx->c0_epc  , ctx->c0_th,
@@ -188,150 +203,26 @@
 }  // 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_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;
+// - CP0 : c0_th , c0_sr , C0_epc
+// - CP2 : c2_ptpr , C2_mode
+/////////////////////////////////////////////////////////////////////////////////////////
+// old_thread  : pointer on current thread descriptor
+// new_thread  : pointer on new thread descriptor
+/////////////////////////////////////////////////////////////////////////////////////////
+void hal_cpu_context_switch( thread_t * old_thread,
+                             thread_t * new_thread )
+{
+    hal_cpu_context_t * ctx_old = old_thread->cpu_context;
+    hal_cpu_context_t * ctx_new = new_thread->cpu_context;
 
     #if CONFIG_CONTEXT_DEBUG
-    hal_cpu_context_display( old );
-    hal_cpu_context_display( new );
+    hal_cpu_context_display( old_thread );
+    hal_cpu_context_display( new_thread );
     #endif
+
+    // reset loadable field in new thread descriptor 
+    new_thread->flags &= ~THREAD_FLAG_LOADABLE;
 
     hal_do_switch( ctx_old , ctx_new );
@@ -377,7 +268,4 @@
 
 
-
-
-
 ///////////////////////////////////////////////////
 error_t hal_fpu_context_create( thread_t * thread )
@@ -385,7 +273,9 @@
     kmem_req_t  req;
 
+    assert( (sizeof(hal_fpu_context_t) <= CONFIG_FPU_CTX_SIZE) , __FUNCTION__ ,
+    "inconsistent FPU context size" );
+
     // allocate memory for uzone
     req.type   = KMEM_FPU_CTX;
-    req.size   = sizeof(hal_fpu_context_t);
     req.flags  = AF_KERNEL | AF_ZERO;
 
@@ -407,5 +297,4 @@
     // allocate memory for dst fpu_context
     req.type   = KMEM_FPU_CTX;
-    req.size   = sizeof(hal_fpu_context_t);
     req.flags  = AF_KERNEL | AF_ZERO;
 
@@ -435,57 +324,4 @@
 
 }  // end hal_fpu_context_destroy()
-
-/////////////////////////////////////////////////////////////////////////////////////////
-// These registers are initialised:
-// - GPR : sp_29 , fp_30 , a0
-// - CP0 : c0_sr , c0_epc , c0_th
-// - CP2 : C2_ptpr , c2_mode
-// TODO Quand cette fonction est-elle appelée? [AG]
-/////////////////////////////////////////////////////////////////////////////////////////
-void hal_cpu_context_load( thread_t * thread )
-{
-    // get relevant values from thread context
-    hal_cpu_context_t * ctx     = (hal_cpu_context_t *)thread->cpu_context;     
-    uint32_t            sp_29   = ctx->sp_29;
-    uint32_t            fp_30   = ctx->fp_30;
-    uint32_t            c0_th   = ctx->c0_th;
-    uint32_t            c0_sr   = ctx->c0_sr;
-    uint32_t            c2_ptpr = ctx->c2_ptpr;
-    uint32_t            c2_mode = ctx->c2_mode;
- 
-    // get pointer on entry function & argument from thread attributes
-    uint32_t            func    = (uint32_t)thread->entry_func;
-    uint32_t            args    = (uint32_t)thread->entry_args;
-
-    // reset loadable field in thread descriptor 
-    thread->flags &= ~THREAD_FLAG_LOADABLE;
-
-    // load registers
-    asm volatile(
-    ".set noreorder                \n"
-	"or       $26,    %0,    $0    \n"   /* $26 <= stack pointer                */
-	"or       $27,    %2,    $0    \n"   /* $27 <= status register              */
-	"addiu    $26,    $26,  -4     \n"   /* decrement stack pointer             */
-	"or       $4,     %7,   $0     \n"   /* load a0                             */
-	"sw       $4,     ($26)        \n"   /* set entry_args in stack             */
-	"ori      $27,    $27,  0x2    \n"   /* set EXL flag in status register     */
-	"mtc0     $27,    $12          \n"   /* load c0_sr                          */
-	"mtc0     %3,     $4,    2     \n"   /* load c0_th                          */
-	"mtc2     %4,     $0           \n"   /* load c2 ptpr                        */
-	"mtc0     %6,     $14          \n"   /* load c0_epc                         */
-	"or	      $29,	  $16,  $0     \n"   /* load sp_29                          */
-	"or	      $30,	  %1,   $0     \n"   /* load fp_30                          */
-    "mtc2     %5,     $1           \n"   /* load c2_mode                        */
-    "nop                           \n"
-    "eret                          \n"   /* jump to user code                   */
-    "nop                           \n"
-    ".set reorder                  \n"
-    : 
-    : "r"(sp_29),"r"(fp_30),"r"(c0_sr),"r"(c0_th),
-      "r"(c2_ptpr),"r"(c2_mode),"r"(func),"r"(args)
-    : "$4","$26","$27","$29","$30" );
-
-}  // end hal_cpu_context_load()
-
 
 //////////////////////////////////////////////
