Index: kernel/arch/sparc32/src/proc/scheduler.c
===================================================================
--- kernel/arch/sparc32/src/proc/scheduler.c	(revision e6a3bfee0d976d9fbe2fc0ae6807ec2a6f68989b)
+++ kernel/arch/sparc32/src/proc/scheduler.c	(revision bbb9454561ed446417b4a514ee1e3f9254c5cb9e)
@@ -44,7 +44,10 @@
 void before_thread_runs_arch(void)
 {
-	uint32_t kernel_sp = (uint32_t) THREAD->kstack + STACK_SIZE;
-	uint32_t uspace_wbuf = (uint32_t) THREAD->arch.uspace_window_buffer;
-	write_to_invalid(kernel_sp, uspace_wbuf, 0xdeadbeef);
+	printf("before_thread_runs_arch(uspace=%d, kernel_sp=0x%08x\n", THREAD->uspace, (uintptr_t)THREAD->kstack + STACK_SIZE);
+	if (THREAD->uspace) {
+		uint32_t kernel_sp = (uint32_t) THREAD->kstack + STACK_SIZE - 8;
+		uint32_t uspace_wbuf = (uint32_t) THREAD->arch.uspace_window_buffer;
+		write_to_invalid(kernel_sp, uspace_wbuf, 0);
+	}
 }
 
Index: kernel/arch/sparc32/src/sparc32.c
===================================================================
--- kernel/arch/sparc32/src/sparc32.c	(revision e6a3bfee0d976d9fbe2fc0ae6807ec2a6f68989b)
+++ kernel/arch/sparc32/src/sparc32.c	(revision bbb9454561ed446417b4a514ee1e3f9254c5cb9e)
@@ -83,5 +83,5 @@
 {
 	/* Test register windows */
-	write_to_invalid(0xdeadbeef, 0xcafebabe, 0xfeedface);
+	write_to_invalid(0xdeadbeef, 0xcafebabe, 0);
 	func1();
 
@@ -110,5 +110,5 @@
 void arch_post_smp_init(void)
 {
-	machine_input_init();
+//	machine_input_init();
 }
 
Index: kernel/arch/sparc32/src/trap_table.S
===================================================================
--- kernel/arch/sparc32/src/trap_table.S	(revision e6a3bfee0d976d9fbe2fc0ae6807ec2a6f68989b)
+++ kernel/arch/sparc32/src/trap_table.S	(revision bbb9454561ed446417b4a514ee1e3f9254c5cb9e)
@@ -59,11 +59,11 @@
 
 .macro switch_to_invalid saved_wim, saved_psr
-	get_wim_number %g6
+	get_wim_number %l3
 	mov %wim, \saved_wim	! save WIM
 	mov %g0, %wim		! clear WIM
 	mov %psr, \saved_psr	! read PSR
-	and \saved_psr, 0xfffffff0, %l0
-	or %l0, %g6, %g6	! set CWP
-	mov %g6, %psr		! write PSR
+	and \saved_psr, 0xfffffff0, %l4
+	or %l4, %l3, %l3	! set CWP
+	mov %l3, %psr		! write PSR
 	nop
 	nop
@@ -230,12 +230,12 @@
 	/* dump registers to uwb */
 2:	save
-	std %l0, [%g4 +  0]
-	std %l2, [%g4 +  8]
-	std %l4, [%g4 + 16]
-	std %l6, [%g4 + 24]
-	std %i0, [%g4 + 32]
-	std %i2, [%g4 + 40]
-	std %i4, [%g4 + 48]
-	std %i6, [%g4 + 56]
+	std %l0, [%g6 +  0]
+	std %l2, [%g6 +  8]
+	std %l4, [%g6 + 16]
+	std %l6, [%g6 + 24]
+	std %i0, [%g6 + 32]
+	std %i2, [%g6 + 40]
+	std %i4, [%g6 + 48]
+	std %i6, [%g6 + 56]
 	add %g4, 64, %g4
 
@@ -276,4 +276,16 @@
 	restore
 	restore
+	/* Save invalid window data */
+	mov %l5, %g5		! kernel stack pointer
+	mov %l6, %g6		! kernel wbuf
+	mov %l7, %g7
+
+	/* we should check whether window needs to be saved
+	 * to kernel stack or uwb
+	 */
+	cmp %g7, 0
+	bne 3f
+	nop
+
         ldd [%sp +  0], %l0
         ldd [%sp +  8], %l2
@@ -290,28 +302,26 @@
 1:	restore
 	restore
-	set uspace_wbuf, %g2
-	ld [%g2], %g1
-        ldd [%g1 +  0], %l0
-        ldd [%g1 +  8], %l2
-        ldd [%g1 + 16], %l4
-        ldd [%g1 + 24], %l6
-        ldd [%g1 + 32], %i0
-        ldd [%g1 + 40], %i2
-        ldd [%g1 + 48], %i4
-        ldd [%g1 + 56], %i6
-	sub %g1, 64, %g1
-	st %g1, [%g2]
-
-2:	/* Save invalid window data */
+	/* Save invalid window data */
 	mov %l5, %g5		! kernel stack pointer
 	mov %l6, %g6		! kernel wbuf
 	mov %l7, %g7
-	save
-	save
-
-        /* Restore invalid window data */
+3:	sub %g6, 64, %g6
+        ldd [%g6 +  0], %l0
+        ldd [%g6 +  8], %l2
+        ldd [%g6 + 16], %l4
+        ldd [%g6 + 24], %l6
+        ldd [%g6 + 32], %i0
+        ldd [%g6 + 40], %i2
+        ldd [%g6 + 48], %i4
+        ldd [%g6 + 56], %i6
+
+2:	/* Restore invalid window data */
+	restore
 	mov %g5, %l5
 	mov %g6, %l6
 	mov %g7, %l7
+	save
+	save
+	save
 
 	/* Set new value of window */
@@ -351,5 +361,4 @@
 	/* Load kernel stack pointer from invalid window */
 	switch_to_invalid %g5, %g6
-	save
 	/* set uspace window mark */
 	mov %psr, %l7
@@ -475,5 +484,5 @@
 	/* Load kernel stack pointer from invalid window */
 	switch_to_invalid %g5, %g6
-	save
+	restore
 	mov %l5, %g7		! stack pointer
 	switch_back %g5, %g6