[SPARC64]: Refine register window trap handling.

When saving and restoing trap state, do the window spill/fill
handling inline so that we never trap deeper than 2 trap levels.
This is important for chips like Niagara.

The window fixup code is massively simplified, and many more
improvements are now possible.

Signed-off-by: David S. Miller <davem@davemloft.net>

arch/sparc64/kernel/etrap.S

@@ -55,7 +55,31 @@ etrap_irq:
 		rd	%y, %g3
 		stx	%g1, [%g2 + STACKFRAME_SZ + PT_V9_TNPC]
 		st	%g3, [%g2 + STACKFRAME_SZ + PT_V9_Y]
-		save	%g2, -STACK_BIAS, %sp	! Ordering here is critical
+
+		rdpr	%cansave, %g1
+		brnz,pt %g1, etrap_save
+		 nop
+
+		rdpr	%cwp, %g1
+		add	%g1, 2, %g1
+		wrpr	%g1, %cwp
+		be,pt	%xcc, etrap_user_spill
+		 mov	ASI_AIUP, %g3
+
+		rdpr	%otherwin, %g3
+		brz	%g3, etrap_kernel_spill
+		 mov	ASI_AIUS, %g3
+
+etrap_user_spill:
+
+		wr	%g3, 0x0, %asi
+		ldx	[%g6 + TI_FLAGS], %g3
+		and	%g3, _TIF_32BIT, %g3
+		brnz,pt	%g3, etrap_user_spill_32bit
+		 nop
+		ba,a,pt	%xcc, etrap_user_spill_64bit
+
+etrap_save:	save	%g2, -STACK_BIAS, %sp
 		mov	%g6, %l6
 
 		bne,pn	%xcc, 3f
@@ -176,83 +200,5 @@ etraptl1:	/* Save tstate/tpc/tnpc of TL 1-->4 and the tl register itself.
 		ba,pt	%xcc, 1b
 		 andcc	%g1, TSTATE_PRIV, %g0
 
-		.align	64
-		.globl	scetrap
-scetrap:
-		TRAP_LOAD_THREAD_REG(%g6, %g1)
-		rdpr	%pil, %g2
-		rdpr	%tstate, %g1
-		sllx	%g2, 20, %g3
-		andcc	%g1, TSTATE_PRIV, %g0
-		or	%g1, %g3, %g1
-		bne,pn	%xcc, 1f
-		 sub	%sp, (STACKFRAME_SZ+TRACEREG_SZ-STACK_BIAS), %g2
-		wrpr	%g0, 7, %cleanwin
-
-		sllx	%g1, 51, %g3
-		sethi	%hi(TASK_REGOFF), %g2
-		or	%g2, %lo(TASK_REGOFF), %g2
-		brlz,pn	%g3, 1f
-		 add	%g6, %g2, %g2
-		wr	%g0, 0, %fprs
-1:		rdpr	%tpc, %g3
-		stx	%g1, [%g2 + STACKFRAME_SZ + PT_V9_TSTATE]
-
-		rdpr	%tnpc, %g1
-		stx	%g3, [%g2 + STACKFRAME_SZ + PT_V9_TPC]
-		stx	%g1, [%g2 + STACKFRAME_SZ + PT_V9_TNPC]
-		save	%g2, -STACK_BIAS, %sp	! Ordering here is critical
-		mov	%g6, %l6
-		bne,pn	%xcc, 2f
-		 mov	ASI_P, %l7
-		rdpr	%canrestore, %g3
-
-		rdpr	%wstate, %g2
-		wrpr	%g0, 0, %canrestore
-		sll	%g2, 3, %g2
-		mov	PRIMARY_CONTEXT, %l4
-		wrpr	%g3, 0, %otherwin
-		wrpr	%g2, 0, %wstate
-		sethi	%hi(sparc64_kern_pri_context), %g2
-		ldx	[%g2 + %lo(sparc64_kern_pri_context)], %g3
-		stxa	%g3, [%l4] ASI_DMMU
-		sethi	%hi(KERNBASE), %l4
-		flush	%l4
-
-		mov	ASI_AIUS, %l7
-2:		mov	%g4, %l4
-		mov	%g5, %l5
-		add	%g7, 0x4, %l2
-		wrpr	%g0, ETRAP_PSTATE1, %pstate
-		stx	%g1, [%sp + PTREGS_OFF + PT_V9_G1]
-		stx	%g2, [%sp + PTREGS_OFF + PT_V9_G2]
-		sllx	%l7, 24, %l7
-
-		stx	%g3, [%sp + PTREGS_OFF + PT_V9_G3]
-		rdpr	%cwp, %l0
-		stx	%g4, [%sp + PTREGS_OFF + PT_V9_G4]
-		stx	%g5, [%sp + PTREGS_OFF + PT_V9_G5]
-		stx	%g6, [%sp + PTREGS_OFF + PT_V9_G6]
-		stx	%g7, [%sp + PTREGS_OFF + PT_V9_G7]
-		or	%l7, %l0, %l7
-		sethi	%hi(TSTATE_RMO | TSTATE_PEF), %l0
-
-		or	%l7, %l0, %l7
-		wrpr	%l2, %tnpc
-		wrpr	%l7, (TSTATE_PRIV | TSTATE_IE), %tstate
-		stx	%i0, [%sp + PTREGS_OFF + PT_V9_I0]
-		stx	%i1, [%sp + PTREGS_OFF + PT_V9_I1]
-		stx	%i2, [%sp + PTREGS_OFF + PT_V9_I2]
-		stx	%i3, [%sp + PTREGS_OFF + PT_V9_I3]
-		stx	%i4, [%sp + PTREGS_OFF + PT_V9_I4]
-
-		stx	%i5, [%sp + PTREGS_OFF + PT_V9_I5]
-		stx	%i6, [%sp + PTREGS_OFF + PT_V9_I6]
-		mov	%l6, %g6
-		stx	%i7, [%sp + PTREGS_OFF + PT_V9_I7]
-		LOAD_PER_CPU_BASE(%g5, %g6, %g4, %g3, %l1)
-		ldx	[%g6 + TI_TASK], %g4
-		done
-
 #undef TASK_REGOFF
 #undef ETRAP_PSTATE1

arch/sparc64/kernel/process.c

@@ -541,6 +541,18 @@ void synchronize_user_stack(void)
 	}
 }
 
+static void stack_unaligned(unsigned long sp)
+{
+	siginfo_t info;
+
+	info.si_signo = SIGBUS;
+	info.si_errno = 0;
+	info.si_code = BUS_ADRALN;
+	info.si_addr = (void __user *) sp;
+	info.si_trapno = 0;
+	force_sig_info(SIGBUS, &info, current);
+}
+
 void fault_in_user_windows(void)
 {
 	struct thread_info *t = current_thread_info();
@@ -556,13 +568,17 @@ void fault_in_user_windows(void)
 	flush_user_windows();
 	window = get_thread_wsaved();
 
-	if (window != 0) {
+	if (likely(window != 0)) {
 		window -= 1;
 		do {
 			unsigned long sp = (t->rwbuf_stkptrs[window] + bias);
 			struct reg_window *rwin = &t->reg_window[window];
 
-			if (copy_to_user((char __user *)sp, rwin, winsize))
+			if (unlikely(sp & 0x7UL))
+				stack_unaligned(sp);
+
+			if (unlikely(copy_to_user((char __user *)sp,
+						  rwin, winsize)))
 				goto barf;
 		} while (window--);
 	}

arch/sparc64/kernel/rtrap.S

@@ -267,15 +267,69 @@ rt_continue:	ldx			[%sp + PTREGS_OFF + PT_V9_G1], %g1
 
 		wrpr			%l2, %g0, %canrestore
 		wrpr			%l1, %g0, %wstate
-		wrpr			%g0, %g0, %otherwin
+		brnz,pt			%l2, user_rtt_restore
+		 wrpr			%g0, %g0, %otherwin
+
+		ldx			[%g6 + TI_FLAGS], %g3
+		wr			%g0, ASI_AIUP, %asi
+		rdpr			%cwp, %g1
+		andcc			%g3, _TIF_32BIT, %g0
+		sub			%g1, 1, %g1
+		bne,pt			%xcc, user_rtt_fill_32bit
+		 wrpr			%g1, %cwp
+		ba,a,pt			%xcc, user_rtt_fill_64bit
+
+user_rtt_fill_fixup:
+		rdpr	%cwp, %g1
+		add	%g1, 1, %g1
+		wrpr	%g1, 0x0, %cwp
+
+		rdpr	%wstate, %g2
+		sll	%g2, 3, %g2
+		wrpr	%g2, 0x0, %wstate
+
+		/* We know %canrestore and %otherwin are both zero.  */
+
+		sethi	%hi(sparc64_kern_pri_context), %g2
+		ldx	[%g2 + %lo(sparc64_kern_pri_context)], %g2
+		mov	PRIMARY_CONTEXT, %g1
+		stxa	%g2, [%g1] ASI_DMMU
+		sethi	%hi(KERNBASE), %g1
+		flush	%g1
+
+		or	%g4, FAULT_CODE_WINFIXUP, %g4
+		stb	%g4, [%g6 + TI_FAULT_CODE]
+		stx	%g5, [%g6 + TI_FAULT_ADDR]
+
+		mov	%g6, %l1
+		wrpr	%g0, 0x0, %tl
+		wrpr	%g0, RTRAP_PSTATE, %pstate
+		mov	%l1, %g6
+		ldx	[%g6 + TI_TASK], %g4
+		LOAD_PER_CPU_BASE(%g5, %g6, %g1, %g2, %g3)
+		call	do_sparc64_fault
+		 add	%sp, PTREGS_OFF, %o0
+		ba,pt	%xcc, rtrap
+		 nop
+
+user_rtt_pre_restore:
+		add			%g1, 1, %g1
+		wrpr			%g1, 0x0, %cwp
+
+user_rtt_restore:
 		restore
 		rdpr			%canrestore, %g1
 		wrpr			%g1, 0x0, %cleanwin
 		retry
 		nop
 
