Index: trunk/hal/tsar_mips32/core/hal_kentry.S
===================================================================
--- trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 296)
+++ trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 406)
@@ -24,21 +24,43 @@
  */
 
-#include <mips32_uzone.h>
-
-#---------------------------------------------------------------------------------
-# This code is the unique kernel entry point in case of exception, interrupt,
-# or syscall for the TSAR_MIPS32 architecture.  
-#
-# When we enter the kernel, we test the status register:
-# - If the core is in user mode, we desactivate the MMU, and we save
-#   the core context in the uzone of the calling thread descriptor.
-# - If the core is already in kernel mode (in case of interrupt), 
-#   we save the context in the kernel stack.
-# - In both cases, we increment the cores_in_kernel variable,
-#   and we call the relevant exception/interrupt/syscall handler
-#
-# When we exit the kernel after handler execution:
-# - we restore the core context from the uzone
-#---------------------------------------------------------------------------------
+#define      UZ_MODE         0                 
+#define      UZ_AT           1
+#define      UZ_V0           2
+#define      UZ_V1           3
+#define      UZ_A0           4 
+#define      UZ_A1           5
+#define      UZ_A2           6
+#define      UZ_A3           7
+#define      UZ_T0           8
+#define      UZ_T1           9
+#define      UZ_T2           10
+#define      UZ_T3           11
+#define      UZ_T4           12
+#define      UZ_T5           13
+#define      UZ_T6           14
+#define      UZ_T7           15
+#define      UZ_T8           16
+#define      UZ_T9           17
+#define      UZ_S0           18
+#define      UZ_S1           19
+#define      UZ_S2           20
+#define      UZ_S3           21
+#define      UZ_S4           22
+#define      UZ_S5           23
+#define      UZ_S6           24
+#define      UZ_S7           25
+#define      UZ_S8           26
+#define      UZ_GP           27
+#define      UZ_RA           28
+#define      UZ_EPC          29
+#define      UZ_CR           30
+#define      UZ_SP           31
+#define      UZ_SR           32
+#define      UZ_LO           33
+#define      UZ_HI           34
+
+#define      UZ_REGS         35
+
+#include <kernel_config.h>
 
 	.section   .kentry, "ax", @progbits
@@ -48,159 +70,134 @@
 	.extern    hal_do_syscall
 	.extern    cluster_core_kernel_enter
-	.extern    cluster_core_kernel_exit
+	.extern    cluster_core_kentry_exit
 
 	.org       0x180
-	.ent       kernel_enter
-	.global    kernel_enter
+
+	.global    hal_kentry_enter
+    .global    hal_kentry_eret
 
 	.set       noat
 	.set       noreorder
 
-#define SAVE_SIZE      CPU_REGS_NR*4
-#define LID_WIDTH      2
-#define CXY_WIDTH      8
-#define CXY_MASK       0xFF
-#define MMU_MODE_MASK  0xF
-#define GID_MASK       0x3FF	
-#define LID_MASK       0x3	
-
 #---------------------------------------------------------------------------------
 # Kernel Entry point for Interrupt / Exception / Syscall
+# The c2_dext and c2_iext CP2 registers must have been previously set
+# to "local_cxy", because the kernel run with MMU desactivated.
 #---------------------------------------------------------------------------------
 
-kernel_enter:
-	mfc0    $26,    $12                 # read SR to test user/kernel mode
-	andi    $26,    $26,  0x10          # User Mode bitmask
-	beq     $26,    $0,	  kernel_mode
-	ori     $26,    $0,	  0x3		    # $26 <= MMU OFF value
+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,	  0x3           # $27 <= code for MMU OFF
 	
 #---------------------------------------------------------------------------------------	
-# this code is executed when the core is in user mode:
-# - we use the uzone defined in user thread descriptor. 
-# - we set the MMU off, and save the CP2_MODE register to uzone.
-# - we save the user thread stack pointer to uzone and load the kernel stack pointer
-# - we store the uzone pointer in $27
+# This code is executed when the core is in user mode:
+# - save current c2_mode in $26.
+# - set MMU OFF.
+# - save user stack pointer in $27.
+# - set kernel stack pointer in $29.
 
 user_mode:
