Index: kernel/arch/sparc64/include/cpu.h
===================================================================
--- kernel/arch/sparc64/include/cpu.h	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/include/cpu.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -64,31 +64,11 @@
 #endif
 
-typedef struct {
-	uint32_t mid;			/**< Processor ID as read from
-					     UPA_CONFIG/FIREPLANE_CONFIG. */
-	ver_reg_t ver;
-	uint32_t clock_frequency;	/**< Processor frequency in Hz. */
-	uint64_t next_tick_cmpr;	/**< Next clock interrupt should be
-					     generated when the TICK register
-					     matches this value. */
-} cpu_arch_t;
 
+#if defined (SUN4U)
+#include <arch/sun4u/cpu.h>
+#elif defined (SUN4V)
+#include <arch/sun4v/cpu.h>
+#endif
 
-/**
- * Reads the module ID (agent ID/CPUID) of the current CPU.
- */
-static inline uint32_t read_mid(void)
-{
-	uint64_t icbus_config = asi_u64_read(ASI_ICBUS_CONFIG, 0);
-	icbus_config = icbus_config >> ICBUS_CONFIG_MID_SHIFT;
-#if defined (US)
-	return icbus_config & 0x1f;
-#elif defined (US3)
-	if (((ver_reg_t) ver_read()).impl == IMPL_ULTRASPARCIII_I)
-		return icbus_config & 0x1f;
-	else
-		return icbus_config & 0x3ff;
-#endif
-}
 
 #endif	
Index: kernel/arch/sparc64/include/sun4u/asm.h
===================================================================
--- kernel/arch/sparc64/include/sun4u/asm.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
+++ kernel/arch/sparc64/include/sun4u/asm.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -0,0 +1,60 @@
+/*
+ * Copyright (c) 2005 Jakub Jermar
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup sparc64	
+ * @{
+ */
+/** @file
+ */
+
+#ifndef KERN_sparc64_sun4u_ASM_H_
+#define KERN_sparc64_sun4u_ASM_H_
+
+extern uint64_t read_from_ag_g7(void);
+extern void write_to_ag_g6(uint64_t val);
+extern void write_to_ag_g7(uint64_t val);
+extern void write_to_ig_g6(uint64_t val);
+
+
+/** Read Version Register.
+ *
+ * @return Value of VER register.
+ */
+static inline uint64_t ver_read(void)
+{
+	uint64_t v;
+	
+	asm volatile ("rdpr %%ver, %0\n" : "=r" (v));
+	
+	return v;
+}
+
+#endif
+
+/** @}
+ */
Index: kernel/arch/sparc64/include/sun4u/cpu.h
===================================================================
--- kernel/arch/sparc64/include/sun4u/cpu.h	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/include/sun4u/cpu.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -33,6 +33,6 @@
  */
 
-#ifndef KERN_sparc64_CPU_H_
-#define KERN_sparc64_CPU_H_
+#ifndef KERN_sparc64_sun4u_CPU_H_
+#define KERN_sparc64_sun4u_CPU_H_
 
 #define MANUF_FUJITSU		0x04
Index: kernel/arch/sparc64/include/sun4v/cpu.h
===================================================================
--- kernel/arch/sparc64/include/sun4v/cpu.h	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/include/sun4v/cpu.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -46,4 +46,5 @@
 struct cpu;
 
+/*
 typedef struct {
 	uint64_t exec_unit_id;
@@ -54,7 +55,6 @@
 	SPINLOCK_DECLARE(proposed_nrdy_lock);
 } exec_unit_t;
+*/
 
-// MH
-#if 0
 typedef struct cpu_arch {
 	uint64_t id;			/**< virtual processor ID */
@@ -63,9 +63,8 @@
 					     generated when the TICK register
 					     matches this value. */
-	exec_unit_t *exec_unit;		/**< Physical core. */
-	unsigned long proposed_nrdy;	/**< Proposed No. of ready threads
-					     so that cores are equally balanced. */
+	//exec_unit_t *exec_unit;		/**< Physical core. */
+	//unsigned long proposed_nrdy;	/**< Proposed No. of ready threads
+	//				     so that cores are equally balanced. */
 } cpu_arch_t;
