Index: kernel/arch/sparc32/src/trap_table.S
===================================================================
--- kernel/arch/sparc32/src/trap_table.S	(revision 32e8cd1b35fed5b4250d558f0d9cf886b3422481)
+++ kernel/arch/sparc32/src/trap_table.S	(revision 208b5f542c8db6c9ce6844cd62a6794e94185293)
@@ -165,6 +165,6 @@
 	save
 	
-	ldd [%sp +  0], %l0
-	ldd [%sp +  8], %l2
+	ldd [%sp + 0], %l0
+	ldd [%sp + 8], %l2
 	ldd [%sp + 16], %l4
 	ldd [%sp + 24], %l6
@@ -430,5 +430,4 @@
 	nop
 	
-	
 	1:
 		/* Rotate WIM on bit LEFT, we have 8 windows */
@@ -442,5 +441,5 @@
 		mov %g0, %wim
 		nop; nop; nop
-		
+	
 	/* Kernel: */
 	restore
@@ -563,4 +562,5 @@
 		mov %psr, %l0
 		or %l0, (1 << 5), %l0
+		or %l0, 0xf00, %l0
 		mov %l0, %psr
 		nop
@@ -568,9 +568,4 @@
 		nop
 		nop
-		
-		/* Get UWB address */
-		##	switch_to_invalid %g5, %g6
-		##	mov %l6, %g1
-		##	switch_back %g5, %g6
 		
 		/* Flush windows to stack */
@@ -582,8 +577,9 @@
 		add %sp, 128, %o1
 		
-		/* Return from handler */
+		/* Return from handler (leave PIL disabled) */
 		ld [%sp + 92], %l1
 		ld [%sp + 96], %l2
 		ld [%sp + 100], %l0
+		or %l0, 0xf00, %l0
 		mov %l0, %psr
 		nop
@@ -653,4 +649,5 @@
 		ld [%sp + 116], %g4
 		ld [%sp + 120], %g7
+		
 		mov %l3, %sp
 		b 10f
@@ -659,4 +656,5 @@
 	9:
 		inline_restore_kernel
+		
 		ld [%sp + 104], %g1
 		ld [%sp + 108], %g2
@@ -664,4 +662,7 @@
 		ld [%sp + 116], %g4
 		ld [%sp + 120], %g7
+	
+		/* Restore old sp */
+		add %sp, 128, %sp
 	
 	10:
@@ -716,5 +717,4 @@
 		switch_back %g5, %g6
 		mov %g7, %sp
-		##	mov %sp, %fp
 	
 	5:
@@ -733,7 +733,8 @@
 		st %l0, [%sp + 120]
 		
-		/* Enable traps */
+		/* Enable traps (without PIL) */
 		mov %psr, %l0
 		or %l0, (1 << 5), %l0
+		or %l0, 0xf00, %l0
 		mov %l0, %psr
 		nop
@@ -747,12 +748,12 @@
 		
 		/* Jump to actual subroutine */
-		mov %g2, %o0
 		call irq_exception
 		add %sp, 128, %o1
 		
-		/* Return from handler */
+		/* Return from handler (leave PIL disabled) */
 		ld [%sp + 92], %l1
 		ld [%sp + 96], %l2
 		ld [%sp + 100], %l0
+		or %l0, 0xf00, %l0
 		mov %l0, %psr
 		nop
@@ -766,5 +767,5 @@
 		 * and save uwb address for future use.
 		 */
-		if_from_kernel 6f
+		if_from_kernel 9f
 		switch_to_invalid %g5, %g6
 		clr %l7
@@ -797,5 +798,5 @@
 		ba 0b
 		nop
-		
+	
 	/*
 	 * We've restored all user space windows. Now time to
@@ -815,26 +816,6 @@
 		switch_back %g5, %g6
 		
-		/* If next window is invalid, do inline restore */
-	6:
-		get_wim_number %g6
-		get_cwp %g7
-		inc %g7
-		and %g7, 0x7, %g7
-		cmp %g6, %g7
-		bne 8f
-		
-		if_from_kernel 7f
-		
-		inline_restore_uspace %g1
-		switch_to_invalid %g5, %g6
-		mov %g1, %l6
-		switch_back %g5, %g6
-		b 8f
-		nop
-	
-	7:
-		inline_restore_kernel
-	
-	8:
+		mov %sp, %l3
+		sub %g2, 128, %sp
 		ld [%sp + 104], %g1
 		ld [%sp + 108], %g2
@@ -842,4 +823,22 @@
 		ld [%sp + 116], %g4
 		ld [%sp + 120], %g7
+		
+		mov %l3, %sp
+		b 10f
+		nop
+	
+	9:
+		inline_restore_kernel
+		
+		ld [%sp + 104], %g1
+		ld [%sp + 108], %g2
+		ld [%sp + 112], %g3
+		ld [%sp + 116], %g4
+		ld [%sp + 120], %g7
+		
+		/* Restore old sp */
+		add %sp, 128, %sp
+	
+	10:
 		jmp %l1
 		rett %l2
@@ -892,5 +891,5 @@
 		switch_back %g5, %g6
 		mov %g7, %sp
-		##	mov %sp, %fp
+		## mov %sp, %fp
 		
 	5:
@@ -909,7 +908,8 @@
 		st %l0, [%sp + 120]
 		
-		/* Enable traps */
+		/* Enable traps (without PIL) */
 		mov %psr, %l0
 		or %l0, (1 << 5), %l0
+		or %l0, 0xf00, %l0
 		mov %l0, %psr
 		nop
@@ -933,5 +933,5 @@
 		mov %i5, %o5
 		
-		/* Return from handler */
+		/* Return from handler (with PIL disabled) */
 		ld [%sp + 92], %l1
 		ld [%sp + 96], %l2
@@ -940,4 +940,5 @@
 		mov %psr, %l1
 		and %l1, 0xf, %l1
+		or %l1, 0x00000f00, %l1
 		and %l0, 0xfffffff0, %l0
 		or %l0, %l1, %l0
@@ -1009,5 +1010,10 @@
 		ld [%sp + 116], %g4
 		ld [%sp + 120], %g7
+		
 		mov %l1, %sp
+		nop
+		nop
+		nop
+		
 		jmp %l2
 		rett %l2 + 4
@@ -1035,7 +1041,7 @@
 #define INTERRUPT(_vector, _priority) \
 	.org trap_table + _vector * TRAP_ENTRY_SIZE; \
-	mov %psr, %l0 ; \
-	mov _priority, %g2 ; \
+	mov _priority, %o0 ; \
 	b interrupt_trap ; \
+	nop ;
 	nop ;
 