-	mtc2    $26,    $1			        # set MMU OFF
-	nop
-
-	mfc0    $26,    $4,   2             # $26 <= thread pointer 
-	lw      $26,    0($26)              # $26 <= uzone pointer
-
-	sw      $29,    (UZ_SP*4)($26) 	    # save user stack to uzone
-	lw      $29,    (UZ_KSP*4)($26)     # load kernel stack from uzone
-
-	ori     $27,    $0,	  0xF	        # MMU old value: assumed ON
-	sw      $27,    (UZ_MODE*4)($26)    # save MMU MODE to uzone
-
-	j	    unified_mode
-	or      $27,    $0,	  $26           # $27 <= uzone
+
+    mfc2    $26,    $1                  # $26 <= c2_mode
+	mtc2    $27,    $1			        # set MMU OFF
+    move    $27,    $29                 # $27 <= user stack pointer
+	mfc0    $29,    $4,   2             # get pointer on thread descriptor from c0_th
+    addi    $29,    $29,    CONFIG_THREAD_DESC_SIZE
+    addi    $29,    $29,    -8          # $29 <= kernel stack pointer
+    j       unified_mode
+    nop
 
 #---------------------------------------------------------------------------------------	
-# this code is executed when the core is in kernel mode:
-# - we use an uzone dynamically allocated in kernel stack.
-# - we set the MMU off, set the MMU data_paddr extension to local_cxy,
-#   and save the CP2_MODE and CP2_DEXT to uzone.
-# - we save the kernel stack pointer to uzone and load the new kernel stack pointer
-# - we store the uzone pointer in $27
+# This code is executed when the core is already in kernel mode:
+# - save current c2_mode in $26.
+# - set MMU OFF.
+# - save current kernel stack pointer in $27.
 
 kernel_mode:
-	mfc2	$26,    $24
-	andi	$26,    $26,  CXY_MASK      # $26 <= CP2_DEXT 
-
-	mfc0    $27,    $15,  1
-	andi	$27,	$27,  GID_MASK	    # $27 <= core_gid (4/4/2 format)	
-
-	srl	    $27,    $27,  LID_WIDTH	    # $27 <= local_cxy
-	mtc2	$27,    $24			        # set local_cxy to CP2_DEXT
-
-    # use $26 to save both CP2_MODE (4 bits) and CP2_DEXT (8 bits) values
-
-	mfc2    $27,    $1
-	andi	$27,	$27,  MMU_MODE_MASK	# $27 <= CP2_MODE
-	sll	    $27,    $27,  CXY_WIDTH	    # $27 <= 0x00000M00
-	or	    $26,    $26,  $27           # $26 <= 0x00000MXY 
-
-	ori     $27,    $0,	  0x3
+
+    mfc2    $26,    $1                  # $26 <= c2_mode
 	mtc2    $27,    $1			        # set MMU OFF
