Index: trunk/hal/tsar_mips32/core/hal_kentry.S
===================================================================
--- trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 625)
+++ trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 654)
@@ -24,5 +24,5 @@
  */
 
-#define      UZ_MODE         0                 
+#define      UZ_MODE_DEXT    0              
 #define      UZ_AT           1
 #define      UZ_V0           2
@@ -86,21 +86,45 @@
 
 #------------------------------------------------------------------------------------
-# Kernel Entry point for Interrupt / Exception / Syscall
-# The c2_dext CP2 register must have been previously set
-# to "local_cxy", because the kernel run with data MMU desactivated.
+#       Kernel Entry point for Interrupt / Exception / Syscall
+#  At kernel entry, the relevant CPU, CP0, & CP2 registers are saved
+#  in a "UZONE" implemented in  the calling thread kernel stack.
+#  All these registers are restored from the UZONE after the kernel
+#  completes the requested service.
+#
+#  Implementation note: As this code can be executed when the core is already
+#  in kernel mode (after a syscall, the core must handle interrupts, or 
+#  non-fatal MMU exceptions), this code implement a two slots stack of UZONE(s).
 #------------------------------------------------------------------------------------
 
 hal_kentry_enter:
 
-	mfc0    $26,    $12                 # get c0_sr 
-	andi    $26,    $26,  0x10          # test User Mode bit
-	beq     $26,    $0,	  kernel_mode   # jump if core already in kernel
-	ori     $27,    $0,	  0xB           # $27 <= code data MMU OFF
+#------------------------------------------------------------------------------------
+# This code made the following actions:
+# - save CP2_MMU_MODE & CP2_MMU_DEXT in $26, 
+# - set local_cxy into CP2_MMU_DEXT,
+# - desactivate DATA MMU in CP2_MMU_MODE,
+# - test if the core is already in kernel mode.
+
+    mfc2    $26,    $1                  # $26 <= CP2_MMU_MODE
+	andi    $27,    $26,  0xB           # $27 <= code data MMU OFF
+	mtc2    $27,    $1			        # set data MMU OFF
+
+    mfc2    $27,    $24                 # $27 <= CP2_MMU_DEXT
+    sll     $27,    $27,  4             # $27 <= CP2_MMU_DEXT << 4
+    or      $26,    $26,  $27           # $26 <= CP2_MMU_DEXT | CP2_MMU_MODE
+
+    mfc0    $27,    $15,  1             # $27 <= core CP0_EBASE         
+    andi    $27,    $27,  0xFFF         # $27 <= gid
+    srl     $27,    $27,  2             # $27 <= local_cxy
+    mtc2    $27,    $24                 # CP2_MMU_DEXT <= local_cxy
+
+	mfc0    $27,    $12                 # $27 <= CP0_SR 
+	andi    $27,    $27,  0x10          # test User Mode bit
+	beq     $27,    $0,	  kernel_mode   # jump if core already in kernel
+    nop
 	
 #------------------------------------------------------------------------------------
 # This code is executed when the core is in user mode,
-# to handle a syscall, an interrupt, or an user exception.
-# - save current c2_mode in $26.
-# - set data MMU OFF.
+# to handle a syscall, an interrupt, or an exception.
 # - copy user stack pointer in $27 to be saved in uzone.
 # - set kernel stack pointer in $29 (kernel stack empty at firts entry).
@@ -108,6 +132,4 @@
 user_mode:
 
-    mfc2    $26,    $1                  # $26 <= c2_mode
-	mtc2    $27,    $1			        # set data MMU OFF
     move    $27,    $29                 # $27 <= user stack pointer
 	mfc0    $29,    $4,   2             # get pointer on thread descriptor from c0_th
@@ -119,31 +141,30 @@
 #------------------------------------------------------------------------------------
 # This code is executed when the core is already in kernel mode,
-# after a syscall, to handle an interrupt, or to handle a non-fatal exception.
-# - save current c2_mode in $26.
-# - set data MMU OFF.
+# (after a syscall), to handle an interrupt, or a non-fatal exception.
 # - copy current kernel stack pointer in $27.
 
 kernel_mode:
 
-    mfc2    $26,    $1                  # $26 <= c2_mode
-	mtc2    $27,    $1			        # set data MMU OFF
     move    $27,    $29                 # $27 <= current kernel stack pointer
+    j       unified_mode
+    nop
 
 #------------------------------------------------------------------------------------	
-# This code is executed in both modes (user or kernel):
-# The assumptions are:
-# - c2_mode contains the data MMU OFF value.
-# - $26 contains the previous c2_mode value.
-# - $27 contains the previous sp value (can be usp or ksp).
-# - $29 contains the curren kernel stack pointer.
-# We execute the following actions:
+# This code is executed in both modes (user or kernel).
+# It executes the following actions:
 # - decrement $29 to allocate an uzone in kernel stack
 # - save GPR, CP0 and CP2 registers to uzone.
 # - set the SR in kernel mode: IRQ disabled, clear EXL.