-kern_rtt:	restore
+kern_rtt:	rdpr			%canrestore, %g1
+		brz,pn			%g1, kern_rtt_fill
+		 nop
+kern_rtt_restore:
+		restore
 		retry
+
 to_kernel:
 #ifdef CONFIG_PREEMPT
 		ldsw			[%g6 + TI_PRE_COUNT], %l5

arch/sparc64/kernel/tsb.S

@@ -115,7 +115,6 @@ sparc64_realfault_common:
 	ba,pt	%xcc, rtrap_clr_l6		! Restore cpu state
 	 nop					! Delay slot (fill me)
 
-	.globl	winfix_trampoline
 winfix_trampoline:
 	rdpr	%tpc, %g3			! Prepare winfixup TNPC
 	or	%g3, 0x7c, %g3			! Compute branch offset

arch/sparc64/kernel/ttable.S

@@ -92,11 +92,11 @@ tl0_resv07c:	BTRAP(0x7c) BTRAP(0x7d) BTRAP(0x7e) BTRAP(0x7f)
 tl0_s0n:	SPILL_0_NORMAL
 tl0_s1n:	SPILL_1_NORMAL
 tl0_s2n:	SPILL_2_NORMAL
-tl0_s3n:	SPILL_3_NORMAL
-tl0_s4n:	SPILL_4_NORMAL
-tl0_s5n:	SPILL_5_NORMAL
-tl0_s6n:	SPILL_6_NORMAL
-tl0_s7n:	SPILL_7_NORMAL
+tl0_s3n:	SPILL_0_NORMAL_ETRAP
+tl0_s4n:	SPILL_1_GENERIC_ETRAP
+tl0_s5n:	SPILL_1_GENERIC_ETRAP_FIXUP
+tl0_s6n:	SPILL_2_GENERIC_ETRAP
+tl0_s7n:	SPILL_2_GENERIC_ETRAP_FIXUP
 tl0_s0o:	SPILL_0_OTHER
 tl0_s1o:	SPILL_1_OTHER
 tl0_s2o:	SPILL_2_OTHER
@@ -110,9 +110,9 @@ tl0_f1n:	FILL_1_NORMAL
 tl0_f2n:	FILL_2_NORMAL
 tl0_f3n:	FILL_3_NORMAL
 tl0_f4n:	FILL_4_NORMAL
-tl0_f5n:	FILL_5_NORMAL
-tl0_f6n:	FILL_6_NORMAL
-tl0_f7n:	FILL_7_NORMAL
+tl0_f5n:	FILL_0_NORMAL_RTRAP
+tl0_f6n:	FILL_1_GENERIC_RTRAP
+tl0_f7n:	FILL_2_GENERIC_RTRAP
 tl0_f0o:	FILL_0_OTHER
 tl0_f1o:	FILL_1_OTHER
 tl0_f2o:	FILL_2_OTHER

arch/sparc64/kernel/winfixup.S

@@ -1,8 +1,6 @@
-/* $Id: winfixup.S,v 1.30 2002/02/09 19:49:30 davem Exp $
+/* winfixup.S: Handle cases where user stack pointer is found to be bogus.
  *
- * winfixup.S: Handle cases where user stack pointer is found to be bogus.
- *
- * Copyright (C) 1997 David S. Miller (davem@caip.rutgers.edu)
+ * Copyright (C) 1997, 2006 David S. Miller (davem@davemloft.net)
  */
 
 #include <asm/asi.h>
@@ -15,367 +13,129 @@
 
 	.text
 
-set_pcontext:
-	sethi	%hi(sparc64_kern_pri_context), %l1
-	ldx	[%l1 + %lo(sparc64_kern_pri_context)], %l1
-	mov	PRIMARY_CONTEXT, %g1
-	stxa	%l1, [%g1] ASI_DMMU
-	sethi	%hi(KERNBASE), %l1
-	flush	%l1
-	retl
-	 nop
+	/* It used to be the case that these register window fault
+	 * handlers could run via the save and restore instructions
+	 * done by the trap entry and exit code.  They now do the
+	 * window spill/fill by hand, so that case no longer can occur.
+	 */
 
 	.align	32
