Index: trunk/hal/tsar_mips32/core/hal_kentry.S
===================================================================
--- trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 407)
+++ trunk/hal/tsar_mips32/core/hal_kentry.S	(revision 408)
@@ -40,25 +40,29 @@
 #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
+#define      UZ_S0           16
+#define      UZ_S1           17
+#define      UZ_S2           18
+#define      UZ_S3           19
+#define      UZ_S4           20
+#define      UZ_S5           21
+#define      UZ_S6           22
+#define      UZ_S7           23
+#define      UZ_T8           24
+#define      UZ_T9           25
+
+#define      UZ_LO           26
+#define      UZ_HI           27
+
+#define      UZ_GP           28
+#define      UZ_SP           29
+#define      UZ_S8           30
+#define      UZ_RA           31
+#define      UZ_PTPR         32
+#define      UZ_EPC          33
+#define      UZ_SR           34
+#define      UZ_TH           35
+#define      UZ_CR           36
+
+#define      UZ_REGS         37
 
 #include <kernel_config.h>
@@ -69,6 +73,7 @@
 	.extern    hal_do_exception
 	.extern    hal_do_syscall
-	.extern    cluster_core_kernel_enter
-	.extern    cluster_core_kentry_exit
+    .extern    puts
+    .extern    putx
+    .extern    putl
 
 	.org       0x180
@@ -80,9 +85,9 @@
 	.set       noreorder
 
-#---------------------------------------------------------------------------------
+#------------------------------------------------------------------------------------
 # 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.
-#---------------------------------------------------------------------------------
+#------------------------------------------------------------------------------------
 
 hal_kentry_enter:
@@ -93,5 +98,5 @@
 	ori     $27,    $0,	  0x3           # $27 <= code for MMU OFF
 	
-#---------------------------------------------------------------------------------------	
+#------------------------------------------------------------------------------------
 # This code is executed when the core is in user mode:
 # - save current c2_mode in $26.
@@ -106,10 +111,10 @@
     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
+    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 already in kernel mode:
 # - save current c2_mode in $26.
@@ -123,5 +128,5 @@
     move    $27,    $29                 # $27 <= current kernel stack pointer
 
-#---------------------------------------------------------------------------------------	
+#------------------------------------------------------------------------------------	
 # This code is executed in both modes (user or kernel):
 # The assumptions are:
@@ -131,8 +136,7 @@
 # - $29 contains the kernel stack pointer.
 # We execute the following actions:
-# - allocate an uzone in kernel stack, incrementing $29
-# - save relevant registers to uzone.
+# - allocate an uzone in kernel stack, decrementing $29.
+# - save relevant GPR, CP0 and CP2 registers to uzone.
 # - set the SR in kernel mode: IRQ disabled, clear exl.
-# - signal the kernel entry.
 
 unified_mode:
@@ -166,42 +170,120 @@
 	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)($29)	    # save c0_epc
-	mflo	$14
-	sw      $14,	(UZ_LO*4)($29)      # save lo
-	mfhi	$15 
-	sw      $15,	(UZ_HI*4)($29)		# save hi
-	mfc0	$18,	$12 
-	sw	    $18,	(UZ_SR*4)($29)		# save c0_sr
-	mfc0    $17,    $13
-	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
-
-#---------------------------------------------------------------------------------------
-# This code call the relevant Interrupt / Exception / Syscall handler,
+	mflo	$1
+	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
+
+	mfc0	$1,     $14
+	sw      $1,     (UZ_EPC*4)($29)	    # save c0_epc
+	mfc0	$1,	    $12 
+	sw	    $1,	    (UZ_SR*4)($29)		# save c0_sr
+	mfc0    $1,     $4,  2
+	sw      $1,	    (UZ_TH*4)($29)		# save c0_th
+	mfc0    $1,     $13   
+	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 
+	srl	    $3,     $3,   5
+	sll     $3,	    $3,   5	            # reset 5 LSB bits
+	mtc0	$3,	    $12			        # set new c0_sr 
+
+#if CONFIG_KENTRY_DEBUG
+
+    # display "enter" message
+    la      $4,     msg_enter
+    jal     puts
+    nop
+    move    $4,     $29
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved SP value
+    la      $4,     msg_sp
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_SP*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved RA value
+    la      $4,     msg_ra
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_RA*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved TH value
+    la      $4,     msg_th
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_TH*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved EPC value
+    la      $4,     msg_epc
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_EPC*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved MODE value
+    la      $4,     msg_mode
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_MODE*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved V0 value
+    la      $4,     msg_v0
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_V0*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+	
+#endif
+    
+#------------------------------------------------------------------------------------
+# This code update the uzone field in thread descriptor, 
+# and call the relevant Interrupt / Exception / Syscall handler,
 # 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 kentry_exit.
 
 	mfc0    $17,    $13                 # $17 <= CR
 	andi    $17,    $17,   0x3F         # $17 <= XCODE
 
-	mfc0    $4,     $4,   2             # $4 <= thread pointer (first arg)
-	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
+	mfc0    $4,     $4,   2             # $4 <= pointer on thread desc
+    sw      $29,    8($4)               # update uzone pointer in thread desc
+
+	ori	    $8,     $0,   0x20
     beq	    $8,	    $17,  cause_sys     # go to syscall handler
     nop
@@ -210,51 +292,116 @@
 
 cause_excp:
-	la      $1,	    hal_do_exception
-	jalr	$1	                        # call exception handler
-	addiu	$29,	$29,  -8	        # hal_do_exception has 2 args
-	addiu	$29,	$29,  8
+	jal     hal_do_exception            # call exception handler
+	nop
 	j       kentry_exit                 # jump to kentry_exit
     nop
 
 cause_sys:
