Index: kernel/arch/sparc32/include/arch/asm.h
===================================================================
--- kernel/arch/sparc32/include/arch/asm.h	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/include/arch/asm.h	(revision 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -161,4 +161,18 @@
 	pil = psr.pil;
 	
+	psr.pil = 0x00;
+	psr_write(psr.value);
+	
+	return pil;
+}
+
+NO_TRACE static inline ipl_t interrupts_disable(void)
+{
+	psr_reg_t psr;
+	psr.value = psr_read();
+	
+	ipl_t pil;
+	pil = psr.pil;
+	
 	psr.pil = 0x0f;
 	psr_write(psr.value);
@@ -167,18 +181,4 @@
 }
 
-NO_TRACE static inline ipl_t interrupts_disable(void)
-{
-	psr_reg_t psr;
-	psr.value = psr_read();
-	
-	ipl_t pil;
-	pil = psr.pil;
-	
-	psr.pil = 0;
-	psr_write(psr.value);
-	
-	return pil;
-}
-
 NO_TRACE static inline void interrupts_restore(ipl_t ipl)
 {
@@ -200,5 +200,5 @@
 	psr_reg_t psr;
 	psr.value = psr_read();
-	return psr.pil == 0;
+	return (psr.pil == 0x0f);
 }
 
Index: kernel/arch/sparc32/include/arch/atomic.h
===================================================================
--- kernel/arch/sparc32/include/arch/atomic.h	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/include/arch/atomic.h	(revision 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -37,4 +37,5 @@
 
 #include <typedefs.h>
+#include <arch/asm.h>
 #include <arch/barrier.h>
 #include <preemption.h>
@@ -47,6 +48,8 @@
     REQUIRES(val->count < ATOMIC_COUNT_MAX)
 {
-	// FIXME TODO
+	// FIXME: Isn't there any intrinsic atomic operation?
+	ipl_t ipl = interrupts_disable();
 	val->count++;
+	interrupts_restore(ipl);
 }
 
@@ -56,6 +59,8 @@
     REQUIRES(val->count > ATOMIC_COUNT_MIN)
 {
-	// FIXME TODO
+	// FIXME: Isn't there any intrinsic atomic operation?
+	ipl_t ipl = interrupts_disable();
 	val->count--;
+	interrupts_restore(ipl);
 }
 
@@ -65,9 +70,11 @@
     REQUIRES(val->count < ATOMIC_COUNT_MAX)
 {
-	// FIXME TODO
+	// FIXME: Isn't there any intrinsic atomic operation?
 	
+	ipl_t ipl = interrupts_disable();
 	atomic_count_t prev = val->count;
 	
 	val->count++;
+	interrupts_restore(ipl);
 	return prev;
 }
@@ -78,9 +85,11 @@
     REQUIRES(val->count > ATOMIC_COUNT_MIN)
 {
-	// FIXME TODO
+	// FIXME: Isn't there any intrinsic atomic operation?
 	
+	ipl_t ipl = interrupts_disable();
 	atomic_count_t prev = val->count;
 	
 	val->count--;
+	interrupts_restore(ipl);
 	return prev;
 }
@@ -93,8 +102,14 @@
     REQUIRES_EXTENT_MUTABLE(val)
 {
-	// FIXME TODO
+	atomic_count_t prev;
+	volatile uintptr_t ptr = (uintptr_t) &val->count;
 	
-	atomic_count_t prev = val->count;
-	val->count = 1;
+	asm volatile (
+		"ldstub [%[ptr]] %[prev]\n"
+		: [prev] "=r" (prev)
+		: [ptr] "r" (ptr)
+		: "memory"
+	);
+	
 	return prev;
 }
@@ -104,9 +119,32 @@
     REQUIRES_EXTENT_MUTABLE(val)
 {
-	// FIXME TODO
+	atomic_count_t tmp1 = 0;
 	
-	do {
-		while (val->count);
-	} while (test_and_set(val));
+	volatile uintptr_t ptr = (uintptr_t) &val->count;
+	
+	preemption_disable();
+	
+	asm volatile (
+		"0:\n"
+			"ldstub %0, %1\n"
+			"tst %1\n"
+			"be 2f\n"
+			"nop\n"
+		"1:\n"
+			"ldub %0, %1\n"
+			"tst %1\n"
+			"bne 1b\n"
+			"nop\n"
+			"ba,a 0b\n"
+		"2:\n"
+		: "+m" (*((atomic_count_t *) ptr)),
+		  "+r" (tmp1)
+		: "r" (0)
+	);
+	
+	/*
+	 * Prevent critical section code from bleeding out this way up.
+	 */
+	CS_ENTER_BARRIER();
 }
 
Index: kernel/arch/sparc32/include/arch/barrier.h
===================================================================
--- kernel/arch/sparc32/include/arch/barrier.h	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/include/arch/barrier.h	(revision 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -36,12 +36,37 @@
 #define KERN_sparc32_BARRIER_H_
 
-// FIXME TODO
+/*
+ * Provisions are made to prevent compiler from reordering instructions itself.
+ */
 
-#define CS_ENTER_BARRIER()
-#define CS_LEAVE_BARRIER()
+#define CS_ENTER_BARRIER() \
+	asm volatile ( \
+		"stbar\n" \
+		::: "memory" \
+	)
 
-#define memory_barrier()
-#define read_barrier()
-#define write_barrier()
+#define CS_LEAVE_BARRIER() \
+	asm volatile ( \
+		"stbar\n" \
+		::: "memory" \
+	)
+
+#define memory_barrier() \
+	asm volatile ( \
+		"stbar\n" \
+		::: "memory" \
+	)
+
+#define read_barrier() \
+	asm volatile ( \
+		"stbar\n" \
+		::: "memory" \
+	)
+
+#define write_barrier() \
+	asm volatile ( \
+		"stbar\n" \
+		::: "memory" \
+	)
 
 #define smc_coherence(addr)
Index: kernel/arch/sparc32/include/arch/mm/page.h
===================================================================
--- kernel/arch/sparc32/include/arch/mm/page.h	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/include/arch/mm/page.h	(revision 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -71,5 +71,5 @@
 /* Page table sizes for each level. */
 #define PTL0_FRAMES_ARCH  1
-#define PTL1_FRAMES_ARCH  0
+#define PTL1_FRAMES_ARCH  1
 #define PTL2_FRAMES_ARCH  1
 #define PTL3_FRAMES_ARCH  1
Index: kernel/arch/sparc32/src/context.S
===================================================================
--- kernel/arch/sparc32/src/context.S	(revision ceb5757df640c89d465c66d3ba4c834d0b017739)
+++ kernel/arch/sparc32/src/context.S	(revision 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 9359aae9d5e8472d1863d2eb319c40c5d632be19)
@@ -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 ;