-
-	/* Here are the rules, pay attention.
-	 *
-	 * The kernel is disallowed from touching user space while
-	 * the trap level is greater than zero, except for from within
-	 * the window spill/fill handlers.  This must be followed
-	 * so that we can easily detect the case where we tried to
-	 * spill/fill with a bogus (or unmapped) user stack pointer.
-	 *
-	 * These are layed out in a special way for cache reasons,
-	 * don't touch...
-	 */
-	.globl	fill_fixup, spill_fixup
 fill_fixup:
 	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	rdpr		%tstate, %g1
-	andcc		%g1, TSTATE_PRIV, %g0
-	or		%g4, FAULT_CODE_WINFIXUP, %g4
-	be,pt		%xcc, window_scheisse_from_user_common
-	 and		%g1, TSTATE_CWP, %g1
-
-	/* This is the extremely complex case, but it does happen from
-	 * time to time if things are just right.  Essentially the restore
-	 * done in rtrap right before going back to user mode, with tl=1
-	 * and that levels trap stack registers all setup, took a fill trap,
-	 * the user stack was not mapped in the tlb, and tlb miss occurred,
-	 * the pte found was not valid, and a simple ref bit watch update
-	 * could not satisfy the miss, so we got here.
-	 *
-	 * We must carefully unwind the state so we get back to tl=0, preserve
-	 * all the register values we were going to give to the user.  Luckily
-	 * most things are where they need to be, we also have the address
-	 * which triggered the fault handy as well.
-	 *
-	 * Also note that we must preserve %l5 and %l6.  If the user was
-	 * returning from a system call, we must make it look this way
-	 * after we process the fill fault on the users stack.
-	 *
-	 * First, get into the window where the original restore was executed.
-	 */
-
-	rdpr		%wstate, %g2			! Grab user mode wstate.
-	wrpr		%g1, %cwp			! Get into the right window.
-	sll		%g2, 3, %g2			! NORMAL-->OTHER
-
-	wrpr		%g0, 0x0, %canrestore		! Standard etrap stuff.
-	wrpr		%g2, 0x0, %wstate		! This must be consistent.
-	wrpr		%g0, 0x0, %otherwin		! We know this.
-	call		set_pcontext			! Change contexts...
+	rdpr	%tstate, %g1
+	and	%g1, TSTATE_CWP, %g1
+	or	%g4, FAULT_CODE_WINFIXUP, %g4
+	stb	%g4, [%g6 + TI_FAULT_CODE]
+	stx	%g5, [%g6 + TI_FAULT_ADDR]
+	wrpr	%g1, %cwp
+	ba,pt	%xcc, etrap
+	 rd	%pc, %g7
+	call	do_sparc64_fault
+	 add	%sp, PTREGS_OFF, %o0
+	ba,pt	%xcc, rtrap_clr_l6
 	 nop
-	rdpr		%pstate, %l1			! Prepare to change globals.
-	mov		%g6, %o7			! Get current.
-
-	andn		%l1, PSTATE_MM, %l1		! We want to be in RMO
-	stb		%g4, [%g6 + TI_FAULT_CODE]
-	stx		%g5, [%g6 + TI_FAULT_ADDR]
-	wrpr		%g0, 0x0, %tl			! Out of trap levels.
-	wrpr		%l1, (PSTATE_IE | PSTATE_AG | PSTATE_RMO), %pstate
-	mov		%o7, %g6
-	ldx		[%g6 + TI_TASK], %g4
-	LOAD_PER_CPU_BASE(%g5, %g6, %g1, %g2, %g3)
 
-	/* This is the same as below, except we handle this a bit special
-	 * since we must preserve %l5 and %l6, see comment above.
-	 */
-	call		do_sparc64_fault
-	 add		%sp, PTREGS_OFF, %o0
-	ba,pt		%xcc, rtrap
-	 nop						! yes, nop is correct
-
-	/* Be very careful about usage of the alternate globals here.
-	 * You cannot touch %g4/%g5 as that has the fault information
-	 * should this be from usermode.  Also be careful for the case
-	 * where we get here from the save instruction in etrap.S when
-	 * coming from either user or kernel (does not matter which, it
-	 * is the same problem in both cases).  Essentially this means
-	 * do not touch %g7 or %g2 so we handle the two cases fine.
+	/* Be very careful about usage of the trap globals here.
+	 * You cannot touch %g5 as that has the fault information.
 	 */
 spill_fixup:
+spill_fixup_mna:
+spill_fixup_dax:
 	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	ldx		[%g6 + TI_FLAGS], %g1