+# The assumptions are:
+# - c2_mode contains the data MMU OFF value.
+# - $26 contains the previous c2_mode and c2_dext values.
+# - $27 contains the previous sp value (can be usp or ksp).
+# - $29 contains the current kernel stack pointer.
 
 unified_mode:
 
-	addiu   $29,    $29,  -(UZ_REGS*4)	# allocate uzone in kernel stack 
-
+	addiu   $29,    $29,  -(UZ_REGS*4)	   # allocate uzone in kernel stack 
+
+    sw      $26,    (UZ_MODE_DEXT*4)($29)  # save previous c2_mode and c2_dext values
 	sw      $1,	    (UZ_AT*4)($29)
 	sw      $2,     (UZ_V0*4)($29)
@@ -173,30 +194,29 @@
 
 	mflo	$1
-	sw      $1,     (UZ_LO*4)($29)      # save lo
+	sw      $1,     (UZ_LO*4)($29)         # save lo
 	mflo	$1
-	sw      $1, 	(UZ_HI*4)($29)	    # save hi
-
-	sw	    $28,	(UZ_GP*4)($29)      # save gp
-	sw	    $27,	(UZ_SP*4)($29)      # save previous sp (can be usp or ksp)
-	sw	    $30,	(UZ_S8*4)($29)      # save s8
-	sw	    $31,	(UZ_RA*4)($29)      # save ra
+	sw      $1, 	(UZ_HI*4)($29)	       # save hi
+
+	sw	    $28,	(UZ_GP*4)($29)         # save gp
+	sw	    $27,	(UZ_SP*4)($29)         # save previous sp (can be usp or ksp)
+	sw	    $30,	(UZ_S8*4)($29)         # save s8
+	sw	    $31,	(UZ_RA*4)($29)         # save ra
 
 	mfc0	$1,     $14
-	sw      $1,     (UZ_EPC*4)($29)	    # save c0_epc
+	sw      $1,     (UZ_EPC*4)($29)	       # save c0_epc
 	mfc0	$1,	    $12 
-	sw	    $1,	    (UZ_SR*4)($29)		# save c0_sr
+	sw	    $1,	    (UZ_SR*4)($29)		   # save c0_sr
 	mfc0    $1,     $4,  2
-	sw      $1,	    (UZ_TH*4)($29)		# save c0_th
+	sw      $1,	    (UZ_TH*4)($29)		   # save c0_th
 	mfc0    $1,     $13   
-	sw      $1,	    (UZ_CR*4)($29)		# save c0_cr
+	sw      $1,	    (UZ_CR*4)($29)		   # save c0_cr
 	mfc2    $1,     $0
-	sw      $1, 	(UZ_PTPR*4)($29)	# save c2_ptpr
-
-    sw      $26,    (UZ_MODE*4)($29)    # save previous c2_mode (can be user or kernel)
-
-    mfc0    $3,     $12                 # $3 <= c0_sr
+	sw      $1, 	(UZ_PTPR*4)($29)	   # save c2_ptpr
+
+
+    mfc0    $3,     $12                    # $3 <= c0_sr
 	srl	    $3,     $3,   5
-	sll     $3,	    $3,   5	            # reset 5 LSB bits
-	mtc0	$3,	    $12			        # set new c0_sr 
+	sll     $3,	    $3,   5	               # reset 5 LSB bits
+	mtc0	$3,	    $12			           # set new c0_sr 
 
 #--------------------
@@ -271,9 +291,9 @@
     jal     puts
     nop    
-    # display saved MODE value
+    # display saved MODE & DEXT values
     la      $4,     msg_mode
     jal     puts
     nop
-    lw	    $4,  	(UZ_MODE*4)($29)
+    lw	    $4,  	(UZ_MODE_DEXT*4)($29)
     jal     putx
     nop
@@ -430,9 +450,9 @@
     jal     puts
     nop    
-    # display saved MODE value
+    # display saved MODE_DEXT value
     la      $4,     msg_mode
     jal     puts
     nop
-    lw	    $4,  	(UZ_MODE*4)($29)
+    lw	    $4,  	(UZ_MODE_DEXT*4)($29)
     jal     putx
     nop
@@ -498,6 +518,9 @@
 	lw	    $31,	(UZ_RA*4)($27)      # restore ra_31 from uzone
 
-	lw	    $26,    (UZ_MODE*4)($27)    
-    mtc2    $26,    $1                  # restore c2_mode from uzone
+	lw	    $26,    (UZ_MODE_DEXT*4)($27)    
+    srl     $27,    $26,  4             # $27 <= CP2_DEXT
+    mtc2    $27,    $24                 # restore c2_dest from uzone
+    andi    $27,    $26,  0xF           # $27 <= CP2_MODE
+    mtc2    $27,    $1                  # restore c2_mode from uzone
 
 # -----------------------------------------------------------------------------------