-	la	    $1, 	hal_do_syscall
-	jalr	$1                          # call syscall handler                 
-	addiu	$29,	$29,  -8            # hal_do_syscall has 2 args
-	addiu	$29,	$29,  8
+	jal 	hal_do_syscall              # call syscall handler                 
+    nop
 	j	    kentry_exit                 # jump to kentry_exit
 	nop 
 	
 cause_int:
-	la	    $1,	    hal_do_interrupt
-	jalr    $1                          # call interrupt handler
-	addiu	$29,	$29,  -8            # hal_do_interrupt has 2 args
-	addiu	$29,	$29,  8
+	jal     hal_do_interrupt            # call interrupt handler
+    nop
 
 # -----------------------------------------------------------------------------------
 # Kernel exit
-# The pointer on uzone is supposed to be stored in $19
+# The pointer on uzone is supposed to be contained in $29
 # -----------------------------------------------------------------------------------
 kentry_exit:
 
+#if CONFIG_KENTRY_DEBUG
+
+    # display "exit" message
+    la      $4,     msg_exit
+    jal     puts
+    nop
+    move    $4,     $29
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved SP value
+    la      $4,     msg_sp
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_SP*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved RA value
+    la      $4,     msg_ra
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_RA*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved TH value
+    la      $4,     msg_th
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_TH*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved EPC value
+    la      $4,     msg_epc
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_EPC*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved MODE value
+    la      $4,     msg_mode
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_MODE*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+    # display saved V0 value
+    la      $4,     msg_v0
+    jal     puts
+    nop
+    lw	    $4,  	(UZ_V0*4)($29)
+    jal     putx
+    nop
+    la      $4,     msg_crlf
+    jal     puts
+    nop    
+	
+#endif
+    
 	# restore registers from uzone
-	or	    $27,    $0,	$19             # $27 <= &uzone
-
-	lw	    $29,	(UZ_SP*4)($27)		# restore SP from uzone
-	lw	    $16,	(UZ_EPC*4)($27)	       
-	mtc0	$16,	$14			        # restore EPC from uzone
-	lw	    $16,    (UZ_HI*4)($27)
-	mthi	$16				            # restore HI from uzone
-	lw	    $16,    (UZ_LO*4)($27)
-	mtlo	$16				            # restore LO from uzone
-
-	lw	    $17,	(UZ_SR*4)($27)		# get saved SR value from uzone
-	andi	$17,	$17,	0x1F        # keep only the 5 LSB bits
-	mfc0	$26,	$12			        # get current SR value from CP0
-	or	    $26,    $26,	$17         # merge the two values
-	mtc0	$26,    $12			        # setup new SR to CP0
+	or	    $27,    $0,	$29             # $27 <= ksp (contains &uzone)
+
+	lw	    $1,	    (UZ_EPC*4)($27)	       
+	mtc0	$1,	    $14			        # restore c0_epc from uzone
+	lw	    $1,	    (UZ_SR*4)($27)
+	mtc0	$1,     $12			        # restore c0_sr from uzone
+
+	lw	    $26,    (UZ_HI*4)($27)
+	mthi	$26				            # restore hi from uzone
+	lw	    $26,    (UZ_LO*4)($27)
+	mtlo	$26				            # restore lo from uzone
 
 	lw	    $1,     (UZ_AT*4)($27)		
-	lw	    $2,	    (UZ_V0*4)($27)
-	lw	    $3,	    (UZ_V1*4)($27)
+    lw	    $2,	    (UZ_V0*4)($27)
+    lw	    $3,	    (UZ_V1*4)($27)
 	lw	    $4,	    (UZ_A0*4)($27)
 	lw	    $5,     (UZ_A1*4)($27)
@@ -279,7 +426,9 @@
 	lw	    $24,    (UZ_T8*4)($27)
 	lw	    $25,    (UZ_T9*4)($27)	
-	lw	    $28,	(UZ_GP*4)($27)
-	lw	    $30,	(UZ_S8*4)($27)
-	lw	    $31,	(UZ_RA*4)($27)
+
+	lw	    $28,	(UZ_GP*4)($27)      # restore gp_28 from uzone
+	lw	    $29,	(UZ_SP*4)($27)		# restore sp_29 from uzone
+	lw	    $30,	(UZ_S8*4)($27)      # restore s8_30 from uzone
+	lw	    $31,	(UZ_RA*4)($27)      # restore ra_31 from uzone
 
 	lw	    $26,    (UZ_MODE*4)($27)    
@@ -291,6 +440,5 @@
 
 hal_kentry_eret:
-	nop
-    eret
+    eret                                # jump to EPC, reset EXL bit
 
     .set reorder
@@ -298,3 +446,32 @@
 
 #------------------------------------------------------------------------------------
-
+    .section .kdata
+
+msg_sp:
+    .align 2
+    .asciiz "- UZ_SP   = " 
+msg_ra:
+    .align 2
+    .asciiz "- UZ_RA   = " 
+msg_epc:
+    .align 2
+    .asciiz "- UZ_EPC  = " 
+msg_th:
+    .align 2
+    .asciiz "- UZ_TH   = " 
+msg_mode:
+    .align 2
+    .asciiz "- UZ_MODE = " 
+msg_v0:
+    .align 2
+    .asciiz "- UZ_V0   = " 
+msg_crlf:
+    .align 2
+    .asciiz "\n" 
+msg_enter:
+    .align 2
+    .asciiz "\nenter kernel : &uzone = " 
+msg_exit:
+    .align 2
+    .asciiz "\nexit kernel : &uzone = " 
+