-	andcc		%g1, _TIF_32BIT, %g0
-	ldub		[%g6 + TI_WSAVED], %g1
-
-	sll		%g1, 3, %g3
-	add		%g6, %g3, %g3
-	stx		%sp, [%g3 + TI_RWIN_SPTRS]
-	sll		%g1, 7, %g3
-	bne,pt		%xcc, 1f
-	 add		%g6, %g3, %g3
-	stx		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-	stx		%l1, [%g3 + TI_REG_WINDOW + 0x08]
-
-	stx		%l2, [%g3 + TI_REG_WINDOW + 0x10]
-	stx		%l3, [%g3 + TI_REG_WINDOW + 0x18]
-	stx		%l4, [%g3 + TI_REG_WINDOW + 0x20]
-	stx		%l5, [%g3 + TI_REG_WINDOW + 0x28]
-	stx		%l6, [%g3 + TI_REG_WINDOW + 0x30]
-	stx		%l7, [%g3 + TI_REG_WINDOW + 0x38]
-	stx		%i0, [%g3 + TI_REG_WINDOW + 0x40]
-	stx		%i1, [%g3 + TI_REG_WINDOW + 0x48]
-
-	stx		%i2, [%g3 + TI_REG_WINDOW + 0x50]
-	stx		%i3, [%g3 + TI_REG_WINDOW + 0x58]
-	stx		%i4, [%g3 + TI_REG_WINDOW + 0x60]
-	stx		%i5, [%g3 + TI_REG_WINDOW + 0x68]
-	stx		%i6, [%g3 + TI_REG_WINDOW + 0x70]
-	b,pt		%xcc, 2f
-	 stx		%i7, [%g3 + TI_REG_WINDOW + 0x78]
-1:	stw		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-
-	stw		%l1, [%g3 + TI_REG_WINDOW + 0x04]
-	stw		%l2, [%g3 + TI_REG_WINDOW + 0x08]
-	stw		%l3, [%g3 + TI_REG_WINDOW + 0x0c]
-	stw		%l4, [%g3 + TI_REG_WINDOW + 0x10]
-	stw		%l5, [%g3 + TI_REG_WINDOW + 0x14]
-	stw		%l6, [%g3 + TI_REG_WINDOW + 0x18]
-	stw		%l7, [%g3 + TI_REG_WINDOW + 0x1c]
-	stw		%i0, [%g3 + TI_REG_WINDOW + 0x20]
-
-	stw		%i1, [%g3 + TI_REG_WINDOW + 0x24]
-	stw		%i2, [%g3 + TI_REG_WINDOW + 0x28]
-	stw		%i3, [%g3 + TI_REG_WINDOW + 0x2c]
-	stw		%i4, [%g3 + TI_REG_WINDOW + 0x30]
-	stw		%i5, [%g3 + TI_REG_WINDOW + 0x34]
-	stw		%i6, [%g3 + TI_REG_WINDOW + 0x38]
-	stw		%i7, [%g3 + TI_REG_WINDOW + 0x3c]
-2:	add		%g1, 1, %g1
-
-	stb		%g1, [%g6 + TI_WSAVED]
-	rdpr		%tstate, %g1
-	andcc		%g1, TSTATE_PRIV, %g0
+	ldx	[%g6 + TI_FLAGS], %g1
+	andcc	%g1, _TIF_32BIT, %g0
+	ldub	[%g6 + TI_WSAVED], %g1
+	sll	%g1, 3, %g3
+	add	%g6, %g3, %g3
+	stx	%sp, [%g3 + TI_RWIN_SPTRS]
+	sll	%g1, 7, %g3
+	bne,pt	%xcc, 1f
+	 add	%g6, %g3, %g3
+	stx	%l0, [%g3 + TI_REG_WINDOW + 0x00]
+	stx	%l1, [%g3 + TI_REG_WINDOW + 0x08]
+	stx	%l2, [%g3 + TI_REG_WINDOW + 0x10]
+	stx	%l3, [%g3 + TI_REG_WINDOW + 0x18]
+	stx	%l4, [%g3 + TI_REG_WINDOW + 0x20]
+	stx	%l5, [%g3 + TI_REG_WINDOW + 0x28]
+	stx	%l6, [%g3 + TI_REG_WINDOW + 0x30]
+	stx	%l7, [%g3 + TI_REG_WINDOW + 0x38]
+	stx	%i0, [%g3 + TI_REG_WINDOW + 0x40]
+	stx	%i1, [%g3 + TI_REG_WINDOW + 0x48]
+	stx	%i2, [%g3 + TI_REG_WINDOW + 0x50]
+	stx	%i3, [%g3 + TI_REG_WINDOW + 0x58]
+	stx	%i4, [%g3 + TI_REG_WINDOW + 0x60]
+	stx	%i5, [%g3 + TI_REG_WINDOW + 0x68]
+	stx	%i6, [%g3 + TI_REG_WINDOW + 0x70]
+	ba,pt	%xcc, 2f
+	 stx	%i7, [%g3 + TI_REG_WINDOW + 0x78]
+1:	stw	%l0, [%g3 + TI_REG_WINDOW + 0x00]
+	stw	%l1, [%g3 + TI_REG_WINDOW + 0x04]
+	stw	%l2, [%g3 + TI_REG_WINDOW + 0x08]
+	stw	%l3, [%g3 + TI_REG_WINDOW + 0x0c]
+	stw	%l4, [%g3 + TI_REG_WINDOW + 0x10]
+	stw	%l5, [%g3 + TI_REG_WINDOW + 0x14]
+	stw	%l6, [%g3 + TI_REG_WINDOW + 0x18]
+	stw	%l7, [%g3 + TI_REG_WINDOW + 0x1c]
+	stw	%i0, [%g3 + TI_REG_WINDOW + 0x20]
+	stw	%i1, [%g3 + TI_REG_WINDOW + 0x24]
+	stw	%i2, [%g3 + TI_REG_WINDOW + 0x28]
+	stw	%i3, [%g3 + TI_REG_WINDOW + 0x2c]
+	stw	%i4, [%g3 + TI_REG_WINDOW + 0x30]
+	stw	%i5, [%g3 + TI_REG_WINDOW + 0x34]
+	stw	%i6, [%g3 + TI_REG_WINDOW + 0x38]
+	stw	%i7, [%g3 + TI_REG_WINDOW + 0x3c]
+2:	add	%g1, 1, %g1
+	stb	%g1, [%g6 + TI_WSAVED]
+	rdpr	%tstate, %g1
+	andcc	%g1, TSTATE_PRIV, %g0
 	saved
-	and		%g1, TSTATE_CWP, %g1
-	be,a,pn		%xcc, window_scheisse_from_user_common
-	 mov		FAULT_CODE_WRITE | FAULT_CODE_DTLB | FAULT_CODE_WINFIXUP, %g4
+	be,pn	%xcc, 1f
+	 and	%g1, TSTATE_CWP, %g1
 	retry
+1:	mov	FAULT_CODE_WRITE | FAULT_CODE_DTLB | FAULT_CODE_WINFIXUP, %g4
+	stb	%g4, [%g6 + TI_FAULT_CODE]
+	stx	%g5, [%g6 + TI_FAULT_ADDR]
+	wrpr	%g1, %cwp
+	ba,pt	%xcc, etrap
+	 rd	%pc, %g7
+	call	do_sparc64_fault
+	 add	%sp, PTREGS_OFF, %o0
+	ba,a,pt	%xcc, rtrap_clr_l6
 
-window_scheisse_from_user_common:
-	stb		%g4, [%g6 + TI_FAULT_CODE]
-	stx		%g5, [%g6 + TI_FAULT_ADDR]
-	wrpr		%g1, %cwp
-	ba,pt		%xcc, etrap
-	 rd		%pc, %g7
-	call		do_sparc64_fault
-	 add		%sp, PTREGS_OFF, %o0
-	ba,a,pt		%xcc, rtrap_clr_l6
-
-	.globl		winfix_mna, fill_fixup_mna, spill_fixup_mna
 winfix_mna:
-	andn		%g3, 0x7f, %g3
-	add		%g3, 0x78, %g3
-	wrpr		%g3, %tnpc
+	andn	%g3, 0x7f, %g3
+	add	%g3, 0x78, %g3
+	wrpr	%g3, %tnpc
 	done
-fill_fixup_mna:
-	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	rdpr		%tstate, %g1
-	andcc		%g1, TSTATE_PRIV, %g0
-	be,pt		%xcc, window_mna_from_user_common
-	 and		%g1, TSTATE_CWP, %g1
-
-	/* Please, see fill_fixup commentary about why we must preserve
-	 * %l5 and %l6 to preserve absolute correct semantics.
-	 */
-	rdpr		%wstate, %g2			! Grab user mode wstate.
-	wrpr		%g1, %cwp			! Get into the right window.
-	sll		%g2, 3, %g2			! NORMAL-->OTHER
-	wrpr		%g0, 0x0, %canrestore		! Standard etrap stuff.
-
-	wrpr		%g2, 0x0, %wstate		! This must be consistent.
-	wrpr		%g0, 0x0, %otherwin		! We know this.
-	call		set_pcontext			! Change contexts...
-	 nop
-	rdpr		%pstate, %l1			! Prepare to change globals.
-	mov		%g4, %o2			! Setup args for
-	mov		%g5, %o1			! final call to mem_address_unaligned.
-	andn		%l1, PSTATE_MM, %l1		! We want to be in RMO
-
-	mov		%g6, %o7			! Stash away current.
-	wrpr		%g0, 0x0, %tl			! Out of trap levels.
-	wrpr		%l1, (PSTATE_IE | PSTATE_AG | PSTATE_RMO), %pstate
-	mov		%o7, %g6			! Get current back.
-	ldx		[%g6 + TI_TASK], %g4		! Finish it.
-	LOAD_PER_CPU_BASE(%g5, %g6, %g1, %g2, %g3)
-	call		mem_address_unaligned
-	 add		%sp, PTREGS_OFF, %o0
 