-#endif
 
 #endif	
Index: kernel/arch/sparc64/include/trap/regwin.h
===================================================================
--- kernel/arch/sparc64/include/trap/regwin.h	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/include/trap/regwin.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -131,33 +131,4 @@
 
 /*
- * Macro used to spill userspace window to userspace window buffer.
- * It can be either triggered from preemptible_handler doing SAVE
- * at (TL=1) or from normal kernel code doing SAVE when OTHERWIN>0
- * at (TL=0).
- */
-.macro SPILL_TO_USPACE_WINDOW_BUFFER
-	stx %l0, [%g7 + L0_OFFSET]	
-	stx %l1, [%g7 + L1_OFFSET]
-	stx %l2, [%g7 + L2_OFFSET]
-	stx %l3, [%g7 + L3_OFFSET]
-	stx %l4, [%g7 + L4_OFFSET]
-	stx %l5, [%g7 + L5_OFFSET]
-	stx %l6, [%g7 + L6_OFFSET]
-	stx %l7, [%g7 + L7_OFFSET]
-	stx %i0, [%g7 + I0_OFFSET]
-	stx %i1, [%g7 + I1_OFFSET]
-	stx %i2, [%g7 + I2_OFFSET]
-	stx %i3, [%g7 + I3_OFFSET]
-	stx %i4, [%g7 + I4_OFFSET]
-	stx %i5, [%g7 + I5_OFFSET]
-	stx %i6, [%g7 + I6_OFFSET]
-	stx %i7, [%g7 + I7_OFFSET]
-	add %g7, STACK_WINDOW_SAVE_AREA_SIZE, %g7
-	saved
-	retry
-.endm
-
-
-/*
  * Macro used by the nucleus and the primary context 0 during normal fills.
  */
@@ -232,6 +203,12 @@
 #endif /* __ASM__ */
 
+#if defined (SUN4U)
+#include <arch/trap/sun4u/regwin.h>
+#elif defined (SUN4V)
+#include <arch/trap/sun4v/regwin.h>
 #endif
 
+#endif
+
 /** @}
  */