-
-	# save old SP, CP2_MODE and CP2_DEXT in uzone allocated in kernel stack
-
-	addiu   $27,    $29,  -(SAVE_SIZE)	# allocate an uzone in stack (use $27 as KSP)
-	sw      $29,	(UZ_SP*4)($27)	    # save old KSP in this uzone
-
-	srl     $29,    $26,  CXY_WIDTH 
-	sw      $29,    (UZ_MODE*4)($27)    # save CP2_MODE in this uzone
-
-	andi	$26,    $26,  CXY_MASK	
-	sw      $26,    (UZ_DEXT*4)($27)    # save CP2_DEXT in this uzone 
-
-	or      $29,    $27,	$0          # load new kernel stack pointer
-
-#--------------------------------------------------------------------------------------	
-# This code is executed in both modes, and saves the core context,
-# with the two following assumptions:
-# - $27 contains the pointer on uzone to save the core registers
-# - $29 contains the kernel stack pointer
-
-unified_mode:	
-	sw      $1,	    (UZ_AT*4)($27)
-	sw      $2,     (UZ_V0*4)($27)
-	sw      $3,     (UZ_V1*4)($27)
-	sw      $4,     (UZ_A0*4)($27)
-	sw      $5,     (UZ_A1*4)($27)
-	sw      $6,     (UZ_A2*4)($27)
-	sw      $7,     (UZ_A3*4)($27)
-	sw      $8,     (UZ_T0*4)($27)
-	sw      $9,     (UZ_T1*4)($27)
-	sw      $10,	(UZ_T2*4)($27)
-	sw      $11,	(UZ_T3*4)($27)
-	sw      $12,	(UZ_T4*4)($27)
-	sw      $13,	(UZ_T5*4)($27)
-	sw      $14,	(UZ_T6*4)($27)
-	sw      $15,	(UZ_T7*4)($27)
-	sw      $16,	(UZ_S0*4)($27)
-	sw      $17,	(UZ_S1*4)($27)
-	sw	    $18,	(UZ_S2*4)($27)
-	sw	    $19,	(UZ_S3*4)($27)
-	sw	    $20,	(UZ_S4*4)($27)
-	sw	    $21,	(UZ_S5*4)($27)
-	sw	    $22,	(UZ_S6*4)($27)
-	sw	    $23,	(UZ_S7*4)($27)
-	sw      $24,	(UZ_T8*4)($27)
-	sw      $25,	(UZ_T9*4)($27)
-	sw	    $28,	(UZ_GP*4)($27)
-	sw	    $30,	(UZ_S8*4)($27)
-	sw	    $31,	(UZ_RA*4)($27)
+    move    $27,    $29                 # $27 <= current kernel stack pointer
+
+#---------------------------------------------------------------------------------------	
+# This code is executed in both modes (user or kernel):
+# The assumptions are:
+# - c2_mode contains the MMU OFF value.
+# - $26 contains the previous c2_mode value.
+# - $27 contains the previous sp value (can be usp or ksp).
+# - $29 contains the kernel stack pointer.
+# We execute the following actions:
+# - allocate an uzone in kernel stack, incrementing $29
+# - save relevant registers to uzone.
+# - set the SR in kernel mode: IRQ disabled, clear exl.
+# - signal the kernel entry.
+
+unified_mode:
+
+	addiu   $29,    $29,  -(UZ_REGS*4)	# allocate uzone in kernel stack 
+
+	sw      $1,	    (UZ_AT*4)($29)
+	sw      $2,     (UZ_V0*4)($29)
+	sw      $3,     (UZ_V1*4)($29)
+	sw      $4,     (UZ_A0*4)($29)
+	sw      $5,     (UZ_A1*4)($29)
+	sw      $6,     (UZ_A2*4)($29)
+	sw      $7,     (UZ_A3*4)($29)
+	sw      $8,     (UZ_T0*4)($29)
+	sw      $9,     (UZ_T1*4)($29)
+	sw      $10,	(UZ_T2*4)($29)
+	sw      $11,	(UZ_T3*4)($29)
+	sw      $12,	(UZ_T4*4)($29)
+	sw      $13,	(UZ_T5*4)($29)
+	sw      $14,	(UZ_T6*4)($29)
+	sw      $15,	(UZ_T7*4)($29)
+	sw      $16,	(UZ_S0*4)($29)
+	sw      $17,	(UZ_S1*4)($29)
+	sw	    $18,	(UZ_S2*4)($29)
+	sw	    $19,	(UZ_S3*4)($29)
+	sw	    $20,	(UZ_S4*4)($29)
+	sw	    $21,	(UZ_S5*4)($29)
+	sw	    $22,	(UZ_S6*4)($29)
+	sw	    $23,	(UZ_S7*4)($29)
+	sw      $24,	(UZ_T8*4)($29)
+	sw      $25,	(UZ_T9*4)($29)
+
+	sw      $26,    (UZ_MODE*4)($29)    # save c2_mode
+	sw      $27,	(UZ_SP*4)($29)	    # save sp
+
+	sw	    $28,	(UZ_GP*4)($29)
+	sw	    $30,	(UZ_S8*4)($29)
+	sw	    $31,	(UZ_RA*4)($29)
 
 	mfc0	$16,    $14
-	sw      $16,	(UZ_EPC*4)($27)	    # Save EPC
+	sw      $16,	(UZ_EPC*4)($29)	    # save c0_epc
 	mflo	$14