-	b,pt		%xcc, rtrap
-	 nop						! yes, the nop is correct
-spill_fixup_mna:
+fill_fixup_mna:
 	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	ldx		[%g6 + TI_FLAGS], %g1
-	andcc		%g1, _TIF_32BIT, %g0
-	ldub		[%g6 + TI_WSAVED], %g1
-	sll		%g1, 3, %g3
-	add		%g6, %g3, %g3
-	stx		%sp, [%g3 + TI_RWIN_SPTRS]
-
-	sll		%g1, 7, %g3
-	bne,pt		%xcc, 1f
-	 add		%g6, %g3, %g3
-	stx		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-	stx		%l1, [%g3 + TI_REG_WINDOW + 0x08]
-	stx		%l2, [%g3 + TI_REG_WINDOW + 0x10]
-	stx		%l3, [%g3 + TI_REG_WINDOW + 0x18]
-	stx		%l4, [%g3 + TI_REG_WINDOW + 0x20]
-
-	stx		%l5, [%g3 + TI_REG_WINDOW + 0x28]
-	stx		%l6, [%g3 + TI_REG_WINDOW + 0x30]
-	stx		%l7, [%g3 + TI_REG_WINDOW + 0x38]
-	stx		%i0, [%g3 + TI_REG_WINDOW + 0x40]
-	stx		%i1, [%g3 + TI_REG_WINDOW + 0x48]
-	stx		%i2, [%g3 + TI_REG_WINDOW + 0x50]
-	stx		%i3, [%g3 + TI_REG_WINDOW + 0x58]
-	stx		%i4, [%g3 + TI_REG_WINDOW + 0x60]
-
-	stx		%i5, [%g3 + TI_REG_WINDOW + 0x68]
-	stx		%i6, [%g3 + TI_REG_WINDOW + 0x70]
-	stx		%i7, [%g3 + TI_REG_WINDOW + 0x78]
-	b,pt		%xcc, 2f
-	 add		%g1, 1, %g1
-1:	std		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-	std		%l2, [%g3 + TI_REG_WINDOW + 0x08]
-	std		%l4, [%g3 + TI_REG_WINDOW + 0x10]
+	rdpr	%tstate, %g1
+	and	%g1, TSTATE_CWP, %g1
+	wrpr	%g1, %cwp
+	ba,pt	%xcc, etrap
+	 rd	%pc, %g7
+	mov	%l4, %o2
+	mov	%l5, %o1
+	call	mem_address_unaligned
+	 add	%sp, PTREGS_OFF, %o0
+	ba,a,pt	%xcc, rtrap_clr_l6
 
-	std		%l6, [%g3 + TI_REG_WINDOW + 0x18]
-	std		%i0, [%g3 + TI_REG_WINDOW + 0x20]
-	std		%i2, [%g3 + TI_REG_WINDOW + 0x28]
-	std		%i4, [%g3 + TI_REG_WINDOW + 0x30]
-	std		%i6, [%g3 + TI_REG_WINDOW + 0x38]
-	add		%g1, 1, %g1
-2:	stb		%g1, [%g6 + TI_WSAVED]
-	rdpr		%tstate, %g1
-
-	andcc		%g1, TSTATE_PRIV, %g0
-	saved
-	be,pn		%xcc, window_mna_from_user_common
-	 and		%g1, TSTATE_CWP, %g1
-	retry
-window_mna_from_user_common:
-	wrpr		%g1, %cwp
-	sethi		%hi(109f), %g7
-	ba,pt		%xcc, etrap
-109:	 or		%g7, %lo(109b), %g7
-	mov		%l4, %o2
-	mov		%l5, %o1
-	call		mem_address_unaligned
-	 add		%sp, PTREGS_OFF, %o0
-	ba,pt		%xcc, rtrap
-	 clr		%l6
-	
-	.globl		winfix_dax, fill_fixup_dax, spill_fixup_dax
 winfix_dax:
-	andn		%g3, 0x7f, %g3
-	add		%g3, 0x74, %g3
-	wrpr		%g3, %tnpc
+	andn	%g3, 0x7f, %g3
+	add	%g3, 0x74, %g3
+	wrpr	%g3, %tnpc
 	done
-fill_fixup_dax:
-	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	rdpr		%tstate, %g1
-	andcc		%g1, TSTATE_PRIV, %g0
-	be,pt		%xcc, window_dax_from_user_common
-	 and		%g1, TSTATE_CWP, %g1
-
-	/* Please, see fill_fixup commentary about why we must preserve
-	 * %l5 and %l6 to preserve absolute correct semantics.
-	 */
-	rdpr		%wstate, %g2			! Grab user mode wstate.
-	wrpr		%g1, %cwp			! Get into the right window.
-	sll		%g2, 3, %g2			! NORMAL-->OTHER
-	wrpr		%g0, 0x0, %canrestore		! Standard etrap stuff.
-
-	wrpr		%g2, 0x0, %wstate		! This must be consistent.
-	wrpr		%g0, 0x0, %otherwin		! We know this.
-	call		set_pcontext			! Change contexts...
-	 nop
-	rdpr		%pstate, %l1			! Prepare to change globals.
-	mov		%g4, %o1			! Setup args for
-	mov		%g5, %o2			! final call to spitfire_data_access_exception.
-	andn		%l1, PSTATE_MM, %l1		! We want to be in RMO
-
-	mov		%g6, %o7			! Stash away current.
-	wrpr		%g0, 0x0, %tl			! Out of trap levels.
-	wrpr		%l1, (PSTATE_IE | PSTATE_AG | PSTATE_RMO), %pstate
-	mov		%o7, %g6			! Get current back.
-	ldx		[%g6 + TI_TASK], %g4		! Finish it.
-	LOAD_PER_CPU_BASE(%g5, %g6, %g1, %g2, %g3)
-	call		spitfire_data_access_exception
-	 add		%sp, PTREGS_OFF, %o0
 
-	b,pt		%xcc, rtrap
-	 nop						! yes, the nop is correct
-spill_fixup_dax:
+fill_fixup_dax:
 	TRAP_LOAD_THREAD_REG(%g6, %g1)