Index: kernel/arch/sparc64/include/trap/sun4u/regwin.h
===================================================================
--- kernel/arch/sparc64/include/trap/sun4u/regwin.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
+++ kernel/arch/sparc64/include/trap/sun4u/regwin.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2005 Jakub Jermar
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup sparc64interrupt
+ * @{
+ */
+#ifndef KERN_sparc64_sun4u_REGWIN_H_
+#define KERN_sparc64_sun4u_REGWIN_H_
+
+#ifdef __ASM__
+
+/*
+ * Macro used to spill userspace window to userspace window buffer.
+ * It can be either triggered from preemptible_handler doing SAVE
+ * at (TL=1) or from normal kernel code doing SAVE when OTHERWIN>0
+ * at (TL=0).
+ */
+.macro SPILL_TO_USPACE_WINDOW_BUFFER
+	stx %l0, [%g7 + L0_OFFSET]	
+	stx %l1, [%g7 + L1_OFFSET]
+	stx %l2, [%g7 + L2_OFFSET]
+	stx %l3, [%g7 + L3_OFFSET]
+	stx %l4, [%g7 + L4_OFFSET]
+	stx %l5, [%g7 + L5_OFFSET]
+	stx %l6, [%g7 + L6_OFFSET]
+	stx %l7, [%g7 + L7_OFFSET]
+	stx %i0, [%g7 + I0_OFFSET]
+	stx %i1, [%g7 + I1_OFFSET]
+	stx %i2, [%g7 + I2_OFFSET]
+	stx %i3, [%g7 + I3_OFFSET]
+	stx %i4, [%g7 + I4_OFFSET]
+	stx %i5, [%g7 + I5_OFFSET]
+	stx %i6, [%g7 + I6_OFFSET]
+	stx %i7, [%g7 + I7_OFFSET]
+	add %g7, STACK_WINDOW_SAVE_AREA_SIZE, %g7
+	saved
+	retry
+.endm
+
+#endif
+
+#endif
+
+/** @}
+ */
Index: kernel/arch/sparc64/include/trap/sun4v/regwin.h
===================================================================
--- kernel/arch/sparc64/include/trap/sun4v/regwin.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
+++ kernel/arch/sparc64/include/trap/sun4v/regwin.h	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2005 Jakub Jermar
+ * Copyright (c) 2009 Pavel Rimsky
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * - Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * - Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in the
+ *   documentation and/or other materials provided with the distribution.
+ * - The name of the author may not be used to endorse or promote products
+ *   derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @addtogroup sparc64interrupt
+ * @{
+ */
+#ifndef KERN_sparc64_sun4v_REGWIN_H_
+#define KERN_sparc64_sun4v_REGWIN_H_
+
+#ifdef __ASM__
+
+/*
+ * Saves the contents of the current window to the userspace window buffer.
+ * Does not modify any register window registers, but updates pointer to the
+ * top of the userspace window buffer.
+ *
+ * Parameters:
+ * 	\tmpreg1	global register to be used for scratching purposes
+ * 	\tmpreg2	global register to be used for scratching purposes
+ */
+.macro SAVE_TO_USPACE_WBUF tmpreg1, tmpreg2
+	set SCRATCHPAD_WBUF, \tmpreg2
+	ldxa [\tmpreg2] ASI_SCRATCHPAD, \tmpreg1
+	stx %l0, [\tmpreg1 + L0_OFFSET]	
+	stx %l1, [\tmpreg1 + L1_OFFSET]
+	stx %l2, [\tmpreg1 + L2_OFFSET]
+	stx %l3, [\tmpreg1 + L3_OFFSET]
+	stx %l4, [\tmpreg1 + L4_OFFSET]
+	stx %l5, [\tmpreg1 + L5_OFFSET]
+	stx %l6, [\tmpreg1 + L6_OFFSET]
+	stx %l7, [\tmpreg1 + L7_OFFSET]
+	stx %i0, [\tmpreg1 + I0_OFFSET]
+	stx %i1, [\tmpreg1 + I1_OFFSET]
+	stx %i2, [\tmpreg1 + I2_OFFSET]
+	stx %i3, [\tmpreg1 + I3_OFFSET]
+	stx %i4, [\tmpreg1 + I4_OFFSET]
+	stx %i5, [\tmpreg1 + I5_OFFSET]
+	stx %i6, [\tmpreg1 + I6_OFFSET]
+	stx %i7, [\tmpreg1 + I7_OFFSET]
+	add \tmpreg1, STACK_WINDOW_SAVE_AREA_SIZE, \tmpreg1
+	stxa \tmpreg1, [\tmpreg2] ASI_SCRATCHPAD
+.endm
+
+/*
+ * Macro used to spill userspace window to userspace window buffer.
+ * It is triggered from normal kernel code doing SAVE when
+ * OTHERWIN>0 at (TL=0).
+ */
+.macro SPILL_TO_USPACE_WINDOW_BUFFER
+	SAVE_TO_USPACE_WBUF %g7, %g4
+	saved
+	retry
+.endm
+
+#endif
+
+#endif
+
+/** @}
+ */
Index: kernel/arch/sparc64/src/mm/sun4v/tlb.c
===================================================================
--- kernel/arch/sparc64/src/mm/sun4v/tlb.c	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/src/mm/sun4v/tlb.c	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -212,4 +212,5 @@
 void fast_instruction_access_mmu_miss(unative_t unused, istate_t *istate)
 {
+	asm volatile ("sethi 0x41906, %g0");
 	uintptr_t page_16k = ALIGN_DOWN(istate->tpc, PAGE_SIZE);
 	size_t index = (istate->tpc >> MMU_PAGE_WIDTH) % MMU_PAGES_PER_PAGE;
Index: kernel/arch/sparc64/src/trap/sun4v/trap_table.S
===================================================================
--- kernel/arch/sparc64/src/trap/sun4v/trap_table.S	(revision 3f35634cf8784398faec3ed02b32b4a4ffe2033a)
+++ kernel/arch/sparc64/src/trap/sun4v/trap_table.S	(revision eb79d60443133ae1a59f38af667844b1683015c0)
@@ -136,6 +136,6 @@
 .global data_access_exception_tl0
 data_access_exception_tl0:
-	/*wrpr %g0, PSTATE_AG_BIT | PSTATE_PRIV_BIT, %pstate
-	PREEMPTIBLE_HANDLER data_access_exception*/
+	wrpr %g0, PSTATE_AG_BIT | PSTATE_PRIV_BIT, %pstate
+	PREEMPTIBLE_HANDLER data_access_exception
 
 /* TT = 0x32, TL = 0, data_access_error */
@@ -293,5 +293,5 @@
 .global fast_data_access_protection_handler_tl0
 fast_data_access_protection_handler_tl0:
-	/*FAST_DATA_ACCESS_PROTECTION_HANDLER 0*/
+	FAST_DATA_ACCESS_PROTECTION_HANDLER 0
 
 /* TT = 0x80, TL = 0, spill_0_normal handler */
@@ -355,7 +355,7 @@
 .global instruction_access_exception_tl1
 instruction_access_exception_tl1:
-	/*wrpr %g0, 1, %tl
+	wrpr %g0, 1, %tl
 	wrpr %g0, PSTATE_AG_BIT | PSTATE_PRIV_BIT, %pstate
-	PREEMPTIBLE_HANDLER instruction_access_exception*/
+	PREEMPTIBLE_HANDLER instruction_access_exception
 
 /* TT = 0x0a, TL > 0, instruction_access_error */
@@ -418,5 +418,5 @@
 .global fast_data_access_protection_handler_tl1
 fast_data_access_protection_handler_tl1:
-	/*FAST_DATA_ACCESS_PROTECTION_HANDLER 1*/
+	FAST_DATA_ACCESS_PROTECTION_HANDLER 1
 
 /* TT = 0x80, TL > 0, spill_0_normal handler */
@@ -525,40 +525,25 @@
 .endm
 
+#define NOT(x)	((x) == 0)
+
 /*
- * Preemptible trap handler for handling traps from kernel.
+ * Perform all the actions of the preemptible trap handler which are common
+ * for trapping from kernel and trapping from userspace, including call of the
+ * higher level service routine.
+ *
+ * Important note:
+ * 	This macro must be inserted between the "2:" and "4:" labels. The
+ *	inserting code must be aware of the usage of all the registers
+ *	contained in this macro.
  */
-.macro PREEMPTIBLE_HANDLER_KERNEL
-
-	/*
-	 * ASSERT(%tl == 1)
-	 */
-	rdpr %tl, %g3
-	cmp %g3, 1
-	be 1f
-	nop
-0:	ba 0b					! this is for debugging, if we ever get here
-	nop					! it will be easy to find
-
-	/* prevent unnecessary CLEANWIN exceptions */
-	wrpr %g0, WSTATE_OTHER(0) | WSTATE_NORMAL(0), %wstate
-1:
-	/*
-	 * Prevent SAVE instruction from causing a spill exception. If the
-	 * CANSAVE register is zero, explicitly spill register window
-	 * at CWP + 2.
-	 */
-
-	rdpr %cansave, %g3
-	brnz %g3, 2f
-	nop
-	INLINE_SPILL %g3, %g4
-
-2:
-	/* ask for new register window */
-	save %sp, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
-
+.macro MIDDLE_PART is_syscall
 	/* copy higher level routine's address and its argument */
 	mov %g1, %l0
+.if NOT(\is_syscall)
 	mov %g2, %o0
+.else
+	! store the syscall number on the stack as 7th argument
+	stx %g2, [%sp + STACK_WINDOW_SAVE_AREA_SIZE + STACK_BIAS + STACK_ARG6] 
+.endif
 
 	/*
@@ -591,7 +576,16 @@
 	SAVE_GLOBALS
 
+.if NOT(\is_syscall)
 	/* call higher-level service routine, pass istate as its 2nd parameter */
 	call %l0
 	add %sp, PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC, %o1
+.else
+	/* Call the higher-level syscall handler. */
+	!wrpr %g0, PSTATE_PRIV_BIT | PSTATE_PEF_BIT | PSTATE_IE_BIT, %pstate
+	call syscall_handler
+	nop
+	/* copy the value returned by the syscall */
+	mov %o0, %i0
+.endif
 
 	/* l1 -> g1, ..., l7 -> g7 */
@@ -664,4 +658,147 @@
 	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I6], %i6
 	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I7], %i7
+.endm
+
+
+#if 0
+/*
+ * Preemptible trap handler for handling traps from kernel.
+ */
+.macro PREEMPTIBLE_HANDLER_KERNEL
+
+	/*
+	 * ASSERT(%tl == 1)
+	 */
+	rdpr %tl, %g3
+	cmp %g3, 1
+	be 1f
+	nop
+0:	ba 0b					! this is for debugging, if we ever get here
+	nop					! it will be easy to find
+
+	/* prevent unnecessary CLEANWIN exceptions */
+	wrpr %g0, WSTATE_OTHER(0) | WSTATE_NORMAL(0), %wstate
+1:
+	/*
+	 * Prevent SAVE instruction from causing a spill exception. If the
+	 * CANSAVE register is zero, explicitly spill register window
+	 * at CWP + 2.
+	 */
+
+	rdpr %cansave, %g3
+	brnz %g3, 2f
+	nop
+	INLINE_SPILL %g3, %g4
+
+2:
+	/* ask for new register window */
+	save %sp, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+
+	/* copy higher level routine's address and its argument */
+	mov %g1, %l0
+	mov %g2, %o0
+
+	/*
+	 * Save TSTATE, TPC and TNPC aside.
+	 */
+	rdpr %tstate, %g1
+	rdpr %tpc, %g2
+	rdpr %tnpc, %g3
+
+	stx %g1, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TSTATE]
+	stx %g2, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TPC]
+	stx %g3, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC]
+
+	/*
+	 * Save the Y register.
+	 * This register is deprecated according to SPARC V9 specification
+	 * and is only present for backward compatibility with previous
+	 * versions of the SPARC architecture.
+	 * Surprisingly, gcc makes use of this register without a notice.
+	 */
+	rd %y, %g4
+	stx %g4, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_Y]
+
+	/* switch to TL = 0, explicitly enable FPU */
+	wrpr %g0, 0, %tl
+	wrpr %g0, 0, %gl
+	wrpr %g0, PSTATE_PRIV_BIT | PSTATE_PEF_BIT, %pstate
+
+	/* g1 -> l1, ..., g7 -> l7 */
+	SAVE_GLOBALS
+
+	/* call higher-level service routine, pass istate as its 2nd parameter */
+	call %l0
+	add %sp, PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC, %o1
+
+	/* l1 -> g1, ..., l7 -> g7 */
+	RESTORE_GLOBALS
+
+	/* we must prserve the PEF bit */
+	rdpr %pstate, %l1
+
+	/* TL := 1, GL := 1 */
+	wrpr %g0, PSTATE_PRIV_BIT, %pstate
+	wrpr %g0, 1, %tl
+	wrpr %g0, 1, %gl
+
+	/* Read TSTATE, TPC and TNPC from saved copy. */
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TSTATE], %g1
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TPC], %g2
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_TNPC], %g3
+
+	/* Copy PSTATE.PEF to the in-register copy of TSTATE. */
+	and %l1, PSTATE_PEF_BIT, %l1
+	sllx %l1, TSTATE_PSTATE_SHIFT, %l1
+	sethi %hi(TSTATE_PEF_BIT), %g4		! reset the PEF bit to 0 ...
+	andn %g1, %g4, %g1
+	or %g1, %l1, %g1			! ... "or" it with saved PEF
+
+	/* Restore TSTATE, TPC and TNPC from saved copies. */
+	wrpr %g1, 0, %tstate
+	wrpr %g2, 0, %tpc
+	wrpr %g3, 0, %tnpc
+
+	/* Restore Y. */
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_Y], %g4
+	wr %g4, %y
+	
+	/* If TSTATE.CWP + 1 == CWP, then we do not have to fix CWP. */
+	and %g1, TSTATE_CWP_MASK, %l0
+	inc %l0
+	and %l0, NWINDOWS - 1, %l0	! %l0 mod NWINDOWS
+	rdpr %cwp, %l1
+	cmp %l0, %l1
+	bz 4f				! CWP is ok
+	nop
+
+3:
+	/*
+	 * Fix CWP.
+	 * In order to recapitulate, the input registers in the current
+	 * window are the output registers of the window to which we want
+	 * to restore. Because the fill trap fills only input and local
+	 * registers of a window, we need to preserve those output
+	 * registers manually.
+	 */
+	mov %sp, %g2
+	stx %i0, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I0]
+	stx %i1, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I1]
+	stx %i2, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I2]
+	stx %i3, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I3]
+	stx %i4, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I4]
+	stx %i5, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I5]
+	stx %i6, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I6]
+	stx %i7, [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I7]
+	wrpr %l0, 0, %cwp
+	mov %g2, %sp
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I0], %i0
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I1], %i1
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I2], %i2
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I3], %i3
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I4], %i4
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I5], %i5
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I6], %i6
+	ldx [%sp + PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE + STACK_BIAS + SAVED_I7], %i7
 
 4:
@@ -682,6 +819,287 @@
 .endm
 
-
-#define NOT(x)	((x) == 0)
+#endif
+
+/*
+ * Preemptible trap handler for handling traps from kernel.
+ */
+.macro PREEMPTIBLE_HANDLER_KERNEL
+
+	/*
+	 * ASSERT(%tl == 1)
+	 */
+	rdpr %tl, %g3
+	cmp %g3, 1
+	be 1f
+	nop
+0:	ba 0b					! this is for debugging, if we ever get here
+	nop					! it will be easy to find
+
+1:
+	/* prevent unnecessary CLEANWIN exceptions */
+	wrpr %g0, NWINDOWS - 1, %cleanwin
+
+	/*
+	 * Prevent SAVE instruction from causing a spill exception. If the
+	 * CANSAVE register is zero, explicitly spill register window
+	 * at CWP + 2.
+	 */
+
+	rdpr %cansave, %g3
+	brnz %g3, 2f
+	nop
+	INLINE_SPILL %g3, %g4
+
+2:
+	/* ask for new register window */
+	save %sp, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+
+	MIDDLE_PART 0
+
+4:
+	/*
+	 * Prevent RESTORE instruction from causing a fill exception. If the
+	 * CANRESTORE register is zero, explicitly fill register window
+	 * at CWP - 1.
+	 */
+	rdpr %canrestore, %g1
+	brnz %g1, 5f
+	nop
+	INLINE_FILL %g3, %g4
+
+5:
+	restore
+	retry
+.endm
+
+
+
+/*
+ * Spills the window at CWP + 2 to the userspace window buffer. This macro
+ * is to be used before doing SAVE when the spill trap is undesirable.
+ * 
+ * Parameters:
+ * 	tmpreg1		global register to be used for scratching purposes
+ * 	tmpreg2		global register to be used for scratching purposes
+ * 	tmpreg3		global register to be used for scratching purposes
+ */
+.macro INLINE_SPILL_TO_WBUF tmpreg1, tmpreg2, tmpreg3
+	! CWP := CWP + 2
+	rdpr %cwp, \tmpreg2
+	add \tmpreg2, 2, \tmpreg1
+	and \tmpreg1, NWINDOWS - 1, \tmpreg1		! modulo NWINDOWS
+	wrpr \tmpreg1, %cwp
+	
+	! spill to userspace window buffer
+	SAVE_TO_USPACE_WBUF \tmpreg3, \tmpreg1
+
+	! CWP := CWP - 2
+	wrpr \tmpreg2, %cwp
+
+	saved
+.endm
+
+/*
+ * Preemptible handler for handling traps from userspace.
+ */
+.macro PREEMPTIBLE_HANDLER_USPACE is_syscall
+	/*
+	 * One of the ways this handler can be invoked is after a nested MMU trap from
+	 * either spill_1_normal or fill_1_normal traps. Both of these traps manipulate
+	 * the CWP register. We deal with the situation by simulating the MMU trap
+	 * on TL=1 and restart the respective SAVE or RESTORE instruction once the MMU
+	 * trap is resolved. However, because we are in the wrong window from the
+	 * perspective of the MMU trap, we need to synchronize CWP with CWP from TL=0.
+	 */ 
+.if NOT(\is_syscall)
+	rdpr %tstate, %g3
+	and %g3, TSTATE_CWP_MASK, %g4
+	wrpr %g4, 0, %cwp			! resynchronize CWP
+.endif
+
+	/* prevent unnecessary CLEANWIN exceptions */
+	wrpr %g0, NWINDOWS - 1, %cleanwin
+
+	/*
+	 * Prevent SAVE instruction from causing a spill exception. If the
+	 * CANSAVE register is zero, explicitly spill register window
+	 * at CWP + 2.
+	 */
+	rdpr %cansave, %g3
+	brnz %g3, 2f
+	nop
+	INLINE_SPILL_TO_WBUF %g3, %g4, %g7
+
+2:
+	set SCRATCHPAD_KSTACK, %g4
+	ldxa [%g4] ASI_SCRATCHPAD, %g6
+	save %g6, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+
+.if \is_syscall
+	/* Copy arguments for the syscall to the new window. */
+	mov %i0, %o0
+	mov %i1, %o1
+	mov %i2, %o2
+	mov %i3, %o3
+	mov %i4, %o4
+	mov %i5, %o5
+.endif
+
+	mov VA_PRIMARY_CONTEXT_REG, %l0 
+	stxa %g0, [%l0] ASI_PRIMARY_CONTEXT_REG
+	rd %pc, %l0
+	flush %l0
+
+	/* Mark the CANRESTORE windows as OTHER windows. */
+	rdpr %canrestore, %l0
+	wrpr %l0, %otherwin
+	wrpr %g0, %canrestore
+
+	/*
+	 * Other window spills will go to the userspace window buffer
+	 * and normal spills will go to the kernel stack.
+	 */
+	wrpr %g0, WSTATE_OTHER(0) | WSTATE_NORMAL(0), %wstate
+
+	MIDDLE_PART \is_syscall
+
+4:
+	/*
+	 * Spills and fills will be processed by the {spill,fill}_1_normal
+	 * handlers.
+	 */
+	wrpr %g0, WSTATE_OTHER(0) | WSTATE_NORMAL(1), %wstate
+
+	/*
+	 * Set primary context according to secondary context.
+	 */
+	wr %g0, ASI_SECONDARY_CONTEXT_REG, %asi
+	ldxa [VA_SECONDARY_CONTEXT_REG] %asi, %g1
+	wr %g0, ASI_PRIMARY_CONTEXT_REG, %asi
+	stxa %g1, [VA_PRIMARY_CONTEXT_REG] %asi
+	rd %pc, %g1
+	flush %g1
+
+	/* Restoring userspace windows: */
+
+	/* Save address of the userspace window buffer to the %g7 register. */
+	set SCRATCHPAD_WBUF, %g5
+	ldxa [%g5] ASI_SCRATCHPAD, %g7
+
+	rdpr %cwp, %g1
+	rdpr %otherwin, %g2
+
+	/*
+	 * Skip all OTHERWIN windows and descend to the first window
+	 * in the userspace window buffer.
+	 */
+	sub %g1, %g2, %g3
+	dec %g3
+	and %g3, NWINDOWS - 1, %g3
+	wrpr %g3, 0, %cwp
+
+	/*
+	 * CWP is now in the window last saved in the userspace window buffer.
+	 * Fill all windows stored in the buffer.
+	 */
+	clr %g4
+5:	andcc %g7, UWB_ALIGNMENT - 1, %g0	! alignment check
+	bz 6f					! %g7 is UWB_ALIGNMENT-aligned, no more windows to refill
+	nop
+
+	add %g7, -STACK_WINDOW_SAVE_AREA_SIZE, %g7
+	ldx [%g7 + L0_OFFSET], %l0
+	ldx [%g7 + L1_OFFSET], %l1
+	ldx [%g7 + L2_OFFSET], %l2
+	ldx [%g7 + L3_OFFSET], %l3
+	ldx [%g7 + L4_OFFSET], %l4
+	ldx [%g7 + L5_OFFSET], %l5
+	ldx [%g7 + L6_OFFSET], %l6
+	ldx [%g7 + L7_OFFSET], %l7
+	ldx [%g7 + I0_OFFSET], %i0
+	ldx [%g7 + I1_OFFSET], %i1
+	ldx [%g7 + I2_OFFSET], %i2
+	ldx [%g7 + I3_OFFSET], %i3
+	ldx [%g7 + I4_OFFSET], %i4
+	ldx [%g7 + I5_OFFSET], %i5
+	ldx [%g7 + I6_OFFSET], %i6
+	ldx [%g7 + I7_OFFSET], %i7
+
+	dec %g3
+	and %g3, NWINDOWS - 1, %g3
+	wrpr %g3, 0, %cwp			! switch to the preceeding window
+
+	ba 5b
+	inc %g4
+
+6:
+	/* Save changes of the address of the userspace window buffer. */
+	stxa %g7, [%g5] ASI_SCRATCHPAD
+
+	/*
+	 * Switch back to the proper current window and adjust
+	 * OTHERWIN, CANRESTORE, CANSAVE and CLEANWIN.
+	 */
+	wrpr %g1, 0, %cwp
+	add %g4, %g2, %g2
+	cmp %g2, NWINDOWS - 2
+	bg 8f					! fix the CANRESTORE=NWINDOWS-1 anomaly
+	mov NWINDOWS - 2, %g1			! use dealy slot for both cases
+	sub %g1, %g2, %g1
+	
+	wrpr %g0, 0, %otherwin
+	wrpr %g1, 0, %cansave			! NWINDOWS - 2 - CANRESTORE
+	wrpr %g2, 0, %canrestore		! OTHERWIN + windows in the buffer
+	wrpr %g2, 0, %cleanwin			! avoid information leak
+
+7:
+	restore
+
+.if \is_syscall
+	done
+.else
+	retry
+.endif
+
+8:
+	/*
+	 * We got here in order to avoid inconsistency of the window state registers.
+	 * If the:
+	 *
+	 * 	save %g6, -PREEMPTIBLE_HANDLER_STACK_FRAME_SIZE, %sp
+	 *
+	 * instruction trapped and spilled a register window into the userspace
+	 * window buffer, we have just restored NWINDOWS - 1 register windows.
+	 * However, CANRESTORE can be only NWINDOW - 2 at most.
+	 *
+	 * The solution is to manually switch to (CWP - 1) mod NWINDOWS
+	 * and set the window state registers so that:
+	 *
+	 * 	CANRESTORE 	= NWINDOWS - 2
+	 *	CLEANWIN	= NWINDOWS - 2
+	 *	CANSAVE 	= 0
+	 *	OTHERWIN	= 0
+	 *
+	 * The RESTORE instruction is therfore to be skipped.
+	 */
+	wrpr %g0, 0, %otherwin
+	wrpr %g0, 0, %cansave
+	wrpr %g1, 0, %canrestore
+	wrpr %g1, 0, %cleanwin
+
+	rdpr %cwp, %g1
+	dec %g1
+	and %g1, NWINDOWS - 1, %g1
+	wrpr %g1, 0, %cwp			! CWP--
+	
+.if \is_syscall
+	done
+.else
+	retry
+.endif
+
+.endm
+
+
 
 /* Preemptible trap handler for TL=1.
@@ -692,5 +1110,17 @@
  */
 .macro PREEMPTIBLE_HANDLER_TEMPLATE is_syscall
+	rdpr %tstate, %g3
+	and %g3, TSTATE_PRIV_BIT, %g3
+	brz %g3, 100f			! trapping from userspace
+	nop
+
 	PREEMPTIBLE_HANDLER_KERNEL
+	ba 101f
+	nop
+
+	100:
+	PREEMPTIBLE_HANDLER_USPACE \is_syscall
+
+	101:
 .endm