Index: kernel/arch/sparc32/src/context.S
===================================================================
--- kernel/arch/sparc32/src/context.S	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/context.S	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -51,6 +51,5 @@
 		subcc %g1, 1, %g1
 		bg 1b
-	
-	save %sp, -64, %sp
+		save %sp, -64, %sp
 	
 	mov 7, %g1
@@ -58,6 +57,5 @@
 		subcc %g1, 1, %g1
 		bg 1b
-	
-	restore
+		restore
 	
 	CONTEXT_SAVE_ARCH_CORE %o0
@@ -88,6 +86,5 @@
 		subcc %g1, 1, %g1
 		bg 1b
-	
-	save %sp, -64, %sp
+		save %sp, -64, %sp
 	
 	mov 7, %g1
@@ -95,6 +92,5 @@
 		subcc %g1, 1, %g1
 		bg 1b
-	
-	restore
+		restore
 	
 	CONTEXT_RESTORE_ARCH_CORE %o0
Index: kernel/arch/sparc32/src/debug/stacktrace.c
===================================================================
--- kernel/arch/sparc32/src/debug/stacktrace.c	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/debug/stacktrace.c	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -65,5 +65,5 @@
 bool kernel_frame_pointer_prev(stack_trace_context_t *ctx, uintptr_t *prev)
 {
-	uint64_t *stack = (void *) ctx->fp;
+	uint32_t *stack = (void *) ctx->fp;
 	alloc_window_and_flush();
 	*prev = stack[FRAME_OFFSET_FP_PREV];
@@ -73,5 +73,5 @@
 bool kernel_return_address_get(stack_trace_context_t *ctx, uintptr_t *ra)
 {
-	uint64_t *stack = (void *) ctx->fp;
+	uint32_t *stack = (void *) ctx->fp;
 	alloc_window_and_flush();
 	*ra = stack[FRAME_OFFSET_RA];
Index: kernel/arch/sparc32/src/machine/leon3/leon3.c
===================================================================
--- kernel/arch/sparc32/src/machine/leon3/leon3.c	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/machine/leon3/leon3.c	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -120,5 +120,5 @@
 	grlib_irqmp_clear(&machine.irqmp, irqnum);
 	
-	irq_t *irq = irq_dispatch_and_lock(irqnum);
+	irq_t *irq = irq_dispatch_and_lock(exc);
 	if (irq) {
 		irq->handler(irq);
@@ -140,5 +140,4 @@
 static void leon3_input_init(void)
 {
-#if 0
 	grlib_uart_t *scons_inst;
 	
@@ -158,5 +157,4 @@
 		}
 	}
-#endif
 }
 
Index: kernel/arch/sparc32/src/mm/tlb.c
===================================================================
--- kernel/arch/sparc32/src/mm/tlb.c	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/mm/tlb.c	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -32,5 +32,8 @@
 
 #include <mm/tlb.h>
+#include <arch/arch.h>
 #include <arch/mm/asid.h>
+#include <arch/mm/as.h>
+#include <arch/mm/page.h>
 #include <arch/asm.h>
 #include <typedefs.h>
@@ -38,4 +41,6 @@
 void tlb_invalidate_all(void)
 {
+	asi_u32_write(ASI_MMUCACHE, 0, 1);
+	asi_u32_write(ASI_MMUFLUSH, 0x400, 1);
 }
 
Index: kernel/arch/sparc32/src/sparc32.c
===================================================================
--- kernel/arch/sparc32/src/sparc32.c	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/sparc32.c	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -55,4 +55,5 @@
 char memcpy_from_uspace_failover_address;
 char memcpy_to_uspace_failover_address;
+
 static bootinfo_t machine_bootinfo;
 
@@ -104,4 +105,5 @@
 void arch_post_smp_init(void)
 {
+	machine_input_init();
 }
 
Index: kernel/arch/sparc32/src/trap_table.S
===================================================================
--- kernel/arch/sparc32/src/trap_table.S	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/trap_table.S	(revision c1ee46f30e78ca597d36aa1584e48e26fa4afcf2)
@@ -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 ;
 