-	ldx		[%g6 + TI_FLAGS], %g1
-	andcc		%g1, _TIF_32BIT, %g0
-	ldub		[%g6 + TI_WSAVED], %g1
-	sll		%g1, 3, %g3
-	add		%g6, %g3, %g3
-	stx		%sp, [%g3 + TI_RWIN_SPTRS]
-
-	sll		%g1, 7, %g3
-	bne,pt		%xcc, 1f
-	 add		%g6, %g3, %g3
-	stx		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-	stx		%l1, [%g3 + TI_REG_WINDOW + 0x08]
-	stx		%l2, [%g3 + TI_REG_WINDOW + 0x10]
-	stx		%l3, [%g3 + TI_REG_WINDOW + 0x18]
-	stx		%l4, [%g3 + TI_REG_WINDOW + 0x20]
-
-	stx		%l5, [%g3 + TI_REG_WINDOW + 0x28]
-	stx		%l6, [%g3 + TI_REG_WINDOW + 0x30]
-	stx		%l7, [%g3 + TI_REG_WINDOW + 0x38]
-	stx		%i0, [%g3 + TI_REG_WINDOW + 0x40]
-	stx		%i1, [%g3 + TI_REG_WINDOW + 0x48]
-	stx		%i2, [%g3 + TI_REG_WINDOW + 0x50]
-	stx		%i3, [%g3 + TI_REG_WINDOW + 0x58]
-	stx		%i4, [%g3 + TI_REG_WINDOW + 0x60]
-
-	stx		%i5, [%g3 + TI_REG_WINDOW + 0x68]
-	stx		%i6, [%g3 + TI_REG_WINDOW + 0x70]
-	stx		%i7, [%g3 + TI_REG_WINDOW + 0x78]
-	b,pt		%xcc, 2f
-	 add		%g1, 1, %g1
-1:	std		%l0, [%g3 + TI_REG_WINDOW + 0x00]
-	std		%l2, [%g3 + TI_REG_WINDOW + 0x08]
-	std		%l4, [%g3 + TI_REG_WINDOW + 0x10]
-
-	std		%l6, [%g3 + TI_REG_WINDOW + 0x18]
-	std		%i0, [%g3 + TI_REG_WINDOW + 0x20]
-	std		%i2, [%g3 + TI_REG_WINDOW + 0x28]
-	std		%i4, [%g3 + TI_REG_WINDOW + 0x30]
-	std		%i6, [%g3 + TI_REG_WINDOW + 0x38]
-	add		%g1, 1, %g1
-2:	stb		%g1, [%g6 + TI_WSAVED]
-	rdpr		%tstate, %g1
-
-	andcc		%g1, TSTATE_PRIV, %g0
-	saved
-	be,pn		%xcc, window_dax_from_user_common
-	 and		%g1, TSTATE_CWP, %g1
-	retry
-window_dax_from_user_common:
-	wrpr		%g1, %cwp
-	sethi		%hi(109f), %g7
-	ba,pt		%xcc, etrap
-109:	 or		%g7, %lo(109b), %g7
-	mov		%l4, %o1
-	mov		%l5, %o2
-	call		spitfire_data_access_exception
-	 add		%sp, PTREGS_OFF, %o0
-	ba,pt		%xcc, rtrap
-	 clr		%l6
+	rdpr	%tstate, %g1
+	and	%g1, TSTATE_CWP, %g1
+	wrpr	%g1, %cwp
+	ba,pt	%xcc, etrap
+	 rd	%pc, %g7
+	mov	%l4, %o1
+	mov	%l5, %o2
+	call	spitfire_data_access_exception
+	 add	%sp, PTREGS_OFF, %o0
+	ba,a,pt	%xcc, rtrap_clr_l6

include/asm-sparc64/ttable.h

@@ -93,7 +93,7 @@
 	
 #define SYSCALL_TRAP(routine, systbl)			\
 	sethi	%hi(109f), %g7;				\
-	ba,pt	%xcc, scetrap;				\
+	ba,pt	%xcc, etrap;				\
 109:	 or	%g7, %lo(109b), %g7;			\
 	sethi	%hi(systbl), %l7;			\
 	ba,pt	%xcc, routine;				\
@@ -219,6 +219,31 @@
 	saved; retry; nop; nop; nop; nop; nop; nop;	\
 	nop; nop; nop; nop; nop; nop; nop; nop;
 
+#define SPILL_0_NORMAL_ETRAP				\
+etrap_kernel_spill:					\
+	stx	%l0, [%sp + STACK_BIAS + 0x00];		\
+	stx	%l1, [%sp + STACK_BIAS + 0x08];		\
+	stx	%l2, [%sp + STACK_BIAS + 0x10];		\
+	stx	%l3, [%sp + STACK_BIAS + 0x18];		\
+	stx	%l4, [%sp + STACK_BIAS + 0x20];		\
+	stx	%l5, [%sp + STACK_BIAS + 0x28];		\
+	stx	%l6, [%sp + STACK_BIAS + 0x30];		\
+	stx	%l7, [%sp + STACK_BIAS + 0x38];		\
+	stx	%i0, [%sp + STACK_BIAS + 0x40];		\
+	stx	%i1, [%sp + STACK_BIAS + 0x48];		\
+	stx	%i2, [%sp + STACK_BIAS + 0x50];		\
+	stx	%i3, [%sp + STACK_BIAS + 0x58];		\
+	stx	%i4, [%sp + STACK_BIAS + 0x60];		\
+	stx	%i5, [%sp + STACK_BIAS + 0x68];		\
+	stx	%i6, [%sp + STACK_BIAS + 0x70];		\
+	stx	%i7, [%sp + STACK_BIAS + 0x78];		\
+	saved;						\
+	sub	%g1, 2, %g1;				\
+	ba,pt	%xcc, etrap_save;			\
+	wrpr	%g1, %cwp;				\
+	nop; nop; nop; nop; nop; nop; nop; nop;		\
+	nop; nop; nop; nop;
+
 /* Normal 64bit spill */
 #define SPILL_1_GENERIC(ASI)				\
 	add	%sp, STACK_BIAS + 0x00, %g1;		\
@@ -252,6 +277,67 @@
 	b,a,pt	%xcc, spill_fixup_mna;			\
 	b,a,pt	%xcc, spill_fixup;
 