-	sw      $14,	(UZ_LO*4)($27)      # save LO
+	sw      $14,	(UZ_LO*4)($29)      # save lo
 	mfhi	$15 
-	sw      $15,	(UZ_HI*4)($27)		# save HI
+	sw      $15,	(UZ_HI*4)($29)		# save hi
 	mfc0	$18,	$12 
-	sw	    $18,	(UZ_SR*4)($27)		# Save SR
+	sw	    $18,	(UZ_SR*4)($29)		# save c0_sr
 	mfc0    $17,    $13
-	sw      $17,	(UZ_CR*4)($27)		# Save CR
-
-    # put SR in kernel mode, IRQ disabled, clear exl
+	sw      $17,	(UZ_CR*4)($29)		# save c0_cr
+	mfc2    $26,    $1
+
 	srl	    $3,     $18,  5
 	sll     $3,	    $3,   5	
-	mtc0	$3,	    $12			        # Set new SR
+	mtc0	$3,	    $12			        # set new sr
 
 	# signal that core enters kernel
 	la      $1,     cluster_core_kernel_enter
-	jal	    $1
+	jalr    $1
     nop
 
 #---------------------------------------------------------------------------------------
 # This code call the relevant Interrupt / Exception / Syscall handler,
-# depending on XCODE in CP0_CR, with the two following assumptions:
-# - $27 contains the pointer on uzone containing to save the core registers
-# - $29 contains the kernel stack pointer
+# depending on XCODE in CP0_CR.
+# assumption: $29 contains the kernel stack pointer, that is the uzone base.
 # The three handlers take the same two arguments: thread pointer and uzone pointer.
-# The uzone pointer is saved in $19 to be used by kernel_exit.
+# The uzone pointer is saved in $19 to be used by kentry_exit.
 
 	mfc0    $17,    $13                 # $17 <= CR
@@ -208,6 +205,6 @@
 
 	mfc0    $4,     $4,   2             # $4 <= thread pointer (first arg)
-	or	    $5,	    $0,   $27		    # $5 <= uzone pointer (second arg)
-	or	    $19,    $0,	  $27	        # $19 <= &uzone (for kernel_exit)
+	or	    $5,	    $0,   $29		    # $5 <= uzone pointer (second arg)
+	or	    $19,    $0,	  $29	        # $19 <= &uzone (for kentry_exit)
 
 	ori	    $8,     $0,   0x20          # $8 <= cause syscall
@@ -222,5 +219,5 @@
 	addiu	$29,	$29,  -8	        # hal_do_exception has 2 args
 	addiu	$29,	$29,  8
-	j       kernel_exit                 # jump to kernel_exit
+	j       kentry_exit                 # jump to kentry_exit
     nop
 
@@ -230,5 +227,5 @@
 	addiu	$29,	$29,  -8            # hal_do_syscall has 2 args
 	addiu	$29,	$29,  8
-	j	    kernel_exit                 # jump to kernel_exit
+	j	    kentry_exit                 # jump to kentry_exit
 	nop 
 	
@@ -243,5 +240,5 @@
 # The pointer on uzone is supposed to be stored in $19
 # -----------------------------------------------------------------------------------
-kernel_exit:
+kentry_exit:
 
 	# signal that core exit kernel
@@ -250,5 +247,5 @@
     nop
 
-	# restore context from uzone
+	# restore registers from uzone
 	or	    $27,    $0,	$19             # $27 <= &uzone
 
@@ -296,17 +293,18 @@
 	lw	    $31,	(UZ_RA*4)($27)
 
-	lw	    $26,    (UZ_DEXT*4)($27)
-	mtc2	$26,    $24			        # restore CP2_DEXT from uzone
-
 	lw	    $26,    (UZ_MODE*4)($27)    
     mtc2    $26,    $1                  # restore CP2_MODE from uzone
 
+# -----------------------------------------------------------------------------------
+# eret function
+# -----------------------------------------------------------------------------------
+
+hal_kentry_eret:
 	nop
     eret
 
-    .end kernel_enter
     .set reorder
     .set at
 
-#-------------------------------------------------------------------------------
-
+#------------------------------------------------------------------------------------
+