+#define SPILL_1_GENERIC_ETRAP				\
+etrap_user_spill_64bit:					\
+	stxa	%l0, [%sp + STACK_BIAS + 0x00] %asi;	\
+	stxa	%l1, [%sp + STACK_BIAS + 0x08] %asi;	\
+	stxa	%l2, [%sp + STACK_BIAS + 0x10] %asi;	\
+	stxa	%l3, [%sp + STACK_BIAS + 0x18] %asi;	\
+	stxa	%l4, [%sp + STACK_BIAS + 0x20] %asi;	\
+	stxa	%l5, [%sp + STACK_BIAS + 0x28] %asi;	\
+	stxa	%l6, [%sp + STACK_BIAS + 0x30] %asi;	\
+	stxa	%l7, [%sp + STACK_BIAS + 0x38] %asi;	\
+	stxa	%i0, [%sp + STACK_BIAS + 0x40] %asi;	\
+	stxa	%i1, [%sp + STACK_BIAS + 0x48] %asi;	\
+	stxa	%i2, [%sp + STACK_BIAS + 0x50] %asi;	\
+	stxa	%i3, [%sp + STACK_BIAS + 0x58] %asi;	\
+	stxa	%i4, [%sp + STACK_BIAS + 0x60] %asi;	\
+	stxa	%i5, [%sp + STACK_BIAS + 0x68] %asi;	\
+	stxa	%i6, [%sp + STACK_BIAS + 0x70] %asi;	\
+	stxa	%i7, [%sp + STACK_BIAS + 0x78] %asi;	\
+	saved;						\
+	sub	%g1, 2, %g1;				\
+	ba,pt	%xcc, etrap_save;			\
+	 wrpr	%g1, %cwp;				\
+	nop; nop; nop; nop; nop;			\
+	nop; nop; nop; nop;				\
+	ba,a,pt	%xcc, etrap_spill_fixup_64bit;		\
+	ba,a,pt	%xcc, etrap_spill_fixup_64bit;		\
+	ba,a,pt	%xcc, etrap_spill_fixup_64bit;
+
+#define SPILL_1_GENERIC_ETRAP_FIXUP			\
+etrap_spill_fixup_64bit:				\
+	ldub	[%g6 + TI_WSAVED], %g1;			\
+	sll	%g1, 3, %g3;				\
+	add	%g6, %g3, %g3;				\
+	stx	%sp, [%g3 + TI_RWIN_SPTRS];		\
+	sll	%g1, 7, %g3;				\
+	add	%g6, %g3, %g3;				\
+	stx	%l0, [%g3 + TI_REG_WINDOW + 0x00];	\
+	stx	%l1, [%g3 + TI_REG_WINDOW + 0x08];	\
+	stx	%l2, [%g3 + TI_REG_WINDOW + 0x10];	\
+	stx	%l3, [%g3 + TI_REG_WINDOW + 0x18];	\
+	stx	%l4, [%g3 + TI_REG_WINDOW + 0x20];	\
+	stx	%l5, [%g3 + TI_REG_WINDOW + 0x28];	\
+	stx	%l6, [%g3 + TI_REG_WINDOW + 0x30];	\
+	stx	%l7, [%g3 + TI_REG_WINDOW + 0x38];	\
+	stx	%i0, [%g3 + TI_REG_WINDOW + 0x40];	\
+	stx	%i1, [%g3 + TI_REG_WINDOW + 0x48];	\
+	stx	%i2, [%g3 + TI_REG_WINDOW + 0x50];	\
+	stx	%i3, [%g3 + TI_REG_WINDOW + 0x58];	\
+	stx	%i4, [%g3 + TI_REG_WINDOW + 0x60];	\
+	stx	%i5, [%g3 + TI_REG_WINDOW + 0x68];	\
+	stx	%i6, [%g3 + TI_REG_WINDOW + 0x70];	\
+	stx	%i7, [%g3 + TI_REG_WINDOW + 0x78];	\
+	add	%g1, 1, %g1;				\
+	stb	%g1, [%g6 + TI_WSAVED];			\
+	saved;						\
+	rdpr	%cwp, %g1;				\
+	sub	%g1, 2, %g1;				\
+	ba,pt	%xcc, etrap_save;			\
+	 wrpr	%g1, %cwp;				\
+	nop; nop; nop
+
 /* Normal 32bit spill */
 #define SPILL_2_GENERIC(ASI)				\
 	srl	%sp, 0, %sp;				\
@@ -285,6 +371,68 @@
 	b,a,pt	%xcc, spill_fixup_mna;			\
 	b,a,pt	%xcc, spill_fixup;
 
+#define SPILL_2_GENERIC_ETRAP		\
+etrap_user_spill_32bit:			\
+	srl	%sp, 0, %sp;		\
+	stwa	%l0, [%sp + 0x00] %asi;	\
+	stwa	%l1, [%sp + 0x04] %asi;	\
+	stwa	%l2, [%sp + 0x08] %asi;	\
+	stwa	%l3, [%sp + 0x0c] %asi;	\
+	stwa	%l4, [%sp + 0x10] %asi;	\
+	stwa	%l5, [%sp + 0x14] %asi;	\
+	stwa	%l6, [%sp + 0x18] %asi;	\
+	stwa	%l7, [%sp + 0x1c] %asi;	\
+	stwa	%i0, [%sp + 0x20] %asi;	\
+	stwa	%i1, [%sp + 0x24] %asi;	\
+	stwa	%i2, [%sp + 0x28] %asi;	\
+	stwa	%i3, [%sp + 0x2c] %asi;	\
+	stwa	%i4, [%sp + 0x30] %asi;	\
+	stwa	%i5, [%sp + 0x34] %asi;	\
+	stwa	%i6, [%sp + 0x38] %asi;	\
+	stwa	%i7, [%sp + 0x3c] %asi;	\
+	saved;				\
+	sub	%g1, 2, %g1;		\
+	ba,pt	%xcc, etrap_save;	\
+	 wrpr	%g1, %cwp;		\
+	nop; nop; nop; nop;		\
+	nop; nop; nop; nop;		\
+	ba,a,pt	%xcc, etrap_spill_fixup_32bit; \
+	ba,a,pt	%xcc, etrap_spill_fixup_32bit; \
+	ba,a,pt	%xcc, etrap_spill_fixup_32bit;
+
+#define SPILL_2_GENERIC_ETRAP_FIXUP			\
+etrap_spill_fixup_32bit:				\
+	ldub	[%g6 + TI_WSAVED], %g1;			\
+	sll	%g1, 3, %g3;				\
+	add	%g6, %g3, %g3;				\
+	stx	%sp, [%g3 + TI_RWIN_SPTRS];		\
+	sll	%g1, 7, %g3;				\
+	add	%g6, %g3, %g3;				\
+	stw	%l0, [%g3 + TI_REG_WINDOW + 0x00];	\
+	stw	%l1, [%g3 + TI_REG_WINDOW + 0x04];	\
+	stw	%l2, [%g3 + TI_REG_WINDOW + 0x08];	\
+	stw	%l3, [%g3 + TI_REG_WINDOW + 0x0c];	\
+	stw	%l4, [%g3 + TI_REG_WINDOW + 0x10];	\
+	stw	%l5, [%g3 + TI_REG_WINDOW + 0x14];	\
+	stw	%l6, [%g3 + TI_REG_WINDOW + 0x18];	\
+	stw	%l7, [%g3 + TI_REG_WINDOW + 0x1c];	\
+	stw	%i0, [%g3 + TI_REG_WINDOW + 0x20];	\
+	stw	%i1, [%g3 + TI_REG_WINDOW + 0x24];	\
+	stw	%i2, [%g3 + TI_REG_WINDOW + 0x28];	\
+	stw	%i3, [%g3 + TI_REG_WINDOW + 0x2c];	\
+	stw	%i4, [%g3 + TI_REG_WINDOW + 0x30];	\
+	stw	%i5, [%g3 + TI_REG_WINDOW + 0x34];	\
+	stw	%i6, [%g3 + TI_REG_WINDOW + 0x38];	\
+	stw	%i7, [%g3 + TI_REG_WINDOW + 0x3c];	\
+	add	%g1, 1, %g1;				\
+	stb	%g1, [%g6 + TI_WSAVED];			\
+	saved;						\
+	rdpr	%cwp, %g1;				\
+	sub	%g1, 2, %g1;				\
+	ba,pt	%xcc, etrap_save;			\
+	 wrpr	%g1, %cwp;				\
+	nop; nop; nop
+
 #define SPILL_1_NORMAL SPILL_1_GENERIC(ASI_AIUP)
 #define SPILL_2_NORMAL SPILL_2_GENERIC(ASI_AIUP)
 #define SPILL_3_NORMAL SPILL_0_NORMAL
@@ -323,6 +471,35 @@
 	restored; retry; nop; nop; nop; nop; nop; nop;	\
 	nop; nop; nop; nop; nop; nop; nop; nop;
 
+#define FILL_0_NORMAL_RTRAP				\
+kern_rtt_fill:						\
+	rdpr	%cwp, %g1;				\
+	sub	%g1, 1, %g1;				\
+	wrpr	%g1, %cwp;				\
+	ldx	[%sp + STACK_BIAS + 0x00], %l0;		\
+	ldx	[%sp + STACK_BIAS + 0x08], %l1;		\
+	ldx	[%sp + STACK_BIAS + 0x10], %l2;		\
+	ldx	[%sp + STACK_BIAS + 0x18], %l3;		\
+	ldx	[%sp + STACK_BIAS + 0x20], %l4;		\
+	ldx	[%sp + STACK_BIAS + 0x28], %l5;		\
+	ldx	[%sp + STACK_BIAS + 0x30], %l6;		\
+	ldx	[%sp + STACK_BIAS + 0x38], %l7;		\
+	ldx	[%sp + STACK_BIAS + 0x40], %i0;		\
+	ldx	[%sp + STACK_BIAS + 0x48], %i1;		\
+	ldx	[%sp + STACK_BIAS + 0x50], %i2;		\
+	ldx	[%sp + STACK_BIAS + 0x58], %i3;		\
+	ldx	[%sp + STACK_BIAS + 0x60], %i4;		\
+	ldx	[%sp + STACK_BIAS + 0x68], %i5;		\
+	ldx	[%sp + STACK_BIAS + 0x70], %i6;		\
+	ldx	[%sp + STACK_BIAS + 0x78], %i7;		\
+	restored;					\
+	add	%g1, 1, %g1;				\
+	ba,pt	%xcc, kern_rtt_restore;			\
+	 wrpr	%g1, %cwp;				\
+	nop; nop; nop; nop; nop;			\
+	nop; nop; nop; nop;
+
+
 /* Normal 64bit fill */
 #define FILL_1_GENERIC(ASI)				\
 	add	%sp, STACK_BIAS + 0x00, %g1;		\
@@ -354,6 +531,33 @@
 	b,a,pt	%xcc, fill_fixup_mna;			\
 	b,a,pt	%xcc, fill_fixup;
 
+#define FILL_1_GENERIC_RTRAP				\
+user_rtt_fill_64bit:					\
+	ldxa	[%sp + STACK_BIAS + 0x00] %asi, %l0;	\
+	ldxa	[%sp + STACK_BIAS + 0x08] %asi, %l1;	\
+	ldxa	[%sp + STACK_BIAS + 0x10] %asi, %l2;	\
+	ldxa	[%sp + STACK_BIAS + 0x18] %asi, %l3;	\
+	ldxa	[%sp + STACK_BIAS + 0x20] %asi, %l4;	\
+	ldxa	[%sp + STACK_BIAS + 0x28] %asi, %l5;	\
+	ldxa	[%sp + STACK_BIAS + 0x30] %asi, %l6;	\
+	ldxa	[%sp + STACK_BIAS + 0x38] %asi, %l7;	\
+	ldxa	[%sp + STACK_BIAS + 0x40] %asi, %i0;	\
+	ldxa	[%sp + STACK_BIAS + 0x48] %asi, %i1;	\
+	ldxa	[%sp + STACK_BIAS + 0x50] %asi, %i2;	\
+	ldxa	[%sp + STACK_BIAS + 0x58] %asi, %i3;	\
+	ldxa	[%sp + STACK_BIAS + 0x60] %asi, %i4;	\
+	ldxa	[%sp + STACK_BIAS + 0x68] %asi, %i5;	\
+	ldxa	[%sp + STACK_BIAS + 0x70] %asi, %i6;	\
+	ldxa	[%sp + STACK_BIAS + 0x78] %asi, %i7;	\
+	ba,pt	%xcc, user_rtt_pre_restore;		\
+	 restored;					\
+	nop; nop; nop; nop; nop; nop;			\
+	nop; nop; nop; nop; nop;			\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;		\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;		\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;
+
+
 /* Normal 32bit fill */
 #define FILL_2_GENERIC(ASI)				\
 	srl	%sp, 0, %sp;				\
@@ -385,6 +589,34 @@
 	b,a,pt	%xcc, fill_fixup_mna;			\
 	b,a,pt	%xcc, fill_fixup;
 
+#define FILL_2_GENERIC_RTRAP				\
+user_rtt_fill_32bit:					\
+	srl	%sp, 0, %sp;				\
+	lduwa	[%sp + 0x00] %asi, %l0;			\
+	lduwa	[%sp + 0x04] %asi, %l1;			\
+	lduwa	[%sp + 0x08] %asi, %l2;			\
+	lduwa	[%sp + 0x0c] %asi, %l3;			\
+	lduwa	[%sp + 0x10] %asi, %l4;			\
+	lduwa	[%sp + 0x14] %asi, %l5;			\
+	lduwa	[%sp + 0x18] %asi, %l6;			\
+	lduwa	[%sp + 0x1c] %asi, %l7;			\
+	lduwa	[%sp + 0x20] %asi, %i0;			\
+	lduwa	[%sp + 0x24] %asi, %i1;			\
+	lduwa	[%sp + 0x28] %asi, %i2;			\
+	lduwa	[%sp + 0x2c] %asi, %i3;			\
+	lduwa	[%sp + 0x30] %asi, %i4;			\
+	lduwa	[%sp + 0x34] %asi, %i5;			\
+	lduwa	[%sp + 0x38] %asi, %i6;			\
+	lduwa	[%sp + 0x3c] %asi, %i7;			\
+	ba,pt	%xcc, user_rtt_pre_restore;		\
+	 restored;					\
+	nop; nop; nop; nop; nop;			\
+	nop; nop; nop; nop; nop;			\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;		\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;		\
+	ba,a,pt	%xcc, user_rtt_fill_fixup;
+		
+
 #define FILL_1_NORMAL FILL_1_GENERIC(ASI_AIUP)
 #define FILL_2_NORMAL FILL_2_GENERIC(ASI_AIUP)
 #define FILL_3_NORMAL FILL_0_NORMAL