More sun4v support. GENERIC and RAMDISK kernels will now boot on both

sun4u and sun4v.  GENERIC.MP won't work yet though.

8 files changed, 1593 insertions, 278 deletions

diff --git a/sys/arch/sparc64/conf/GENERIC b/sys/arch/sparc64/conf/GENERIC
index 0a6c33779c1..2c58ae98628 100644
--- a/sys/arch/sparc64/conf/GENERIC
+++ b/sys/arch/sparc64/conf/GENERIC
@@ -1,4 +1,4 @@
-#	$OpenBSD: GENERIC,v 1.197 2008/03/13 23:06:19 kettenis Exp $
+#	$OpenBSD: GENERIC,v 1.198 2008/03/30 12:30:02 kettenis Exp $
 #
 # For further information on compiling OpenBSD kernels, see the config(8)
 # man page.
@@ -13,6 +13,8 @@ machine		sparc64
 include		"../../../conf/GENERIC"
 maxusers	64			# estimated number of users
 
+option		SUN4V
+
 option		PCIVERBOSE
 option		USER_PCICONF		# user-space PCI configuration
 option		APERTURE
@@ -40,10 +42,13 @@ schizo*	at mainbus0
 pci*	at schizo?
 pyro*	at mainbus0
 pci*	at pyro?
+vpci*	at mainbus0
+pci*	at vpci?
 pci*	at ppb?
 ppb*	at pci?
 ebus*	at mainbus0
 ebus*	at pci?
+vbus0	at mainbus0
 
 # PCI Ethernet
 lmc*	at pci?			# Lan Media Corp SSI/T3/HSSI
@@ -290,6 +295,10 @@ prtc0	at mainbus0
 ## Timer chip found on (some) sun4u systems.
 timer*	at mainbus0
 
+# Virtual devices for sun4v systems.
+vcons0	at vbus?
+vrtc0	at vbus?
+
 ## Lance Ethernet - AMD7990
 le*		at sbus?
 ledma*		at sbus?
diff --git a/sys/arch/sparc64/conf/RAMDISK b/sys/arch/sparc64/conf/RAMDISK
index c93413e95d6..4aeb1155772 100644
--- a/sys/arch/sparc64/conf/RAMDISK
+++ b/sys/arch/sparc64/conf/RAMDISK
@@ -1,4 +1,4 @@
-#	$OpenBSD: RAMDISK,v 1.68 2008/03/13 23:06:19 kettenis Exp $
+#	$OpenBSD: RAMDISK,v 1.69 2008/03/30 12:30:02 kettenis Exp $
 
 # Machine architecture; required by config(8)
 machine		sparc64
@@ -9,6 +9,8 @@ maxusers	2
 option		RAMDISK_HOOKS
 option		MINIROOTSIZE=6144
 
+option		SUN4V
+
 option		PCIVERBOSE
 #option		USBVERBOSE
 
@@ -50,10 +52,13 @@ schizo*	at mainbus0
 pci*	at schizo?
 pyro*	at mainbus0
 pci*	at pyro?
+vpci*	at mainbus0
+pci*	at vpci?
 pci*	at ppb?
 ppb*	at pci?
 ebus*	at mainbus0
 ebus*	at pci?
+vbus0	at mainbus0
 
 # PCI Ethernet
 hme*	at pci?			# Sun Happy Meal 10/100
@@ -87,6 +92,10 @@ clock0	at fhc?
 rtc*	at ebus?			# DS1287/M5819 clock
 timer*	at mainbus0			# Timer chip
 
+# Virtual devices for sun4v systems.
+vcons0	at vbus?
+vrtc0	at vbus?
+
 ## Lance Ethernet - AMD7990
 le*		at sbus?
 ledma*		at sbus?
diff --git a/sys/arch/sparc64/include/pte.h b/sys/arch/sparc64/include/pte.h
index 5389bfd01c1..d12e61bff59 100644
--- a/sys/arch/sparc64/include/pte.h
+++ b/sys/arch/sparc64/include/pte.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: pte.h,v 1.9 2008/03/23 21:49:48 kettenis Exp $	*/
+/*	$OpenBSD: pte.h,v 1.10 2008/03/30 12:30:02 kettenis Exp $	*/
 /*	$NetBSD: pte.h,v 1.7 2001/07/31 06:55:46 eeh Exp $ */
 
 /*
@@ -231,54 +231,6 @@ void smp_tlb_flush_ctx(int);
 ((priv)?SUN4V_TLB_P:0LL)|((write)?SUN4V_TLB_W:0LL)|((g)?SUN4V_TLB_G:0LL)|\
 ((ie)?SUN4V_TLB_IE:0LL))
 
-#ifdef SUN4V
-#define TLB_SZ		SUN4V_TLB_SZ
-
-#define TLB_V		SUN4V_TLB_V
-#define TLB_NFO		SUN4V_TLB_NFO
-#define TLB_IE		SUN4V_TLB_IE
-#define TLB_PA_MASK	SUN4V_TLB_PA_MASK
-#define TLB_ACCESS	SUN4V_TLB_ACCESS
-#define TLB_MODIFY	SUN4V_TLB_MODIFY
-#define TLB_REAL_W	SUN4V_TLB_REAL_W
-#define TLB_TSB_LOCK	SUN4V_TLB_TSB_LOCK
-#define TLB_EXEC	SUN4V_TLB_EXEC
-#define TLB_EXEC_ONLY	SUN4V_TLB_EXEC_ONLY
-#define TLB_L		0ULL
-#define TLB_CACHE_MASK	SUN4V_TLB_CACHE_MASK
-#define TLB_CP		SUN4V_TLB_CP
-#define TLB_CV		SUN4V_TLB_CV
-#define TLB_E		SUN4V_TLB_E
-#define TLB_P		SUN4V_TLB_P
-#define TLB_W		SUN4V_TLB_W
-#define TLB_G		SUN4V_TLB_G
-
-#define TSB_DATA	SUN4V_TSB_DATA
-#else
-#define TLB_SZ		SUN4U_TLB_SZ
-
-#define TLB_V		SUN4U_TLB_V
-#define TLB_NFO		SUN4U_TLB_NFO
-#define TLB_IE		SUN4U_TLB_IE
-#define TLB_PA_MASK	SUN4U_TLB_PA_MASK
-#define TLB_ACCESS	SUN4U_TLB_ACCESS
-#define TLB_MODIFY	SUN4U_TLB_MODIFY
-#define TLB_REAL_W	SUN4U_TLB_REAL_W
-#define TLB_TSB_LOCK	SUN4U_TLB_TSB_LOCK
-#define TLB_EXEC	SUN4U_TLB_EXEC
-#define TLB_EXEC_ONLY	SUN4U_TLB_EXEC_ONLY
-#define TLB_L		SUN4U_TLB_L
-#define TLB_CACHE_MASK	SUN4U_TLB_CACHE_MASK
-#define TLB_CP		SUN4U_TLB_CP
-#define TLB_CV		SUN4U_TLB_CV
-#define TLB_E		SUN4U_TLB_E
-#define TLB_P		SUN4U_TLB_P
-#define TLB_W		SUN4U_TLB_W
-#define TLB_G		SUN4U_TLB_G
-
-#define TSB_DATA	SUN4U_TSB_DATA
-#endif
-
 
 #define MMU_CACHE_VIRT	0x3
 #define MMU_CACHE_PHYS	0x2
diff --git a/sys/arch/sparc64/sparc64/autoconf.c b/sys/arch/sparc64/sparc64/autoconf.c
index 86550c9f9c2..ed64488df53 100644
--- a/sys/arch/sparc64/sparc64/autoconf.c
+++ b/sys/arch/sparc64/sparc64/autoconf.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: autoconf.c,v 1.83 2008/03/22 21:10:28 kettenis Exp $	*/
+/*	$OpenBSD: autoconf.c,v 1.84 2008/03/30 12:30:01 kettenis Exp $	*/
 /*	$NetBSD: autoconf.c,v 1.51 2001/07/24 19:32:11 eeh Exp $ */
 
 /*
@@ -67,10 +67,12 @@
 
 #include <machine/bus.h>
 #include <machine/autoconf.h>
+#include <machine/hypervisor.h>
 #include <machine/openfirm.h>
 #include <machine/sparc64.h>
 #include <machine/cpu.h>
 #include <machine/pmap.h>
+#include <sparc64/sparc64/cache.h>
 #include <sparc64/sparc64/timerreg.h>
 
 #include <dev/ata/atavar.h>
@@ -284,6 +286,53 @@ bootstrap(nctx)
 		cputyp = CPU_SUN4V;
 #endif
 
+#ifdef SUN4V
+	if (CPU_ISSUN4V) {
+		extern vaddr_t dlflush_start, ctxid_start;
+		extern vaddr_t gl0_start, gl1_start;
+		vaddr_t *pva;
+		u_int32_t insn;
+		int32_t disp;
+
+		for (pva = &dlflush_start; *pva; pva++) {
+			*(u_int32_t *)(*pva) = 0x01000000; /* nop */
+			flush((void *)(*pva));
+		}
+
+		for (pva = &ctxid_start; *pva; pva++) {
+			insn = *(u_int32_t *)(*pva);
+			insn &= ~(ASI_DMMU << 5);
+			insn |= (ASI_MMU_CONTEXTID << 5);
+			*(u_int32_t *)(*pva) = insn;
+			flush((void *)(*pva));
+		}
+
+		for (pva = &gl0_start; *pva; pva++) {
+			*(u_int32_t *)(*pva) = 0xa1902000; /* wr %g0, 0, %gl */
+			flush((void *)(*pva));
+		}
+
+		for (pva = &gl1_start; *pva; pva++) {
+			*(u_int32_t *)(*pva) = 0xa1902001; /* wr %g0, 1, %gl */
+			flush((void *)(*pva));
+		}
+
+		disp = (vaddr_t)hv_mmu_demap_page - (vaddr_t)sp_tlb_flush_pte;
+		insn = 0x10800000 | disp >> 2;	/* ba hv_mmu_demap_page */
+		((u_int32_t *)sp_tlb_flush_pte)[0] = insn;
+		insn = 0x94102003; 		/* mov MAP_ITLB|MAP_DTLB, %o2 */
+		((u_int32_t *)sp_tlb_flush_pte)[1] = insn;
+
+		disp =  (vaddr_t)hv_mmu_demap_ctx - (vaddr_t)sp_tlb_flush_ctx;
+		insn = 0x10800000 | disp >> 2;	/* ba hv_mmu_demap_ctx */
+		((u_int32_t *)sp_tlb_flush_ctx)[0] = insn;
+		insn = 0x94102003; 		/* mov MAP_ITLB|MAP_DTLB, %o2 */
+		((u_int32_t *)sp_tlb_flush_ctx)[1] = insn;
+
+		cacheinfo.c_dcache_flush_page = no_dcache_flush_page;
+	}
+#endif
+
 	ncpus = get_ncpus();
 	pmap_bootstrap(KERNBASE, (u_long)&end, nctx, ncpus);
 }
diff --git a/sys/arch/sparc64/sparc64/cache.h b/sys/arch/sparc64/sparc64/cache.h
index 5d3b8844495..3e35de1a32c 100644
--- a/sys/arch/sparc64/sparc64/cache.h
+++ b/sys/arch/sparc64/sparc64/cache.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cache.h,v 1.5 2007/01/12 22:09:08 kettenis Exp $	*/
+/*	$OpenBSD: cache.h,v 1.6 2008/03/30 12:30:01 kettenis Exp $	*/
 /*	$NetBSD: cache.h,v 1.3 2000/08/01 00:28:02 eeh Exp $ */
 
 /*
@@ -143,6 +143,7 @@ int	cache_flush(vaddr_t, vsize_t);	/* flush region */
 #define dcache_flush_page(pa) cacheinfo.c_dcache_flush_page(pa)
 void 	us_dcache_flush_page(paddr_t);	/* flush page from D$ */
 void 	us3_dcache_flush_page(paddr_t);	/* flush page from D$ */
+void	no_dcache_flush_page(paddr_t);
 
 /* The following flush a range from the D$ and I$ but not E$. */
 void	cache_flush_virt(vaddr_t, vsize_t);
diff --git a/sys/arch/sparc64/sparc64/genassym.cf b/sys/arch/sparc64/sparc64/genassym.cf
index 64eca8fa4a4..57d47b282b8 100644
--- a/sys/arch/sparc64/sparc64/genassym.cf
+++ b/sys/arch/sparc64/sparc64/genassym.cf
@@ -1,4 +1,4 @@
-#	$OpenBSD: genassym.cf,v 1.29 2008/03/23 21:49:48 kettenis Exp $
+#	$OpenBSD: genassym.cf,v 1.30 2008/03/30 12:30:01 kettenis Exp $
 #	$NetBSD: genassym.cf,v 1.23 2001/08/08 00:09:30 eeh Exp $
 
 #
@@ -157,6 +157,13 @@ member	ci_intrpending
 member	ci_spinup
 member	ci_initstack
 member	ci_paddr
+ifdef SUN4V
+member	ci_rw
+member	ci_rwsp
+member	ci_mmfsa
+member	ci_cpumq
+member	ci_devmq
+endif
 
 # FPU state
 struct	fpstate64
@@ -271,6 +278,12 @@ export	SUN4U_TLB_REAL_W
 export	SUN4U_TLB_EXEC
 export	SUN4U_TLB_W
 
+export	SUN4V_TLB_ACCESS
+export	SUN4V_TLB_MODIFY
+export	SUN4V_TLB_REAL_W
+export	SUN4V_TLB_EXEC
+export	SUN4V_TLB_W
+
 struct	mutex
 member	mtx_wantipl
 member	mtx_oldipl
diff --git a/sys/arch/sparc64/sparc64/locore.s b/sys/arch/sparc64/sparc64/locore.s
index 070fe349299..92d21c43192 100644
--- a/sys/arch/sparc64/sparc64/locore.s
+++ b/sys/arch/sparc64/sparc64/locore.s
@@ -1,4 +1,4 @@
-/*	$OpenBSD: locore.s,v 1.126 2008/03/23 21:49:48 kettenis Exp $	*/
+/*	$OpenBSD: locore.s,v 1.127 2008/03/30 12:30:01 kettenis Exp $	*/
 /*	$NetBSD: locore.s,v 1.137 2001/08/13 06:10:10 jdolecek Exp $	*/
 
 /*
@@ -107,6 +107,18 @@
 	GET_CPUINFO_VA(curproc)			;\
 	ldx	[curproc + CI_CURPROC], curproc
 
+#ifdef SUN4V
+
+#define GET_CPUINFO_PA(ci) \
+	nop					;\
+	ldxa	[%g0] ASI_SCRATCHPAD, ci
+
+#define GET_MMFSA(mmfsa) \
+	GET_CPUINFO_VA(mmfsa)			;\
+	ldx	[mmfsa + CI_MMFSA], mmfsa
+
+#endif
+
 /*
  * This macro will clear out a cache line before an explicit
  * access to that location.  It's mostly used to make certain
@@ -626,6 +638,48 @@ _C_LABEL(cold):
 	TA32
 	.endm
 
+#ifdef SUN4V
+
+	.macro	sun4v_tl0_reserved	count
+	.rept	\count
+	ba,a,pt	%xcc, slowtrap
+	 nop
+	.align	32
+	.endr
+	.endm
+
+#define sun4v_tl0_unused sun4v_tl0_reserved
+
+	.macro	sun4v_tl1_reserved	count
+	.rept	\count
+	ba,a,pt	%xcc, slowtrap
+	 nop
+	.align	32
+	.endr
+	.endm
+
+#define sun4v_tl1_unused sun4v_tl1_reserved
+
+	.macro sun4v_tl1_kspill_normal
+	ba,a,pt	%xcc,kspill_normal
+	 nop
+	.align 128
+	.endm
+
+	.macro sun4v_tl1_uspill_normal
+	ba,a,pt	%xcc,pcbspill_normals
+	 nop
+	.align 128
+	.endm
+
+	.macro sun4v_tl1_uspill_other
+	ba,a,pt	%xcc,pcbspill_others
+	 nop
+	.align 128
+	.endm
+
+#endif
+
 	.globl	start, _C_LABEL(kernel_text)
 	_C_LABEL(kernel_text) = start		! for kvm_mkdb(8)
 start:
@@ -945,6 +999,147 @@ TABLE/**/syscall:
 	UTRAP 0x1f0; UTRAP 0x1f1; UTRAP 0x1f2; UTRAP 0x1f3; UTRAP 0x1f4; UTRAP 0x1f5; UTRAP 0x1f6; UTRAP 0x1f7
 	UTRAP 0x1f8; UTRAP 0x1f9; UTRAP 0x1fa; UTRAP 0x1fb; UTRAP 0x1fc; UTRAP 0x1fd; UTRAP 0x1fe; UTRAP 0x1ff
 
+#ifdef SUN4V
+
+	.align	0x8000
+	.globl	_C_LABEL(trapbase_sun4v)
+_C_LABEL(trapbase_sun4v):
+	sun4v_tl0_reserved 8				! 0x0-0x7
+	VTRAP T_INST_EXCEPT, sun4v_tl0_itsb_miss	! 0x8
+	VTRAP T_TEXTFAULT, sun4v_tl0_itsb_miss		! 0x9
+	sun4v_tl0_reserved 6				! 0xa-0xf
+	TRAP T_ILLINST					! 0x10
+	TRAP T_PRIVINST					! 0x11
+	sun4v_tl0_reserved 14				! 0x12-0x1f
+	TRAP T_FPDISABLED				! 0x20
+	TRAP T_FP_IEEE_754				! 0x21
+	TRAP T_FP_OTHER					! 0x22
+	TRAP T_TAGOF					! 0x23
+	UCLEANWIN					! 0x24-0x27
+	TRAP T_DIV0					! 0x28
+	sun4v_tl0_reserved 7				! 0x29-0x2f
+	VTRAP T_DATAFAULT, sun4v_datatrap		! 0x30
+	VTRAP T_DATA_MMU_MISS, sun4v_tl0_dtsb_miss	! 0x31
+	sun4v_tl0_reserved 2				! 0x32-0x33
+	TRAP T_ALIGN					! 0x34
+	TRAP T_LDDF_ALIGN				! 0x35
+	TRAP T_STDF_ALIGN				! 0x36
+	TRAP T_PRIVACT					! 0x37
+	TRAP T_LDQF_ALIGN				! 0x38
+	TRAP T_STQF_ALIGN				! 0x39
+	sun4v_tl0_reserved 7				! 0x3a-0x40
+	HARDINT4U 1					! 0x41
+	HARDINT4U 2					! 0x42
+	HARDINT4U 3					! 0x43
+	HARDINT4U 4					! 0x44
+	HARDINT4U 5					! 0x45
+	HARDINT4U 6					! 0x46
+	HARDINT4U 7					! 0x47
+	HARDINT4U 8					! 0x48
+	HARDINT4U 9					! 0x49
+	HARDINT4U 10					! 0x4a
+	HARDINT4U 11					! 0x4b
+	HARDINT4U 12					! 0x4c
+	HARDINT4U 13					! 0x4d
+	HARDINT4U 14					! 0x4e
+	HARDINT4U 15					! 0x4f
+	sun4v_tl0_reserved 18				! 0x50-0x61
+	TRAP T_VA_WATCHPT				! 0x62
+	sun4v_tl0_reserved 9				! 0x63-0x6b
+	VTRAP 0x6c, sun4v_tl0_dtsb_prot			! 0x6c
+	sun4v_tl0_reserved 15				! 0x6d-0x7b
+	VTRAP 0x7c, sun4v_cpu_mondo			! 0x7c
+	VTRAP 0x7c, sun4v_dev_mondo			! 0x7d
+	TRAP 0x7e					! 0x7e
+	TRAP 0x7f					! 0x7f
+	USPILL64 uspill8v, ASI_AIUS			! 0x80
+	SPILL32 uspill4v, ASI_AIUS			! 0x84
+	SPILLBOTH uspill8v,uspill4v, ASI_AIUS		! 0x88
+	sun4v_tl0_unused 4				! 0x8c
+	SPILL64 kspill8v, ASI_N				! 0x90
+	SPILL32 kspill4v, ASI_N				! 0x94
+	SPILLBOTH kspill8v, kspill4v, ASI_N		! 0x98
+	sun4v_tl0_unused 4				! 0x9c
+	USPILL64 uspillk8v, ASI_AIUS			! 0xa0
+	SPILL32 uspillk4v, ASI_AIUS			! 0xa4
+	SPILLBOTH uspillk8v, uspillk4v, ASI_AIUS	! 0xa8
+	sun4v_tl0_unused 4				! 0xac
+	sun4v_tl0_unused 16				! 0xb0-0xbc
+	UFILL64 ufill8v, ASI_AIUS			! 0xc0
+	FILL32 ufill4v, ASI_AIUS			! 0xc4
+	FILLBOTH ufill8v, ufill4v, ASI_AIUS		! 0xc8
+	sun4v_tl0_unused 4				! 0xcf
+	FILL64 kfill8v, ASI_N				! 0xd0
+	FILL32 kfill4v, ASI_N				! 0xd4
+	FILLBOTH kfill8v, kfill4v, ASI_N		! 0xd8
+	sun4v_tl0_unused 4				! 0xdf
+	UFILL64 ufillk8v, ASI_AIUS			! 0xe0
+	FILL32 ufillk4v, ASI_AIUS			! 0xe4
+	FILLBOTH ufillk8v, ufillk4v, ASI_AIUS		! 0xe8
+	sun4v_tl0_unused 4				! 0xef
+	sun4v_tl0_unused 16				! 0xf0-0xfc
+	SYSCALL						! 0x100
+	TRAP T_BREAKPOINT				! 0x101
+	sun4v_tl0_unused 6				! 0x102-0x107
+	SYSCALL						! 0x108
+	SYSCALL						! 0x109
+	sun4v_tl0_unused 54				! 0x10a-0x13f
+	SYSCALL						! 0x140
+	SYSCALL						! 0x141
+	SYSCALL						! 0x142
+	SYSCALL						! 0x143
+	sun4v_tl0_unused 60				! 0x144-0x17f
+	sun4v_tl0_reserved 128				! 0x180-0x1ff
+
+	sun4v_tl1_reserved 8				! 0x0-0x7
+	TRAP T_INST_EXCEPT				! 0x8
+	TRAP T_TEXTFAULT				! 0x9
+	sun4v_tl1_reserved 6				! 0xa-0xf
+	TRAP T_ILLINST					! 0x10
+	TRAP T_PRIVINST					! 0x11
+	sun4v_tl1_reserved 14				! 0x12-0x1f
+	TRAP T_FPDISABLED				! 0x20
+	TRAP T_FP_IEEE_754				! 0x21
+	TRAP T_FP_OTHER					! 0x22
+	TRAP T_TAGOF					! 0x23
+	KCLEANWIN					! 0x24-0x27
+	TRAP T_DIV0					! 0x28
+	sun4v_tl1_reserved 7				! 0x29-0x2f
+	VTRAP T_DATAFAULT, sun4v_tl1_ptbl_miss		! 0x30
+	VTRAP T_DATA_MMU_MISS, sun4v_tl1_dtsb_miss	! 0x31
+	VTRAP T_DATA_ERROR, sun4v_tl1_ptbl_miss
+	VTRAP T_DATA_PROT, sun4v_tl1_ptbl_miss
+!	sun4v_tl1_reserved 2				! 0x32-0x33
+	VTRAP T_ALIGN, sun4v_tl1_ptbl_miss		! 0x34
+	TRAP T_LDDF_ALIGN				! 0x35
+	TRAP T_STDF_ALIGN				! 0x36
+	TRAP T_PRIVACT					! 0x37
+	TRAP T_LDQF_ALIGN				! 0x38
+	TRAP T_STQF_ALIGN				! 0x39
+	sun4v_tl1_reserved 40				! 0x3a-0x61
+	TRAP T_VA_WATCHPT				! 0x62
+	sun4v_tl1_reserved 9				! 0x63-0x6b
+	VTRAP 0x6c, sun4v_tl1_dtsb_prot			! 0x6c
+	sun4v_tl1_reserved 19				! 0x6d-0x7f
+	sun4v_tl1_uspill_normal				! 0x80
+	sun4v_tl1_uspill_normal				! 0x84
+	sun4v_tl1_uspill_normal				! 0x88
+	sun4v_tl1_unused 4				! 0x8c
+	sun4v_tl1_kspill_normal				! 0x90
+	sun4v_tl1_kspill_normal				! 0x94
+	sun4v_tl1_kspill_normal				! 0x98
+	sun4v_tl1_unused 4				! 0x9c
+	sun4v_tl1_uspill_other				! 0xa0
+	sun4v_tl1_uspill_other				! 0xa4
+	sun4v_tl1_uspill_other				! 0xa8
+	sun4v_tl1_unused 4				! 0xac
+	sun4v_tl1_unused 16				! 0xb0-0xbc
+	sun4v_tl1_unused 64				! 0xc0-0xfc
+	sun4v_tl1_unused 128
+	sun4v_tl1_reserved 128
+
+#endif
+
 #ifdef DEBUG
 	.macro CHKREG r
 	ldx	[%o0 + 8*1], %o1
@@ -1231,7 +1426,7 @@ intr_setup_msg:
 	_ALIGN
 	.text
 
-	.macro	TRAP_SETUP stackspace
+	.macro	TRAP_SETUP stackspace, label
 	GET_CPCB(%g6)
 	sethi	%hi((\stackspace)), %g5
 	sethi	%hi(USPACE), %g7		! Always multiple of page size
@@ -1263,17 +1458,15 @@ intr_setup_msg:
 	stx	%i6, [%sp + CC64FSZ + BIAS + TF_O + (6*8)]
 	brz,pt	%g7, 1f			! If we were in kernel mode start saving globals
 	 stx	%i7, [%sp + CC64FSZ + BIAS + TF_O + (7*8)]
-	mov	CTX_PRIMARY, %g7
 
 	! came from user mode -- switch to kernel mode stack
 	rdpr	%canrestore, %g5		! Fixup register window state registers
-
 	wrpr	%g0, 0, %canrestore
-
 	wrpr	%g0, %g5, %otherwin
-
 	wrpr	%g0, WSTATE_KERN, %wstate	! Enable kernel mode window traps -- now we can trap again
 
+	mov	CTX_PRIMARY, %g7
+\label:
 	stxa	%g0, [%g7] ASI_DMMU		! Switch MMU to kernel primary context
 	sethi	%hi(KERNBASE), %g5
 	membar	#Sync				! XXXX Should be taken care of by flush
@@ -1289,7 +1482,7 @@ intr_setup_msg:
  * We don't guarantee that any registers are preserved during this operation,
  * so we can be more efficient.
  */
-	.macro	INTR_SETUP stackspace
+	.macro	INTR_SETUP stackspace, label
 	rdpr	%wstate, %g7			! Find if we're from user mode
 
 	sethi	%hi(EINTSTACK-BIAS), %g6
@@ -1314,7 +1507,6 @@ intr_setup_msg:
 	add	%g6, %g5, %g5			! Allocate a stack frame
 	btst	1, %g6
 	bnz,pt	%icc, 1f
-
 	 mov	%g5, %g6
 
 	add	%g5, -BIAS, %g6
@@ -1333,84 +1525,24 @@ intr_setup_msg:
 	stx	%i6, [%sp + CC64FSZ + BIAS + TF_G + (0*8)]	! Save fp in clockframe->cf_fp
 	brz,pt	%g3, 1f				! If we were in kernel mode start saving globals
 	 stx	%i7, [%sp + CC64FSZ + BIAS + TF_O + (7*8)]
-	! came from user mode -- switch to kernel mode stack
-	 rdpr	%otherwin, %g5			! Has this already been done?
 
-!	tst %g5; tnz %xcc, 1; nop; ! DEBUG -- this should _NEVER_ happen
-	brnz,pn	%g5, 1f			! Don't set this twice
+	! came from user mode -- switch to kernel mode stack
+	rdpr	%otherwin, %g5			! Has this already been done?
+	brnz,pn	%g5, 1f				! Don't set this twice
 
 	 rdpr	%canrestore, %g5		! Fixup register window state registers
-
 	wrpr	%g0, 0, %canrestore
-
 	wrpr	%g0, %g5, %otherwin
-
-	sethi	%hi(KERNBASE), %g5
-	mov	CTX_PRIMARY, %g7
-
 	wrpr	%g0, WSTATE_KERN, %wstate	! Enable kernel mode window traps -- now we can trap again
 
+	mov	CTX_PRIMARY, %g7
+\label:
 	stxa	%g0, [%g7] ASI_DMMU		! Switch MMU to kernel primary context
+	sethi	%hi(KERNBASE), %g5
 	membar	#Sync				! XXXX Should be taken care of by flush
-
 	flush	%g5				! Some convenient address that won't trap
 1:
 	.endm
-	
-
-#ifdef DEBUG
-
-	/* Look up kpte to test algorithm */
-	.globl	asmptechk
-asmptechk:
-	mov	%o0, %g4	! pmap->pm_segs
-	mov	%o1, %g3	! Addr to lookup -- mind the context
-
-	srax	%g3, HOLESHIFT, %g5			! Check for valid address
-	brz,pt	%g5, 0f					! Should be zero or -1
-	 inc	%g5					! Make -1 -> 0
-	brnz,pn	%g5, 1f					! Error!
-0:
-	 srlx	%g3, STSHIFT, %g5
-	and	%g5, STMASK, %g5
-	sll	%g5, 3, %g5
-	add	%g4, %g5, %g4
-	DLFLUSH %g4,%g5
-	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Remember -- UNSIGNED
-	DLFLUSH2 %g5
-	brz,pn	%g4, 1f					! NULL entry? check somewhere else
-
-	 srlx	%g3, PDSHIFT, %g5
-	and	%g5, PDMASK, %g5
-	sll	%g5, 3, %g5
-	add	%g4, %g5, %g4
-	DLFLUSH %g4,%g5
-	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Remember -- UNSIGNED
-	DLFLUSH2 %g5
-	brz,pn	%g4, 1f					! NULL entry? check somewhere else
-
-	 srlx	%g3, PTSHIFT, %g5			! Convert to ptab offset
-	and	%g5, PTMASK, %g5
-	sll	%g5, 3, %g5
-	add	%g4, %g5, %g4
-	DLFLUSH %g4,%g5
-	ldxa	[%g4] ASI_PHYS_CACHED, %g6
-	DLFLUSH2 %g5
-	brgez,pn %g6, 1f				! Entry invalid?  Punt
-	 srlx	%g6, 32, %o0
-	retl
-	 srl	%g6, 0, %o1
-1:
-	mov	%g0, %o1
-	retl
-	 mov	%g0, %o0
-
-	.data
-2:
-	.asciz	"asmptechk: %x %x %x %x:%x\r\n"
-	_ALIGN
-	.text
-#endif	/* DEBUG */
 
 /*
  * This is the MMU protection handler.  It's too big to fit
@@ -1807,7 +1939,7 @@ winfixfill:
 	stxa	%g0, [SFSR] %asi			! Clear out fault now
 	membar	#Sync					! No real reason for this XXXX
 
-	TRAP_SETUP -CC64FSZ-TF_SIZE
+	TRAP_SETUP -CC64FSZ-TF_SIZE, 99
 	saved						! Blow away that one register window we didn't ever use.
 	ba,a,pt	%icc, Ldatafault_internal		! Now we should return directly to user mode
 	 nop
@@ -2134,7 +2266,7 @@ datafault:
 	stxa	%g0, [SFSR] %asi			! Clear out fault now
 	membar	#Sync					! No real reason for this XXXX
 
-	TRAP_SETUP -CC64FSZ-TF_SIZE
+	TRAP_SETUP -CC64FSZ-TF_SIZE, 99
 Ldatafault_internal:
 	INCR _C_LABEL(uvmexp)+V_FAULTS			! cnt.v_faults++ (clobbers %o0,%o1,%o2) should not fault
 !	ldx	[%sp + CC64FSZ + BIAS + TF_FAULT], %g1		! DEBUG make sure this has not changed
@@ -2376,7 +2508,7 @@ textfault:
 	stxa	%g0, [SFSR] %asi			! Clear out old info
 	membar	#Sync					! No real reason for this XXXX
 
-	TRAP_SETUP -CC64FSZ-TF_SIZE
+	TRAP_SETUP -CC64FSZ-TF_SIZE, 99
 	INCR _C_LABEL(uvmexp)+V_FAULTS			! cnt.v_faults++ (clobbers %o0,%o1,%o2)
 
 	mov	%g3, %o3
@@ -2444,6 +2576,870 @@ text_error:
 	 nop
 	NOTREACHED
 
+#ifdef SUN4V
+
+/*
+ * Traps for sun4v.
+ */
+
+sun4v_tl1_dtsb_miss:
+	GET_MMFSA(%g1)
+	add	%g1, 0x48, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	sethi	%hi(_C_LABEL(ctxbusy)), %g4
+	ldx	[%g4 + %lo(_C_LABEL(ctxbusy))], %g4
+
+	sllx	%g6, 3, %g6				! Make it into an offset into ctxbusy
+	ldx	[%g4 + %g6], %g4			! Load up our page table.
+
+	srax	%g3, HOLESHIFT, %g5			! Check for valid address
+	brz,pt	%g5, 0f					! Should be zero or -1
+	 inc	%g5					! Make -1 -> 0
+	brnz,pn	%g5, sun4v_tl1_ptbl_miss		! Error! In hole!
+0:
+	srlx	%g3, STSHIFT, %g6
+	and	%g6, STMASK, %g6			! Index into pm_segs
+	sll	%g6, 3, %g6
+	add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page directory pointer
+
+	srlx	%g3, PDSHIFT, %g6
+	and	%g6, PDMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_tl1_ptbl_miss		! NULL entry? check somewhere else
+	 add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page table pointer
+
+	srlx	%g3, PTSHIFT, %g6			! Convert to ptab offset
+	and	%g6, PTMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_tl1_ptbl_miss		! NULL entry? check somewhere else
+	 add	%g4, %g6, %g6
+1:
+	ldxa	[%g6] ASI_PHYS_CACHED, %g4
+	brgez,pn %g4, sun4v_tl1_ptbl_miss		! Entry invalid?  Punt
+	 or	%g4, SUN4V_TLB_ACCESS, %g7		! Update the access bit
+
+	btst	SUN4V_TLB_ACCESS, %g4			! Need to update access git?
+	bne,pt	%xcc, 2f
+	 nop
+	casxa	[%g6] ASI_PHYS_CACHED, %g4, %g7		!  and write it out
+	cmp	%g4, %g7
+	bne,pn	%xcc, 1b
+	 or	%g4, SUN4V_TLB_ACCESS, %g4		! Update the modified bit
+2:
+	sethi	%hi(_C_LABEL(tsb_dmmu)), %g2
+	ldx	[%g2 + %lo(_C_LABEL(tsb_dmmu))], %g2
+
+	mov	%g1, %g7
+	/* Construct TSB tag word. */
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	mov	%g3, %g1
+	srlx	%g1, 22, %g1
+	sllx	%g6, 48, %g6
+	or	%g1, %g6, %g1
+
+	srlx	%g3, PTSHIFT, %g3
+	sethi	%hi(_C_LABEL(tsbsize)), %g5
+	mov	512, %g6
+	ld	[%g5 + %lo(_C_LABEL(tsbsize))], %g5
+	sllx	%g6, %g5, %g5
+	sub	%g5, 1, %g5
+	and	%g3, %g5, %g3
+	sllx	%g3, 4, %g3
+	add	%g2, %g3, %g2
+
+#define TSBTAG_LOCKED	0x400
+3:
+	ld	[%g2], %g3
+	btst	TSBTAG_LOCKED, %g3
+	bnz,pn	%icc, 3b
+	 or	%g3, TSBTAG_LOCKED, %g5
+	casa	[%g2] ASI_NUCLEUS, %g3, %g5
+	cmp	%g3, %g5
+	bne,pn	%icc, 3b
+	 nop
+	membar	#StoreStore
+	stx	%g4, [%g2 + 8]
+	stx	%g1, [%g2]		! unlock
+
+	retry
+	NOTREACHED
+
+sun4v_tl1_dtsb_prot:
+	GET_MMFSA(%g1)
+	add	%g1, 0x48, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	sethi	%hi(_C_LABEL(ctxbusy)), %g4
+	ldx	[%g4 + %lo(_C_LABEL(ctxbusy))], %g4
+
+	sllx	%g6, 3, %g6				! Make it into an offset into ctxbusy
+	ldx	[%g4 + %g6], %g4			! Load up our page table.
+
+	srax	%g3, HOLESHIFT, %g5			! Check for valid address
+	brz,pt	%g5, 0f					! Should be zero or -1
+	 inc	%g5					! Make -1 -> 0
+	brnz,pn	%g5, sun4v_tl1_ptbl_miss		! Error! In hole!
+0:
+	srlx	%g3, STSHIFT, %g6
+	and	%g6, STMASK, %g6			! Index into pm_segs
+	sll	%g6, 3, %g6
+	add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page directory pointer
+
+	srlx	%g3, PDSHIFT, %g6
+	and	%g6, PDMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_tl1_ptbl_miss		! NULL entry? check somewhere else
+	 add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page table pointer
+
+	srlx	%g3, PTSHIFT, %g6			! Convert to ptab offset
+	and	%g6, PTMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_tl1_ptbl_miss		! NULL entry? check somewhere else
+	 add	%g4, %g6, %g6
+1:
+	ldxa	[%g6] ASI_PHYS_CACHED, %g4
+	brgez,pn %g4, sun4v_tl1_ptbl_miss		! Entry invalid?  Punt
+	 or	%g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g7
+		! Update the modified bit
+
+#	btst	SUN4V_TLB_REAL_W|SUN4V_TLB_W, %g4	! Is it a ref fault?
+	mov	1, %g2
+	sllx	%g2, 61, %g2
+	or	%g2, SUN4V_TLB_W, %g2
+	btst	%g2, %g4
+	bz,pn	%xcc, sun4v_tl1_ptbl_miss		! No -- really fault
+	 nop
+	casxa	[%g6] ASI_PHYS_CACHED, %g4, %g7		!  and write it out
+	cmp	%g4, %g7
+	bne,pn	%xcc, 1b
+	 or	%g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g4
+		! Update the modified bit
+2:
+	sethi	%hi(_C_LABEL(tsb_dmmu)), %g2
+	ldx	[%g2 + %lo(_C_LABEL(tsb_dmmu))], %g2
+
+	mov	%g1, %g7
+	/* Construct TSB tag word. */
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	mov	%g3, %g1
+	srlx	%g1, 22, %g1
+	sllx	%g6, 48, %g6
+	or	%g1, %g6, %g1
+
+	srlx	%g3, PTSHIFT, %g3
+	sethi	%hi(_C_LABEL(tsbsize)), %g5
+	mov	512, %g6
+	ld	[%g5 + %lo(_C_LABEL(tsbsize))], %g5
+	sllx	%g6, %g5, %g5
+	sub	%g5, 1, %g5
+	and	%g3, %g5, %g3
+	sllx	%g3, 4, %g3
+	add	%g2, %g3, %g2
+
+#define TSBTAG_LOCKED	0x400
+3:
+	ld	[%g2], %g3
+	btst	TSBTAG_LOCKED, %g3
+	bnz,pn	%icc, 3b
+	 or	%g3, TSBTAG_LOCKED, %g5
+	casa	[%g2] ASI_NUCLEUS, %g3, %g5
+	cmp	%g3, %g5
+	bne,pn	%icc, 3b
+	 nop
+	membar	#StoreStore
+	stx	%g4, [%g2 + 8]
+	stx	%g1, [%g2]		! unlock
+
+	mov	%o0, %g1
+	mov	%o1, %g2
+	mov	%o2, %g3
+
+#define MAP_DTLB	0x1
+#define MAP_ITLB	0x2
+#define MMU_UNMAP_ADDR	0x84
+	add	%g7, 0x48, %o0
+	ldxa	[%o0] ASI_PHYS_CACHED, %o0
+	add	%g7, 0x50, %o1
+	ldxa	[%o1] ASI_PHYS_CACHED, %o1
+	mov	MAP_DTLB, %o2
+	ta	MMU_UNMAP_ADDR
+
+	mov	%g1, %o0
+	mov	%g2, %o1
+	mov	%g3, %o2
+
+	retry
+	NOTREACHED
+
+
+sun4v_tl1_ptbl_miss:
+	rdpr	%tpc, %g1
+
+	set	rft_user_fault_start, %g2
+	cmp	%g1, %g2
+	blu,pt	%xcc, 1f
+	 set	rft_user_fault_end, %g2
+	cmp	%g1, %g2
+	bgeu,pt	%xcc, 1f
+	 nop
+
+	/* Fixup %cwp. */
+	rdpr	%cwp, %g1
+	inc	%g1
+	wrpr	%g1, %cwp
+
+	rdpr	%tt, %g1
+	wrpr	1, %tl
+	wrpr	%g1, %tt
+	rdpr	%cwp, %g1
+	set	TSTATE_KERN, %g2
+	wrpr	%g1, %g2, %tstate
+	set	return_from_trap, %g1
+	wrpr	%g1, %tpc
+	add	%g1, 4, %g1
+	wrpr	%g1, %tnpc
+	wrpr	%g0, 1, %gl
+
+	ba,pt %xcc, sun4v_datatrap
+	 wrpr	WSTATE_KERN, %wstate
+
+1:
+	rdpr	%tstate, %g3
+	rdpr	%tt, %g4
+
+	rdpr	%tl, %g1
+	dec	%g1
+	wrpr	%g1, %tl
+	rdpr	%tt, %g2
+	inc	%g1
+	wrpr	%g1, %tl
+
+	wrpr	%g0, %g3, %tstate
+	wrpr	%g0, %g4, %tt
+
+	andn	%g2, 0x00f, %g3
+	cmp	%g3, 0x080
+	be,pn	%icc, flush_normals
+	 nop
+	cmp	%g3, 0x0a0
+	be,pn	%icc, flush_others
+	 nop
+	cmp	%g3, 0x0c0
+	be,pn	%icc, ufill_trap
+	 nop
+
+	Debugger()
+	NOTREACHED
+
+flush_others:
+	set	pcbspill_others, %g1
+	wrpr	%g1, %tnpc
+	done
+	NOTREACHED
+
+flush_normals:
+ufill_trap:
+	/*
+	 * Rearrange our trap state such that it appears as if we got
+	 * this trap directly from user mode.  Then process it at TL = 1.
+	 * We'll take the spill/fill trap again once we return to user mode.
+	 */
+	rdpr	%tt, %g1
+	rdpr	%tstate, %g3
+	wrpr	%g0, 1, %tl
+	wrpr	%g0, %g1, %tt
+	rdpr	%tstate, %g2
+	wrpr	%g0, 2, %tl
+	and	%g2, TSTATE_CWP, %g2
+	andn	%g3, TSTATE_CWP, %g3
+	wrpr	%g2, %g3, %tstate
+	set	sun4v_datatrap, %g4
+	wrpr	%g0, %g4, %tnpc
+	done
+
+sun4v_tl0_dtsb_miss:
+	GET_MMFSA(%g1)
+	add	%g1, 0x48, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	sethi	%hi(_C_LABEL(ctxbusy)), %g4
+	ldx	[%g4 + %lo(_C_LABEL(ctxbusy))], %g4
+
+	sllx	%g6, 3, %g6				! Make it into an offset into ctxbusy
+	ldx	[%g4 + %g6], %g4			! Load up our page table.
+
+	srax	%g3, HOLESHIFT, %g5			! Check for valid address
+	brz,pt	%g5, 0f					! Should be zero or -1
+	 inc	%g5					! Make -1 -> 0
+	brnz,pn	%g5, sun4v_datatrap			! Error! In hole!
+0:
+	srlx	%g3, STSHIFT, %g6
+	and	%g6, STMASK, %g6			! Index into pm_segs
+	sll	%g6, 3, %g6
+	add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page directory pointer
+
+	srlx	%g3, PDSHIFT, %g6
+	and	%g6, PDMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_datatrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page table pointer
+
+	srlx	%g3, PTSHIFT, %g6			! Convert to ptab offset
+	and	%g6, PTMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_datatrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g6
+1:
+	ldxa	[%g6] ASI_PHYS_CACHED, %g4
+	brgez,pn %g4, sun4v_datatrap			! Entry invalid?  Punt
+	 or	%g4, SUN4V_TLB_ACCESS, %g7		! Update the access bit
+
+	btst	SUN4V_TLB_ACCESS, %g4			! Need to update access git?
+	bne,pt	%xcc, 2f
+	 nop
+	casxa	[%g6] ASI_PHYS_CACHED, %g4, %g7		!  and write it out
+	cmp	%g4, %g7
+	bne,pn	%xcc, 1b
+	 or	%g4, SUN4V_TLB_ACCESS, %g4		! Update the modified bit
+2:
+	sethi	%hi(_C_LABEL(tsb_dmmu)), %g2
+	ldx	[%g2 + %lo(_C_LABEL(tsb_dmmu))], %g2
+
+	mov	%g1, %g7
+	/* Construct TSB tag word. */
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	mov	%g3, %g1
+	srlx	%g1, 22, %g1
+	sllx	%g6, 48, %g6
+	or	%g1, %g6, %g1
+
+	srlx	%g3, PTSHIFT, %g3
+	sethi	%hi(_C_LABEL(tsbsize)), %g5
+	mov	512, %g6
+	ld	[%g5 + %lo(_C_LABEL(tsbsize))], %g5
+	sllx	%g6, %g5, %g5
+	sub	%g5, 1, %g5
+	and	%g3, %g5, %g3
+	sllx	%g3, 4, %g3
+	add	%g2, %g3, %g2
+
+#define TSBTAG_LOCKED	0x400
+3:
+	ld	[%g2], %g3
+	btst	TSBTAG_LOCKED, %g3
+	bnz,pn	%icc, 3b
+	 or	%g3, TSBTAG_LOCKED, %g5
+	casa	[%g2] ASI_NUCLEUS, %g3, %g5
+	cmp	%g3, %g5
+	bne,pn	%icc, 3b
+	 nop
+	membar	#StoreStore
+	stx	%g4, [%g2 + 8]
+	stx	%g1, [%g2]		! unlock
+
+	retry
+	NOTREACHED
+
+sun4v_tl0_dtsb_prot:
+	GET_MMFSA(%g1)
+	add	%g1, 0x48, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	sethi	%hi(_C_LABEL(ctxbusy)), %g4
+	ldx	[%g4 + %lo(_C_LABEL(ctxbusy))], %g4
+
+	sllx	%g6, 3, %g6				! Make it into an offset into ctxbusy
+	ldx	[%g4 + %g6], %g4			! Load up our page table.
+
+	srax	%g3, HOLESHIFT, %g5			! Check for valid address
+	brz,pt	%g5, 0f					! Should be zero or -1
+	 inc	%g5					! Make -1 -> 0
+	brnz,pn	%g5, sun4v_datatrap			! Error! In hole!
+0:
+	srlx	%g3, STSHIFT, %g6
+	and	%g6, STMASK, %g6			! Index into pm_segs
+	sll	%g6, 3, %g6
+	add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page directory pointer
+
+	srlx	%g3, PDSHIFT, %g6
+	and	%g6, PDMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_datatrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page table pointer
+
+	srlx	%g3, PTSHIFT, %g6			! Convert to ptab offset
+	and	%g6, PTMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_datatrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g6
+1:
+	ldxa	[%g6] ASI_PHYS_CACHED, %g4
+	brgez,pn %g4, sun4v_datatrap			! Entry invalid?  Punt
+	 or	%g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g7
+		! Update the modified bit
+
+#	btst	SUN4V_TLB_REAL_W|SUN4V_TLB_W, %g4	! Is it a ref fault?
+	mov	1, %g2
+	sllx	%g2, 61, %g2
+	or	%g2, SUN4V_TLB_W, %g2
+	btst	%g2, %g4
+	bz,pn	%xcc, sun4v_datatrap			! No -- really fault
+	 nop
+	casxa	[%g6] ASI_PHYS_CACHED, %g4, %g7		!  and write it out
+	cmp	%g4, %g7
+	bne,pn	%xcc, 1b
+	 or	%g4, SUN4V_TLB_MODIFY|SUN4V_TLB_ACCESS|SUN4V_TLB_W, %g4
+		! Update the modified bit
+2:
+	sethi	%hi(_C_LABEL(tsb_dmmu)), %g2
+	ldx	[%g2 + %lo(_C_LABEL(tsb_dmmu))], %g2
+
+	mov	%g1, %g7
+	/* Construct TSB tag word. */
+	add	%g1, 0x50, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	mov	%g3, %g1
+	srlx	%g1, 22, %g1
+	sllx	%g6, 48, %g6
+	or	%g1, %g6, %g1
+
+	srlx	%g3, PTSHIFT, %g3
+	sethi	%hi(_C_LABEL(tsbsize)), %g5
+	mov	512, %g6
+	ld	[%g5 + %lo(_C_LABEL(tsbsize))], %g5
+	sllx	%g6, %g5, %g5
+	sub	%g5, 1, %g5
+	and	%g3, %g5, %g3
+	sllx	%g3, 4, %g3
+	add	%g2, %g3, %g2
+
+#define TSBTAG_LOCKED	0x400
+3:
+	ld	[%g2], %g3
+	btst	TSBTAG_LOCKED, %g3
+	bnz,pn	%icc, 3b
+	 or	%g3, TSBTAG_LOCKED, %g5
+	casa	[%g2] ASI_NUCLEUS, %g3, %g5
+	cmp	%g3, %g5
+	bne,pn	%icc, 3b
+	 nop
+	membar	#StoreStore
+	stx	%g4, [%g2 + 8]
+	stx	%g1, [%g2]		! unlock
+
+	mov	%o0, %g1
+	mov	%o1, %g2
+	mov	%o2, %g3
+
+#define MAP_DTLB	0x1
+#define MMU_UNMAP_ADDR	0x84
+	add	%g7, 0x48, %o0
+	ldxa	[%o0] ASI_PHYS_CACHED, %o0
+	add	%g7, 0x50, %o1
+	ldxa	[%o1] ASI_PHYS_CACHED, %o1
+	mov	MAP_DTLB, %o2
+	ta	MMU_UNMAP_ADDR
+
+	mov	%g1, %o0
+	mov	%g2, %o1
+	mov	%g3, %o2
+
+	retry
+	NOTREACHED
+
+sun4v_tl0_itsb_miss:
+	GET_MMFSA(%g1)
+	add	%g1, 0x8, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	add	%g1, 0x10, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	sethi	%hi(_C_LABEL(ctxbusy)), %g4
+	ldx	[%g4 + %lo(_C_LABEL(ctxbusy))], %g4
+
+	sllx	%g6, 3, %g6				! Make it into an offset into ctxbusy
+	ldx	[%g4 + %g6], %g4			! Load up our page table.
+
+	srax	%g3, HOLESHIFT, %g5			! Check for valid address
+	brz,pt	%g5, 0f					! Should be zero or -1
+	 inc	%g5					! Make -1 -> 0
+	brnz,pn	%g5, sun4v_texttrap			! Error! In hole!
+0:
+	srlx	%g3, STSHIFT, %g6
+	and	%g6, STMASK, %g6			! Index into pm_segs
+	sll	%g6, 3, %g6
+	add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page directory pointer
+
+	srlx	%g3, PDSHIFT, %g6
+	and	%g6, PDMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_texttrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g4
+	ldxa	[%g4] ASI_PHYS_CACHED, %g4		! Load page table pointer
+
+	srlx	%g3, PTSHIFT, %g6			! Convert to ptab offset
+	and	%g6, PTMASK, %g6
+	sll	%g6, 3, %g6
+	brz,pn	%g4, sun4v_texttrap			! NULL entry? check somewhere else
+	 add	%g4, %g6, %g6
+1:
+	ldxa	[%g6] ASI_PHYS_CACHED, %g4
+	brgez,pn %g4, sun4v_texttrap			! Entry invalid?  Punt
+	 or	%g4, SUN4V_TLB_ACCESS, %g7		! Update the access bit
+
+	btst	SUN4V_TLB_EXEC, %g4
+	bz,pn	%xcc, sun4v_texttrap
+	 nop
+	btst	SUN4V_TLB_ACCESS, %g4			! Need to update access git?
+	bne,pt	%xcc, 2f
+	 nop
+	casxa	[%g6] ASI_PHYS_CACHED, %g4, %g7		!  and write it out
+	cmp	%g4, %g7
+	bne,pn	%xcc, 1b
+	 or	%g4, SUN4V_TLB_ACCESS, %g4		! Update the modified bit
+2:
+	sethi	%hi(_C_LABEL(tsb_dmmu)), %g2
+	ldx	[%g2 + %lo(_C_LABEL(tsb_dmmu))], %g2
+
+	mov	%g1, %g7
+	/* Construct TSB tag word. */
+	add	%g1, 0x10, %g6
+	ldxa	[%g6] ASI_PHYS_CACHED, %g6
+	mov	%g3, %g1
+	srlx	%g1, 22, %g1
+	sllx	%g6, 48, %g6
+	or	%g1, %g6, %g1
+
+	srlx	%g3, PTSHIFT, %g3
+	sethi	%hi(_C_LABEL(tsbsize)), %g5
+	mov	512, %g6
+	ld	[%g5 + %lo(_C_LABEL(tsbsize))], %g5
+	sllx	%g6, %g5, %g5
+	sub	%g5, 1, %g5
+	and	%g3, %g5, %g3
+	sllx	%g3, 4, %g3
+	add	%g2, %g3, %g2
+
+#define TSBTAG_LOCKED	0x400
+3:
+	ld	[%g2], %g3
+	btst	TSBTAG_LOCKED, %g3
+	bnz,pn	%icc, 3b
+	 or	%g3, TSBTAG_LOCKED, %g5
+	casa	[%g2] ASI_NUCLEUS, %g3, %g5
+	cmp	%g3, %g5
+	bne,pn	%icc, 3b
+	 nop
+	membar	#StoreStore
+	stx	%g4, [%g2 + 8]
+	stx	%g1, [%g2]		! unlock
+
+	retry
+	NOTREACHED
+
+kspill_normal:
+	wrpr	0x90, %tt
+
+	GET_CPUINFO_PA(%g1)
+	wr	%g0, ASI_PHYS_CACHED, %asi
+
+	SPILL	stxa, %g1 + CI_RW, 8, %asi
+	saved
+
+	stxa	%sp, [%g1 + CI_RWSP] %asi
+
+	retry
+	NOTREACHED
+
+/*
+ * Spill user windows into the PCB.
+ */
+pcbspill_normals:
+	ba,pt	%xcc, pcbspill
+	 wrpr	0x80, %tt
+
+pcbspill_others:
+	wrpr	0xa0, %tt
+
+pcbspill:
+	GET_CPUINFO_PA(%g6)
+	wr	%g0, ASI_PHYS_CACHED, %asi
+	ldxa	[%g6 + CI_CPCB] %asi, %g6
+
+	sethi	%hi(_C_LABEL(ctxbusy)), %g1
+	ldx	[%g1 + %lo(_C_LABEL(ctxbusy))], %g1
+	ldx	[%g1], %g1
+
+	srlx	%g6, STSHIFT, %g7
+	and	%g7, STMASK, %g7
+	sll	%g7, 3, %g7
+	add	%g7, %g1, %g1
+	ldxa	[%g1] ASI_PHYS_CACHED, %g1		! Load pointer to directory
+
+	srlx	%g6, PDSHIFT, %g7			! Do page directory
+	and	%g7, PDMASK, %g7
+	sll	%g7, 3, %g7
+	brz,pn	%g1, pcbspill_fail
+	 add	%g7, %g1, %g1
+	ldxa	[%g1] ASI_PHYS_CACHED, %g1
+
+	srlx	%g6, PTSHIFT, %g7			! Convert to ptab offset
+	and	%g7, PTMASK, %g7
+	brz	%g1, pcbspill_fail
+	 sll	%g7, 3, %g7
+	add	%g1, %g7, %g7
+	ldxa	[%g7] ASI_PHYS_CACHED, %g7		! This one is not
+	brgez	%g7, pcbspill_fail
+	 srlx	%g7, PGSHIFT, %g7			! Isolate PA part
+	sll	%g6, 32-PGSHIFT, %g6			! And offset
+	sllx	%g7, PGSHIFT+23, %g7			! There are 23 bits to the left of the PA in the TTE
+	srl	%g6, 32-PGSHIFT, %g6
+	srax	%g7, 23, %g7
+	or	%g7, %g6, %g6				! Then combine them to form PA
+
+!	wr	%g0, ASI_PHYS_CACHED, %asi		! Use ASI_PHYS_CACHED to prevent possible page faults
+	
+	lduba	[%g6 + PCB_NSAVED] %asi, %g7
+	sllx	%g7, 7, %g5
+	add	%g6, %g5, %g5
+	SPILL	stxa, %g5 + PCB_RW, 8, %asi
+	saved
+
+	sllx	%g7, 3, %g5
+	add	%g6, %g5, %g5
+	stxa	%sp, [%g5 + PCB_RWSP] %asi
+
+	inc	%g7
+	stba	%g7, [%g6 + PCB_NSAVED] %asi
+
+	retry
+	NOTREACHED
+
+pcbspill_fail:
+	Debugger()
+	NOTREACHED
+
+
+sun4v_datatrap:
+	GET_MMFSA(%g3)
+	add	%g3, 0x48, %g1
+	ldxa	[%g1] ASI_PHYS_CACHED, %g1
+	add	%g3, 0x50, %g2
+	ldxa	[%g2] ASI_PHYS_CACHED, %g2
+	add	%g3, 0x60, %g4
+	stxa	%sp, [%g4] ASI_PHYS_CACHED
+
+	TRAP_SETUP -CC64FSZ-TF_SIZE, ctxid_1
+	or	%g1, %g2, %o3
+	mov	%g1, %o4
+
+	rdpr	%tt, %g4
+	rdpr	%tstate, %g1
+	rdpr	%tpc, %g2
+	rdpr	%tnpc, %g3
+
+	stx	%g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
+	mov	%g4, %o1		! (type)
+	stx	%g2, [%sp + CC64FSZ + BIAS + TF_PC]
+	rd	%y, %g5
+	stx	%g3, [%sp + CC64FSZ + BIAS + TF_NPC]
+	st	%g5, [%sp + CC64FSZ + BIAS + TF_Y]
+	mov	%g2, %o2		! (pc)
+	sth	%o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug
+
+	cmp	%o1, T_FDMMU_PROT
+	bne,pn	%icc, 1f
+	 mov	SFSR_FV, %o5
+	or	%o5, SFSR_W, %o5
+
+1:
+	wrpr	%g0, PSTATE_KERN, %pstate		! Get back to normal globals
+	wrpr	%g0, 0, %gl
+
+	stx	%g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
+	stx	%g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
+	add	%sp, CC64FSZ + BIAS, %o0		! (&tf)
+	stx	%g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]
+	stx	%g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]
+	stx	%g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]
+	rdpr	%pil, %g5
+	stx	%g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]
+	stx	%g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]
+	stb	%g5, [%sp + CC64FSZ + BIAS + TF_PIL]
+	stb	%g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]
+
+	/*
+	 * Phew, ready to enable traps and call C code.
+	 */
+	wrpr	%g0, 0, %tl
+
+#ifdef SUN4V
+	GET_CPUINFO_VA(%g1)
+	ldx	[%g1 + CI_RWSP], %g2
+	brz,pt	%g2, 1f
+	 nop
+
+	ldx	[%g1 + CI_RW + (0*8)], %l0
+	ldx	[%g1 + CI_RW + (1*8)], %l1
+	ldx	[%g1 + CI_RW + (2*8)], %l2
+	ldx	[%g1 + CI_RW + (3*8)], %l3
+	ldx	[%g1 + CI_RW + (4*8)], %l4
+	ldx	[%g1 + CI_RW + (5*8)], %l5
+	ldx	[%g1 + CI_RW + (6*8)], %l6
+	ldx	[%g1 + CI_RW + (7*8)], %l7
+	stx	%l0, [%g2 + BIAS + (0*8)]
+	stx	%l1, [%g2 + BIAS + (1*8)]
+	stx	%l2, [%g2 + BIAS + (2*8)]
+	stx	%l3, [%g2 + BIAS + (3*8)]
+	stx	%l4, [%g2 + BIAS + (4*8)]
+	stx	%l5, [%g2 + BIAS + (5*8)]
+	stx	%l6, [%g2 + BIAS + (6*8)]
+	stx	%l7, [%g2 + BIAS + (7*8)]
+	ldx	[%g1 + CI_RW + (8*8)], %l0
+	ldx	[%g1 + CI_RW + (9*8)], %l1
+	ldx	[%g1 + CI_RW + (10*8)], %l2
+	ldx	[%g1 + CI_RW + (11*8)], %l3
+	ldx	[%g1 + CI_RW + (12*8)], %l4
+	ldx	[%g1 + CI_RW + (13*8)], %l5
+	ldx	[%g1 + CI_RW + (14*8)], %l6
+	ldx	[%g1 + CI_RW + (15*8)], %l7
+	stx	%l0, [%g2 + BIAS + (8*8)]
+	stx	%l1, [%g2 + BIAS + (9*8)]
+	stx	%l2, [%g2 + BIAS + (10*8)]
+	stx	%l3, [%g2 + BIAS + (11*8)]
+	stx	%l4, [%g2 + BIAS + (12*8)]
+	stx	%l5, [%g2 + BIAS + (13*8)]
+	stx	%l6, [%g2 + BIAS + (14*8)]
+	stx	%l7, [%g2 + BIAS + (15*8)]
+
+	stx	%g0, [%g1 + CI_RWSP]
+1:
+#endif
+
+	wr	%g0, ASI_PRIMARY_NOFAULT, %asi	! Restore default ASI
+	wrpr	%g0, PSTATE_INTR, %pstate	! traps on again
+	call	_C_LABEL(data_access_fault)	! data_acces_fault(tf, type, ...)
+	 nop
+
+	ba,a,pt	%icc, return_from_trap
+	 nop
+	NOTREACHED
+
+sun4v_texttrap:
+	GET_MMFSA(%g3)
+	add	%g3, 0x08, %g1
+	ldxa	[%g1] ASI_PHYS_CACHED, %g1
+	add	%g3, 0x10, %g2
+	ldxa	[%g2] ASI_PHYS_CACHED, %g2
+
+	TRAP_SETUP -CC64FSZ-TF_SIZE, ctxid_2
+
+	or	%g1, %g2, %o2
+	clr	%o3
+
+	rdpr	%tt, %g4
+	rdpr	%tstate, %g1
+	rdpr	%tpc, %g2
+	rdpr	%tnpc, %g3
+
+	stx	%g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
+	mov	%g4, %o1		! (type)
+	stx	%g2, [%sp + CC64FSZ + BIAS + TF_PC]
+	rd	%y, %g5
+	stx	%g3, [%sp + CC64FSZ + BIAS + TF_NPC]
+	st	%g5, [%sp + CC64FSZ + BIAS + TF_Y]
+	sth	%o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug
+
+	wrpr	%g0, PSTATE_KERN, %pstate		! Get back to normal globals
+	wrpr	%g0, 0, %gl
+
+	stx	%g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
+	stx	%g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
+	add	%sp, CC64FSZ + BIAS, %o0		! (&tf)
+	stx	%g3, [%sp + CC64FSZ + BIAS + TF_G + (3*8)]
+	stx	%g4, [%sp + CC64FSZ + BIAS + TF_G + (4*8)]
+	stx	%g5, [%sp + CC64FSZ + BIAS + TF_G + (5*8)]
+	rdpr	%pil, %g5
+	stx	%g6, [%sp + CC64FSZ + BIAS + TF_G + (6*8)]
+	stx	%g7, [%sp + CC64FSZ + BIAS + TF_G + (7*8)]
+	stb	%g5, [%sp + CC64FSZ + BIAS + TF_PIL]
+	stb	%g5, [%sp + CC64FSZ + BIAS + TF_OLDPIL]
+
+	/*
+	 * Phew, ready to enable traps and call C code.
+	 */
+	wrpr	%g0, 0, %tl
+
+#ifdef SUN4V
+	GET_CPUINFO_VA(%g1)
+	ldx	[%g1 + CI_RWSP], %g2
+	brz,pt	%g2, 1f
+	 nop
+
+	ldx	[%g1 + CI_RW + (0*8)], %l0
+	ldx	[%g1 + CI_RW + (1*8)], %l1
+	ldx	[%g1 + CI_RW + (2*8)], %l2
+	ldx	[%g1 + CI_RW + (3*8)], %l3
+	ldx	[%g1 + CI_RW + (4*8)], %l4
+	ldx	[%g1 + CI_RW + (5*8)], %l5
+	ldx	[%g1 + CI_RW + (6*8)], %l6
+	ldx	[%g1 + CI_RW + (7*8)], %l7
+	stx	%l0, [%g2 + BIAS + (0*8)]
+	stx	%l1, [%g2 + BIAS + (1*8)]
+	stx	%l2, [%g2 + BIAS + (2*8)]
+	stx	%l3, [%g2 + BIAS + (3*8)]
+	stx	%l4, [%g2 + BIAS + (4*8)]
+	stx	%l5, [%g2 + BIAS + (5*8)]
+	stx	%l6, [%g2 + BIAS + (6*8)]
+	stx	%l7, [%g2 + BIAS + (7*8)]
+	ldx	[%g1 + CI_RW + (8*8)], %l0
+	ldx	[%g1 + CI_RW + (9*8)], %l1
+	ldx	[%g1 + CI_RW + (10*8)], %l2
+	ldx	[%g1 + CI_RW + (11*8)], %l3
+	ldx	[%g1 + CI_RW + (12*8)], %l4
+	ldx	[%g1 + CI_RW + (13*8)], %l5
+	ldx	[%g1 + CI_RW + (14*8)], %l6
+	ldx	[%g1 + CI_RW + (15*8)], %l7
+	stx	%l0, [%g2 + BIAS + (8*8)]
+	stx	%l1, [%g2 + BIAS + (9*8)]
+	stx	%l2, [%g2 + BIAS + (10*8)]
+	stx	%l3, [%g2 + BIAS + (11*8)]
+	stx	%l4, [%g2 + BIAS + (12*8)]
+	stx	%l5, [%g2 + BIAS + (13*8)]
+	stx	%l6, [%g2 + BIAS + (14*8)]
+	stx	%l7, [%g2 + BIAS + (15*8)]
+
+	stx	%g0, [%g1 + CI_RWSP]
+1:
+#endif
+
+	wr	%g0, ASI_PRIMARY_NOFAULT, %asi	! Restore default ASI
+	wrpr	%g0, PSTATE_INTR, %pstate	! traps on again
+	call	_C_LABEL(text_access_fault)	! text_access_fault(tf, type, ...)
+	 nop
+
+	ba,a,pt	%icc, return_from_trap
+	 nop
+	NOTREACHED
+
+#endif
+
 /*
  * We're here because we took an alignment fault in NUCLEUS context.
  * This could be a kernel bug or it could be due to saving or restoring
@@ -2520,32 +3516,12 @@ checkalign:
  *
  */
 slowtrap:
-#ifdef DIAGNOSTIC
-	/* Make sure kernel stack is aligned */
-	btst	0x03, %sp		! 32-bit stack OK?
-	and	%sp, 0x07, %g4		! 64-bit stack OK?
-	bz,pt	%icc, 1f
-	cmp	%g4, 0x1		! Must end in 0b001
-	be,pt	%icc, 1f
-	 rdpr	%wstate, %g4
-	cmp	%g7, WSTATE_KERN
-	bnz,pt	%icc, 1f		! User stack -- we'll blow it away
-	 nop
-#ifdef DEBUG
-	set	panicstack, %sp		! Kernel stack corrupt -- use panicstack
-#else	/* DEBUG */
-	set	estack0, %sp
-	ldx	[%sp], %sp
-	add	%sp, -CC64FSZ-BIAS, %sp	! Overwrite proc 0's stack.
-#endif	/* DEBUG */
-1:
-#endif	/* DIAGNOSTIC */
 	rdpr	%tt, %g4
 	rdpr	%tstate, %g1
 	rdpr	%tpc, %g2
 	rdpr	%tnpc, %g3
 
-	TRAP_SETUP -CC64FSZ-TF_SIZE
+	TRAP_SETUP -CC64FSZ-TF_SIZE, ctxid_3
 Lslowtrap_reenter:
 	stx	%g1, [%sp + CC64FSZ + BIAS + TF_TSTATE]
 	mov	%g4, %o1		! (type)
@@ -2558,6 +3534,8 @@ Lslowtrap_reenter:
 	sth	%o1, [%sp + CC64FSZ + BIAS + TF_TT]! debug
 
 	wrpr	%g0, PSTATE_KERN, %pstate		! Get back to normal globals
+gl0_1:	nop
+
 	stx	%g1, [%sp + CC64FSZ + BIAS + TF_G + (1*8)]
 	stx	%g2, [%sp + CC64FSZ + BIAS + TF_G + (2*8)]
 	add	%sp, CC64FSZ + BIAS, %o0		! (&tf)
@@ -2573,10 +3551,50 @@ Lslowtrap_reenter:
 	/*
 	 * Phew, ready to enable traps and call C code.
 	 */
-	rdpr	%tl, %g1
-	dec	%g1
-	movrlz	%g1, %g0, %g1
-	wrpr	%g0, %g1, %tl		! Revert to kernel mode
+	wrpr	%g0, 0, %tl
+
+#ifdef SUN4V
+	GET_CPUINFO_VA(%g1)
+	ldx	[%g1 + CI_RWSP], %g2
+	brz,pt	%g2, 1f
+	 nop
+
+	ldx	[%g1 + CI_RW + (0*8)], %l0
+	ldx	[%g1 + CI_RW + (1*8)], %l1
+	ldx	[%g1 + CI_RW + (2*8)], %l2
+	ldx	[%g1 + CI_RW + (3*8)], %l3
+	ldx	[%g1 + CI_RW + (4*8)], %l4
+	ldx	[%g1 + CI_RW + (5*8)], %l5
+	ldx	[%g1 + CI_RW + (6*8)], %l6
+	ldx	[%g1 + CI_RW + (7*8)], %l7
+	stx	%l0, [%g2 + BIAS + (0*8)]
+	stx	%l1, [%g2 + BIAS + (1*8)]
+	stx	%l2, [%g2 + BIAS + (2*8)]
+	stx	%l3, [%g2 + BIAS + (3*8)]
+	stx	%l4, [%g2 + BIAS + (4*8)]
+	stx	%l5, [%g2 + BIAS + (5*8)]
+	stx	%l6, [%g2 + BIAS + (6*8)]
+	stx	%l7, [%g2 + BIAS + (7*8)]
+	ldx	[%g1 + CI_RW + (8*8)], %l0
+	ldx	[%g1 + CI_RW + (9*8)], %l1
+	ldx	[%g1 + CI_RW + (10*8)], %l2
+	ldx	[%g1 + CI_RW + (11*8)], %l3
+	ldx	[%g1 + CI_RW + (12*8)], %l4
+	ldx	[%g1 + CI_RW + (13*8)], %l5
+	ldx	[%g1 + CI_RW + (14*8)], %l6
+	ldx	[%g1 + CI_RW + (15*8)], %l7
+	stx	%l0, [%g2 + BIAS + (8*8)]
+	stx	%l1, [%g2 + BIAS + (9*8)]
+	stx	%l2, [%g2 + BIAS + (10*8)]
+	stx	%l3, [%g2 + BIAS + (11*8)]
+	stx	%l4, [%g2 + BIAS + (12*8)]
+	stx	%l5, [%g2 + BIAS + (13*8)]
+	stx	%l6, [%g2 + BIAS + (14*8)]
+	stx	%l7, [%g2 + BIAS + (15*8)]
+
+	stx	%g0, [%g1 + CI_RWSP]
+1:
+#endif
 
 	wr	%g0, ASI_PRIMARY_NOFAULT, %asi		! Restore default ASI
 	wrpr	%g0, PSTATE_INTR, %pstate	! traps on again
@@ -2671,7 +3689,7 @@ softtrap:
  *	ptrace...
  */
 syscall_setup:
-	TRAP_SETUP -CC64FSZ-TF_SIZE
+	TRAP_SETUP -CC64FSZ-TF_SIZE, ctxid_4
 
 #ifdef DEBUG
 	rdpr	%tt, %o1	! debug
@@ -2679,6 +3697,8 @@ syscall_setup:
 #endif	/* DEBUG */
 
 	wrpr	%g0, PSTATE_KERN, %pstate	! Get back to normal globals
+gl0_2:	nop
+
 	stx	%g1, [%sp + CC64FSZ + BIAS + TF_G + ( 1*8)]
 	mov	%g1, %o1			! code
 	rdpr	%tpc, %o2			! (pc)
@@ -2901,6 +3921,61 @@ ret_from_intr_vector:
 	ba,a	ret_from_intr_vector
 	 nop				! XXX spitfire bug?
 
+#ifdef SUN4V
+
+sun4v_cpu_mondo:
+	mov	0x3c0, %g1
+	ldxa	[%g1] ASI_QUEUE, %g2
+
+	GET_CPUINFO_PA(%g3)
+	add	%g3, CI_CPUMQ, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	ldxa	[%g3 + %g2] ASI_PHYS_CACHED, %g4
+	add	%g2, 8, %g5
+	ldxa	[%g3 + %g5] ASI_PHYS_CACHED, %g5
+	add	%g2, 16, %g6
+	ldxa	[%g3 + %g6] ASI_PHYS_CACHED, %g6
+	add	%g2, 64, %g2
+	and	%g2, 0x7ff, %g2
+	stxa	%g2, [%g1] ASI_QUEUE
+	membar	#Sync
+
+	mov	%g4, %g2
+	mov	%g5, %g3
+	mov	%g6, %g5
+	jmpl	%g2, %g0
+	 nop			! No store here!
+	retry
+	NOTREACHED
+
+sun4v_dev_mondo:
+	mov	0x3d0, %g1
+	ldxa	[%g1] ASI_QUEUE, %g2
+
+	GET_CPUINFO_PA(%g3)
+	add	%g3, CI_DEVMQ, %g3
+	ldxa	[%g3] ASI_PHYS_CACHED, %g3
+	ldxa	[%g3 + %g2] ASI_PHYS_CACHED, %g4
+	add	%g2, 64, %g2
+	and	%g2, 0x7ff, %g2
+	stxa	%g2, [%g1] ASI_QUEUE
+	membar	#Sync
+
+	and	%g4, 0x7ff, %g4
+	sllx	%g4, 3, %g5
+
+	sethi	%hi(_C_LABEL(intrlev)), %g3
+	or	%g3, %lo(_C_LABEL(intrlev)), %g3
+	ldx	[%g3 + %g5], %g5	! We have a pointer to the handler
+
+	brnz,pt	%g5, setup_sparcintr
+	 nop
+
+	ba,a	3b
+	 nop
+
+#endif
+
 #ifdef MULTIPROCESSOR
 ENTRY(ipi_tlb_page_demap)
 	rdpr	%pstate, %g1
@@ -3109,13 +4184,14 @@ _C_LABEL(sparc_interrupt):
 	bz,pt	%icc, 0f
 	 set	_C_LABEL(intrlev), %g3
 	wr	%g0, 1, CLEAR_SOFTINT
-	DLFLUSH %g3, %g2
 	ba,pt	%icc, setup_sparcintr
 	 ldx	[%g3 + 8], %g5	! intrlev[1] is reserved for %tick intr.
 0:
-	INTR_SETUP -CC64FSZ-TF_SIZE-8
+	INTR_SETUP -CC64FSZ-TF_SIZE-8, ctxid_5
 	! Switch to normal globals so we can save them
 	wrpr	%g0, PSTATE_KERN, %pstate
+gl0_3:	nop
+
 	stx	%g1, [%sp + CC64FSZ + BIAS + TF_G + ( 1*8)]
 	stx	%g2, [%sp + CC64FSZ + BIAS + TF_G + ( 2*8)]
 	stx	%g3, [%sp + CC64FSZ + BIAS + TF_G + ( 3*8)]
@@ -3124,18 +4200,69 @@ _C_LABEL(sparc_interrupt):
 	stx	%g6, [%sp + CC64FSZ + BIAS + TF_G + ( 6*8)]
 	stx	%g7, [%sp + CC64FSZ + BIAS + TF_G + ( 7*8)]
 
+gl0_x:
 	flushw			! Do not remove this instruction -- causes interrupt loss
+
+#ifdef SUN4V
+	GET_CPUINFO_VA(%g1)
+	ldx	[%g1 + CI_RWSP], %g2
+	brz,pt	%g2, 1f
+	 nop
+
+	ldx	[%g1 + CI_RW + (0*8)], %l0
+	ldx	[%g1 + CI_RW + (1*8)], %l1
+	ldx	[%g1 + CI_RW + (2*8)], %l2
+	ldx	[%g1 + CI_RW + (3*8)], %l3
+	ldx	[%g1 + CI_RW + (4*8)], %l4
+	ldx	[%g1 + CI_RW + (5*8)], %l5
+	ldx	[%g1 + CI_RW + (6*8)], %l6
+	ldx	[%g1 + CI_RW + (7*8)], %l7
+	stx	%l0, [%g2 + BIAS + (0*8)]
+	stx	%l1, [%g2 + BIAS + (1*8)]
+	stx	%l2, [%g2 + BIAS + (2*8)]
+	stx	%l3, [%g2 + BIAS + (3*8)]
+	stx	%l4, [%g2 + BIAS + (4*8)]
+	stx	%l5, [%g2 + BIAS + (5*8)]
+	stx	%l6, [%g2 + BIAS + (6*8)]
+	stx	%l7, [%g2 + BIAS + (7*8)]
+	ldx	[%g1 + CI_RW + (8*8)], %l0
+	ldx	[%g1 + CI_RW + (9*8)], %l1
+	ldx	[%g1 + CI_RW + (10*8)], %l2
+	ldx	[%g1 + CI_RW + (11*8)], %l3
+	ldx	[%g1 + CI_RW + (12*8)], %l4
+	ldx	[%g1 + CI_RW + (13*8)], %l5
+	ldx	[%g1 + CI_RW + (14*8)], %l6
+	ldx	[%g1 + CI_RW + (15*8)], %l7
+	stx	%l0, [%g2 + BIAS + (8*8)]
+	stx	%l1, [%g2 + BIAS + (9*8)]
+	stx	%l2, [%g2 + BIAS + (10*8)]
+	stx	%l3, [%g2 + BIAS + (11*8)]
+	stx	%l4, [%g2 + BIAS + (12*8)]
+	stx	%l5, [%g2 + BIAS + (13*8)]
+	stx	%l6, [%g2 + BIAS + (14*8)]
+	stx	%l7, [%g2 + BIAS + (15*8)]
+
+	stx	%g0, [%g1 + CI_RWSP]
+1:
+#endif
+
 	rd	%y, %l6
 	INCR _C_LABEL(uvmexp)+V_INTR	! cnt.v_intr++; (clobbers %o0,%o1,%o2)
 	rdpr	%tt, %l5		! Find out our current IPL
 	rdpr	%tstate, %l0
 	rdpr	%tpc, %l1
 	rdpr	%tnpc, %l2
+#if 0
 	rdpr	%tl, %l3		! Dump our trap frame now we have taken the IRQ
 	stw	%l6, [%sp + CC64FSZ + BIAS + TF_Y]	! Silly, but we need to save this for rft
 	dec	%l3
-	CHKPT %l4,%l7,0x26
 	wrpr	%g0, %l3, %tl
+#else
+	wrpr	%g0, 0, %tl
+
+	! Dump our trap frame now we have taken the IRQ
+	stw	%l6, [%sp + CC64FSZ + BIAS + TF_Y]	! Silly, but we need to save this for rft
+#endif
 	sth	%l5, [%sp + CC64FSZ + BIAS + TF_TT]! debug
 	stx	%l0, [%sp + CC64FSZ + BIAS + TF_TSTATE]	! set up intrframe/clockframe
 	stx	%l1, [%sp + CC64FSZ + BIAS + TF_PC]
@@ -3150,7 +4277,7 @@ _C_LABEL(sparc_interrupt):
 	clr	%l5			! Zero handled count
 	mov	1, %l3			! Ack softint
 	sll	%l3, %l6, %l3		! Generate IRQ mask
-	
+
 	GET_CPUINFO_VA(%l4)
 
 	wrpr	%l6, %pil
@@ -3268,7 +4395,6 @@ intrcmplt:
 	.globl	softtrap, slowtrap
 	.globl	syscall
 
-
 /*
  * Various return-from-trap routines (see return_from_trap).
  */
@@ -3310,6 +4436,7 @@ return_from_trap:
 	wrpr	%g0, %g0, %pil				! Lower IPL
 1:
 	wrpr	%g0, PSTATE_KERN, %pstate		! Make sure we have normal globals & no IRQs
+gl0_4:	nop
 
 	/* Restore normal globals */
 	ldx	[%sp + CC64FSZ + BIAS + TF_G + (1*8)], %g1
@@ -3320,6 +4447,7 @@ return_from_trap:
 	ldx	[%sp + CC64FSZ + BIAS + TF_G + (6*8)], %g6
 	ldx	[%sp + CC64FSZ + BIAS + TF_G + (7*8)], %g7
 	/* Switch to alternate globals and load outs */
+gl1_1:
 	wrpr	%g0, PSTATE_KERN|PSTATE_AG, %pstate
 	ldx	[%sp + CC64FSZ + BIAS + TF_O + (0*8)], %i0
 	ldx	[%sp + CC64FSZ + BIAS + TF_O + (1*8)], %i1
@@ -3357,6 +4485,19 @@ rft_kernel:
 	wrpr	%g2, 0, %tpc
 	wrpr	%g1, 0, %tstate
 
+	rdpr	%canrestore, %g2
+	brnz	%g2, 1f
+	 nop
+
+	wr	%g0, ASI_NUCLEUS, %asi
+	rdpr	%cwp, %g1
+	dec	%g1
+	wrpr	%g1, %cwp
+	FILL	ldxa, %sp+BIAS, 8, %asi
+	restored
+	inc	%g1
+	wrpr	%g1, %cwp
+1:
 	restore
 
 	rdpr	%tstate, %g1			! Since we may have trapped our regs may be toast
@@ -3375,10 +4516,6 @@ rft_kernel:
  *	%g3 = return %npc
  * If returning to a valid window, just set psr and return.
  */
-	.data
-rft_wcnt:	.word 0
-	.text
-
 rft_user:
 	GET_CURPROC(%g7)
 	lduw	[%g7 + P_MD_ASTPENDING], %g7	! want AST trap?
@@ -3403,13 +4540,6 @@ rft_user:
 	brnz,pn	%g7, softtrap
 	 mov	T_RWRET, %g4
 
-	rdpr	%otherwin, %g7			! restore register window controls
-	wrpr	%g0, %g7, %canrestore
-	wrpr	%g0, 0, %otherwin
-
-	CHKPT %g4,%g7,9
-	wrpr	%g0, WSTATE_USER, %wstate	! Need to know where our sp points
-
 	/*
 	 * Set up our return trapframe so we can recover if we trap from here
 	 * on in.
@@ -3417,26 +4547,61 @@ rft_user:
 	wrpr	%g0, 1, %tl			! Set up the trap state
 	wrpr	%g2, 0, %tpc
 	wrpr	%g3, 0, %tnpc
-	wrpr	%g1, %g0, %tstate
-	restore
+	rdpr	%cwp, %g7
+	andn	%g1, CWP, %g1
+	wrpr	%g1, %g7, %tstate
 
-	CHKPT %g4,%g7,0xa
-	rdpr	%canrestore, %g5
-	wrpr	%g5, 0, %cleanwin			! Force cleanup of kernel windows
+	/* XXX Rewrite sun4u code to handle faults like sun4v. */
+	sethi	%hi(_C_LABEL(cputyp)), %g2
+	ld	[%g2 + %lo(_C_LABEL(cputyp))], %g2
+	cmp	%g2, CPU_SUN4V
+	bne,pt	%icc, 1f
+	 nop
+
+	rdpr	%otherwin, %g2
+	brnz	%g2, 1f
+	 nop
+
+	wr	%g0, ASI_AIUS, %asi
+	rdpr	%cwp, %g1
+	dec	%g1
+	wrpr	%g1, 0, %cwp
+rft_user_fault_start:
+	FILL	ldxa, %sp+BIAS, 8, %asi
+	ldx	[%g6 + PCB_WCOOKIE], %g7
+	xor	%g7, %i7, %i7		! stackghost
+rft_user_fault_end:
+	restored
+	inc	%g1
+	wrpr	%g1, 0, %cwp
+
+	rdpr	%canrestore, %g7
+	wrpr	%g7, 0, %otherwin
+	wrpr	%g0, 0, %canrestore
+1:
+	rdpr	%otherwin, %g7			! restore register window controls
+	wrpr	%g7, 0, %canrestore
+	wrpr	%g0, 0, %otherwin
+	wrpr	WSTATE_USER, %wstate		! Need to know where our sp points
+	wrpr	%g7, 0, %cleanwin		! Force cleanup of kernel windows
+
+	restore
 
 	rdpr	%tstate, %g1
 	rdpr	%cwp, %g7			! Find our cur window
 	andn	%g1, CWP, %g1			! Clear it from %tstate
 	wrpr	%g1, %g7, %tstate		! Set %tstate with %cwp
 
-	wr	%g0, ASI_DMMU, %asi		! restore the user context
-	ldxa	[CTX_SECONDARY] %asi, %g4
+	mov	CTX_SECONDARY, %g1		! Restore the user context
+ctxid_6:
+	ldxa	[%g1] ASI_DMMU, %g4
+	mov	CTX_PRIMARY, %g2
+ctxid_7:
+	stxa	%g4, [%g2] ASI_DMMU
 	sethi	%hi(KERNBASE), %g7		! Should not be needed due to retry
-	stxa	%g4, [CTX_PRIMARY] %asi
 	membar	#Sync				! Should not be needed due to retry
 	flush	%g7				! Should not be needed due to retry
-	CLRTT
-	CHKPT %g4,%g7,0xd
+
 #ifdef DEBUG
 	GET_CPCB(%g5)
 	ldub	[%g5 + PCB_NSAVED], %g5		! Any saved reg windows?
@@ -3635,6 +4800,17 @@ _C_LABEL(cpu_initialize):
 
 	/* Change the trap base register */
 	set	_C_LABEL(trapbase), %l1
+#ifdef SUN4V
+	sethi	%hi(_C_LABEL(cputyp)), %l0
+	ld	[%l0 + %lo(_C_LABEL(cputyp))], %l0
+	cmp	%l0, CPU_SUN4V
+	bne,pt	%icc, 1f
+	 nop
+	set	_C_LABEL(trapbase_sun4v), %l1
+	GET_CPUINFO_VA(%o1)
+	ldx	[%o1 + CI_MMFSA], %o1
+1:
+#endif
 	call	_C_LABEL(prom_set_trap_table)	! Now we should be running 100% from our handlers
 	 mov	%l1, %o0
 	wrpr	%l1, 0, %tba			! Make sure the PROM didn't foul up.
@@ -3654,9 +4830,8 @@ _C_LABEL(cpu_initialize):
 
 	GET_CPUINFO_VA(%l0)
 	ldx	[%l0 + CI_SPINUP], %o1
-
 	call	%o1				! Call routine
-	 clr	%o0				! our frame arg is ignored
+	 nop
 	NOTREACHED
 
 	set	1f, %o0				! Main should never come back here
@@ -3983,6 +5158,11 @@ _C_LABEL(us3_dcache_flush_page):
 	retl
 	 nop
 
+	.globl no_dcache_flush_page
+ENTRY(no_dcache_flush_page)
+	retl
+	 nop
+
 /*
  * cache_flush_virt(va, len)
  *
@@ -4882,6 +6062,7 @@ Lsw_havectx:
 	/*
 	 * We probably need to flush the cache here.
 	 */
+ctxid_8:
 	stxa	%o0, [%l5] ASI_DMMU		! Maybe we should invalidate the old context?
 	membar	#Sync				! Maybe we should use flush here?
 	flush	%sp
@@ -7902,4 +9083,32 @@ _C_LABEL(dlflush_start):
 	.xword	dlflush3
 	.xword	dlflush4
 	.xword	dlflush5
-	.xword 0
+	.xword	0
+
+#ifdef SUN4V
+	.globl	_C_LABEL(gl0_start)
+_C_LABEL(gl0_start):
+	.xword	gl0_1
+	.xword	gl0_2
+	.xword	gl0_3
+	.xword	gl0_4
+	.xword	gl0_x
+	.xword	0
+
+	.globl	_C_LABEL(gl1_start)
+_C_LABEL(gl1_start):
+	.xword	gl1_1
+	.xword	0
+
+	.globl	_C_LABEL(ctxid_start)
+_C_LABEL(ctxid_start):
+	.xword	ctxid_1
+	.xword	ctxid_2
+	.xword	ctxid_3
+	.xword	ctxid_4
+	.xword	ctxid_5
+	.xword	ctxid_6
+	.xword	ctxid_7
+	.xword	ctxid_8
+	.xword	0
+#endif
diff --git a/sys/arch/sparc64/sparc64/pmap.c b/sys/arch/sparc64/sparc64/pmap.c
index fa3dfa9dab1..02d651c643a 100644
--- a/sys/arch/sparc64/sparc64/pmap.c
+++ b/sys/arch/sparc64/sparc64/pmap.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: pmap.c,v 1.55 2008/03/23 23:46:21 kettenis Exp $	*/
+/*	$OpenBSD: pmap.c,v 1.56 2008/03/30 12:30:01 kettenis Exp $	*/
 /*	$NetBSD: pmap.c,v 1.107 2001/08/31 16:47:41 eeh Exp $	*/
 #undef	NO_VCACHE /* Don't forget the locked TLB in dostart */
 /*
@@ -221,6 +221,16 @@ pa_to_pvh(paddr_t pa)
 	return pg ? &pg->mdpage.pvent : NULL;
 }
 
+static __inline u_int
+pmap_tte2flags(u_int64_t tte)
+{
+	if (CPU_ISSUN4V)
+		return (((tte & SUN4V_TLB_ACCESS) ? PV_REF : 0) |
+		    ((tte & SUN4V_TLB_MODIFY) ? PV_MOD : 0));
+	else
+		return (((tte & SUN4U_TLB_ACCESS) ? PV_REF : 0) |
+		    ((tte & SUN4U_TLB_MODIFY) ? PV_MOD : 0));
+}
 
 /*
  * Here's the CPU TSB stuff.  It's allocated in pmap_bootstrap.
@@ -234,7 +244,27 @@ int tsbsize;		/* tsbents = 512 * 2^^tsbsize */
  * The invalid tsb tag uses the fact that the last context we have is
  * never allocated.
  */
-#define TSB_TAG_INVALID (~0LL)
+#define TSB_TAG_INVALID	(~0LL << 48)
+
+#define TSB_DATA(g,sz,pa,priv,write,cache,aliased,valid,ie) \
+  (CPU_ISSUN4V ?\
+    SUN4V_TSB_DATA(g,sz,pa,priv,write,cache,aliased,valid,ie) : \
+    SUN4U_TSB_DATA(g,sz,pa,priv,write,cache,aliased,valid,ie))
+
+/* The same for sun4u and sun4v. */
+#define TLB_V		SUN4U_TLB_V
+
+/* Only used for DEBUG. */
+#define TLB_NFO		(CPU_ISSUN4V ? SUN4V_TLB_NFO : SUN4U_TLB_NFO)
+
+/*
+ * UltraSPARC T1 & T2 implement only a 40-bit real address range, just
+ * like older UltraSPARC CPUs.
+ */
+#define TLB_PA_MASK	SUN4U_TLB_PA_MASK
+
+/* XXX */
+#define TLB_TSB_LOCK	(CPU_ISSUN4V ? SUN4V_TLB_TSB_LOCK : SUN4U_TLB_TSB_LOCK)
 
 #ifdef SUN4V
 struct tsb_desc *tsb_desc;
@@ -1285,6 +1315,9 @@ remap_data:
 	BDPRINTF(PDB_BOOT1, ("Done inserting mesgbuf into pmap_kernel()\r\n"));
 	
 	BDPRINTF(PDB_BOOT1, ("Inserting PROM mappings into pmap_kernel()\r\n"));
+	data = 0;
+	if (CPU_ISSUN4U)
+		data = SUN4U_TLB_EXEC;
 	for (i = 0; i < prom_map_size; i++)
 		if (prom_map[i].vstart && ((prom_map[i].vstart>>32) == 0))
 			for (j = 0; j < prom_map[i].vsize; j += NBPG) {
@@ -1303,7 +1336,7 @@ remap_data:
 #endif
 				/* Enter PROM map into pmap_kernel() */
 				pmap_enter_kpage(prom_map[i].vstart + j,
-					(prom_map[i].tte + j)|TLB_EXEC|
+					(prom_map[i].tte + j)|data|
 					page_size_map[k].code);
 			}
 	BDPRINTF(PDB_BOOT1, ("Done inserting PROM mappings into pmap_kernel()\r\n"));
@@ -1717,12 +1750,7 @@ pmap_release(pm)
 							Debugger();
 #endif
 							/* Save REF/MOD info */
-							if (data & TLB_ACCESS)
-								pv->pv_va |=
-									PV_REF;
-							if (data & (TLB_MODIFY))
-								pv->pv_va |=
-									PV_MOD;
+							pv->pv_va |= pmap_tte2flags(data);
 
 							pmap_remove_pv(pm, 
 								       (long)((u_int64_t)i<<STSHIFT)|((long)k<<PDSHIFT)|((long)j<<PTSHIFT), 
@@ -1873,7 +1901,10 @@ pmap_activate(p)
 		write_user_windows();
 		if (pmap->pm_ctx == NULL)
 			ctx_alloc(pmap);
-		stxa(CTX_SECONDARY, ASI_DMMU, pmap->pm_ctx);
+		if (CPU_ISSUN4V)
+			stxa(CTX_SECONDARY, ASI_MMU_CONTEXTID, pmap->pm_ctx);
+		else
+			stxa(CTX_SECONDARY, ASI_DMMU, pmap->pm_ctx);
 	}
 	splx(s);
 }
@@ -1921,17 +1952,29 @@ pmap_kenter_pa(va, pa, prot)
 	enter_stats.unmanaged ++;
 #endif
 	tte.tag = TSB_TAG(0,pm->pm_ctx,va);
-	tte.data = TSB_DATA(0, PGSZ_8K, pa, 1 /* Privileged */,
-				 (VM_PROT_WRITE & prot),
-				 1, 0, 1, 0);
-	/*
-	 * We don't track modification on kenter mappings.
-	 */
-	if (prot & VM_PROT_WRITE)
-		tte.data |= TLB_REAL_W|TLB_W;
-	if (prot & VM_PROT_EXECUTE)
-		tte.data |= TLB_EXEC;
-	tte.data |= TLB_TSB_LOCK;	/* wired */
+	if (CPU_ISSUN4V) {
+		tte.data = SUN4V_TSB_DATA(0, PGSZ_8K, pa, 1 /* Privileged */,
+		    (VM_PROT_WRITE & prot), 1, 0, 1, 0);
+		/*
+		 * We don't track modification on kenter mappings.
+		 */
+		if (prot & VM_PROT_WRITE)
+			tte.data |= SUN4V_TLB_REAL_W|SUN4V_TLB_W;
+		if (prot & VM_PROT_EXECUTE)
+			tte.data |= SUN4V_TLB_EXEC;
+		tte.data |= SUN4V_TLB_TSB_LOCK;	/* wired */
+	} else {
+		tte.data = SUN4U_TSB_DATA(0, PGSZ_8K, pa, 1 /* Privileged */,
+		    (VM_PROT_WRITE & prot), 1, 0, 1, 0);
+		/*
+		 * We don't track modification on kenter mappings.
+		 */
+		if (prot & VM_PROT_WRITE)
+			tte.data |= SUN4U_TLB_REAL_W|SUN4U_TLB_W;
+		if (prot & VM_PROT_EXECUTE)
+			tte.data |= SUN4U_TLB_EXEC;
+		tte.data |= SUN4U_TLB_TSB_LOCK;	/* wired */
+	}
 	KDASSERT((tte.data & TLB_NFO) == 0);
 
 	/* Kernel page tables are pre-allocated. */
@@ -1981,10 +2024,6 @@ pmap_kremove(va, size)
 #endif
 		/* Shouldn't need to do this if the entry's not valid. */
 		if ((data = pseg_get(pm, va))) {
-			paddr_t entry;
-
-			flush |= 1;
-			entry = (data&TLB_PA_MASK);
 			/* We need to flip the valid bit and clear the access statistics. */
 			if (pseg_set(pm, va, 0, 0)) {
 				printf("pmap_kremove: gotten pseg empty!\n");
@@ -2005,6 +2044,7 @@ pmap_kremove(va, size)
 #endif
 			/* Here we assume nothing can get into the TLB unless it has a PTE */
 			tlb_flush_pte(va, pm->pm_ctx);
+			flush = 1;
 		}
 		va += NBPG;
 		size -= NBPG;
@@ -2075,10 +2115,11 @@ pmap_enter(pm, va, pa, prot, flags)
 			panic("pmap_enter: access_type exceeds prot");
 #endif
 		/* If we don't have the traphandler do it, set the ref/mod bits now */
-		if ((flags & VM_PROT_ALL) || (tte.data & TLB_ACCESS))
+		if (flags & VM_PROT_ALL)
 			pv->pv_va |= PV_REF;
-		if ((flags & VM_PROT_WRITE) || (tte.data & (TLB_MODIFY)))
+		if (flags & VM_PROT_WRITE)
 			pv->pv_va |= PV_MOD;
+		pv->pv_va |= pmap_tte2flags(tte.data);
 #ifdef DEBUG
 		enter_stats.managed ++;
 #endif
@@ -2095,15 +2136,27 @@ pmap_enter(pm, va, pa, prot, flags)
 #ifdef DEBUG
 	enter_stats.ci ++;
 #endif
-	tte.data = TSB_DATA(0, size, pa, pm == pmap_kernel(),
-		(flags & VM_PROT_WRITE), (!(pa & PMAP_NC)), 
-		aliased, 1, (pa & PMAP_LITTLE));
-	if (prot & VM_PROT_WRITE)
-		tte.data |= TLB_REAL_W;
-	if (prot & VM_PROT_EXECUTE)
-		tte.data |= TLB_EXEC;
-	if (wired)
-		tte.data |= TLB_TSB_LOCK;
+	if (CPU_ISSUN4V) {
+		tte.data = SUN4V_TSB_DATA(0, size, pa, pm == pmap_kernel(),
+		    (flags & VM_PROT_WRITE), (!(pa & PMAP_NC)), 
+		    aliased, 1, (pa & PMAP_LITTLE));
+		if (prot & VM_PROT_WRITE)
+			tte.data |= SUN4V_TLB_REAL_W;
+		if (prot & VM_PROT_EXECUTE)
+			tte.data |= SUN4V_TLB_EXEC;
+		if (wired)
+			tte.data |= SUN4V_TLB_TSB_LOCK;
+	} else {
+		tte.data = SUN4U_TSB_DATA(0, size, pa, pm == pmap_kernel(),
+		    (flags & VM_PROT_WRITE), (!(pa & PMAP_NC)), 
+		    aliased, 1, (pa & PMAP_LITTLE));
+		if (prot & VM_PROT_WRITE)
+			tte.data |= SUN4U_TLB_REAL_W;
+		if (prot & VM_PROT_EXECUTE)
+			tte.data |= SUN4U_TLB_EXEC;
+		if (wired)
+			tte.data |= SUN4U_TLB_TSB_LOCK;
+	}
 	KDASSERT((tte.data & TLB_NFO) == 0);
 
 	pg = NULL;
@@ -2184,9 +2237,7 @@ pmap_remove(pm, va, endva)
 			pv = pa_to_pvh(entry);
 			if (pv != NULL) {
 				/* Save REF/MOD info */
-				if (data & TLB_ACCESS) pv->pv_va |= PV_REF;
-				if (data & (TLB_MODIFY))  pv->pv_va |= PV_MOD;
-
+				pv->pv_va |= pmap_tte2flags(data);
 				pmap_remove_pv(pm, va, entry);
 			}
 			/* We need to flip the valid bit and clear the access statistics. */
@@ -2276,26 +2327,25 @@ pmap_protect(pm, sva, eva, prot)
 			if (pmapdebug & (PDB_CHANGEPROT|PDB_REF))
 				printf("pmap_protect: va=%08x data=%x:%08x seg=%08x pte=%08x\r\n", 
 					    (u_int)sva, (int)(pa>>32), (int)pa, (int)va_to_seg(sva), (int)va_to_pte(sva));
-/* Catch this before the assertion */
-			if (data & TLB_NFO) {
-				printf("pmap_protect: pm=%p  NFO mapping va=%x data=%x:%x\n",
-				       pm, (u_int)sva, (int)(data>>32), (int)data);
-				Debugger();
 			}
 #endif
 			pv = pa_to_pvh(pa);
 			if (pv != NULL) {
 				/* Save REF/MOD info */
-				if (data & TLB_ACCESS)
-					pv->pv_va |= PV_REF;
-				if (data & (TLB_MODIFY))  
-					pv->pv_va |= PV_MOD;
+				pv->pv_va |= pmap_tte2flags(data);
 			}
 			/* Just do the pmap and TSB, not the pv_list */
-			if ((prot & VM_PROT_WRITE) == 0)
-				data &= ~(TLB_W|TLB_REAL_W);
-			if ((prot & VM_PROT_EXECUTE) == 0)
-				data &= ~(TLB_EXEC);
+			if (CPU_ISSUN4V) {
+				if ((prot & VM_PROT_WRITE) == 0)
+					data &= ~(SUN4V_TLB_W|SUN4V_TLB_REAL_W);
+				if ((prot & VM_PROT_EXECUTE) == 0)
+					data &= ~(SUN4V_TLB_EXEC);
+			} else {
+				if ((prot & VM_PROT_WRITE) == 0)
+					data &= ~(SUN4U_TLB_W|SUN4U_TLB_REAL_W);
+				if ((prot & VM_PROT_EXECUTE) == 0)
+					data &= ~(SUN4U_TLB_EXEC);
+			}
 			KDASSERT((data & TLB_NFO) == 0);
 			if (pseg_set(pm, sva, data, 0)) {
 				printf("pmap_protect: gotten pseg empty!\n");
@@ -2345,6 +2395,14 @@ pmap_extract(pm, va, pap)
 			    (u_long)va, (unsigned long long)pa);
 		}
 #endif
+	} else if (pm == pmap_kernel() && va >= INTSTACK && va < EINTSTACK) {
+		pa = curcpu()->ci_paddr + va - INTSTACK;
+#ifdef DEBUG
+		if (pmapdebug & PDB_EXTRACT) {
+			printf("pmap_extract: va=%lx pa=%llx\n",
+			    (u_long)va, (unsigned long long)pa);
+		}
+#endif
 	} else {
 		int s;
 
@@ -2632,9 +2690,15 @@ pmap_clear_modify(pg)
 			/* First clear the mod bit in the PTE and make it R/O */
 			data = pseg_get(pv->pv_pmap, pv->pv_va&PV_VAMASK);
 			/* Need to both clear the modify and write bits */
-			if (data & (TLB_MODIFY))
-				changed |= 1;
-			data &= ~(TLB_MODIFY|TLB_W);
+			if (CPU_ISSUN4V) {
+				if (data & (SUN4V_TLB_MODIFY))
+					changed |= 1;
+				data &= ~(SUN4V_TLB_MODIFY|SUN4V_TLB_W);
+			} else {
+				if (data & (SUN4U_TLB_MODIFY))
+					changed |= 1;
+				data &= ~(SUN4U_TLB_MODIFY|SUN4U_TLB_W);
+			}
 			KDASSERT((data & TLB_NFO) == 0);
 			if (pseg_set(pv->pv_pmap, pv->pv_va&PV_VAMASK, data, 0)) {
 				printf("pmap_clear_modify: gotten pseg empty!\n");
@@ -2715,9 +2779,15 @@ pmap_clear_reference(struct vm_page *pg)
 				printf("clearing ref pm:%p va:%p ctx:%lx data:%x:%x\n", pv->pv_pmap,
 				       (void *)(u_long)pv->pv_va, (u_long)pv->pv_pmap->pm_ctx, (int)(data>>32), (int)data);
 #endif
-			if (data & TLB_ACCESS)
-				changed |= 1;
-			data &= ~TLB_ACCESS;
+			if (CPU_ISSUN4V) {
+				if (data & SUN4V_TLB_ACCESS)
+					changed |= 1;
+				data &= ~SUN4V_TLB_ACCESS;
+			} else {
+				if (data & SUN4U_TLB_ACCESS)
+					changed |= 1;
+				data &= ~SUN4U_TLB_ACCESS;
+			}
 			KDASSERT((data & TLB_NFO) == 0);
 			if (pseg_set(pv->pv_pmap, pv->pv_va, data, 0)) {
 				printf("pmap_clear_reference: gotten pseg empty!\n");
@@ -2781,7 +2851,7 @@ pmap_is_modified(struct vm_page *pg)
 			int64_t data;
 			
 			data = pseg_get(npv->pv_pmap, npv->pv_va&PV_VAMASK);
-			if (data & (TLB_MODIFY)) i = 1;
+			if (pmap_tte2flags(data) & PV_MOD) i = 1;
 			/* Migrate modify info to head pv */
 			if (npv->pv_va & PV_MOD) i = 1;
 			npv->pv_va &= ~PV_MOD;
@@ -2825,7 +2895,7 @@ pmap_is_referenced(struct vm_page* pg)
 			int64_t data;
 			
 			data = pseg_get(npv->pv_pmap, npv->pv_va&PV_VAMASK);
-			if (data & TLB_ACCESS) i = 1;
+			if (pmap_tte2flags(data) & PV_REF) i = 1;
 			/* Migrate modify info to head pv */
 			if (npv->pv_va & PV_REF) i = 1;
 			npv->pv_va &= ~PV_REF;
@@ -2884,7 +2954,10 @@ pmap_unwire(pmap, va)
 	simple_lock(&pmap->pm_lock);
 	data = pseg_get(pmap, va&PV_VAMASK);
 
-	data &= ~TLB_TSB_LOCK;
+	if (CPU_ISSUN4V)
+		data &= ~SUN4V_TLB_TSB_LOCK;
+	else
+		data &= ~SUN4U_TLB_TSB_LOCK;
 
 	if (pseg_set(pmap, va&PV_VAMASK, data, 0)) {
 		printf("pmap_unwire: gotten pseg empty!\n");
@@ -2928,13 +3001,21 @@ pmap_page_protect(pg, prot)
 		/* copy_on_write */
 
 		set = TLB_V;
-		clear = TLB_REAL_W|TLB_W;
-		if (VM_PROT_EXECUTE & prot)
-			set |= TLB_EXEC;
-		else
-			clear |= TLB_EXEC;
-		if (VM_PROT_EXECUTE == prot)
-			set |= TLB_EXEC_ONLY;
+		if (CPU_ISSUN4V) {
+			clear = SUN4V_TLB_REAL_W|SUN4V_TLB_W;
+			if (VM_PROT_EXECUTE & prot)
+				set |= SUN4V_TLB_EXEC;
+			else
+				clear |= SUN4V_TLB_EXEC;
+		} else {
+			clear = SUN4U_TLB_REAL_W|SUN4U_TLB_W;
+			if (VM_PROT_EXECUTE & prot)
+				set |= SUN4U_TLB_EXEC;
+			else
+				clear |= SUN4U_TLB_EXEC;
+			if (VM_PROT_EXECUTE == prot)
+				set |= SUN4U_TLB_EXEC_ONLY;
+		}
 
 		pv = pa_to_pvh(pa);
 		s = splvm();
@@ -2964,9 +3045,7 @@ pmap_page_protect(pg, prot)
 				data = pseg_get(pv->pv_pmap, pv->pv_va&PV_VAMASK);
 
 				/* Save REF/MOD info */
-				if (data & TLB_ACCESS) pv->pv_va |= PV_REF;
-				if (data & (TLB_MODIFY))  
-					pv->pv_va |= PV_MOD;
+				pv->pv_va |= pmap_tte2flags(data);
 
 				data &= ~(clear);
 				data |= (set);
@@ -3018,11 +3097,8 @@ pmap_page_protect(pg, prot)
 			/* clear the entry in the page table */
 			data = pseg_get(npv->pv_pmap, npv->pv_va&PV_VAMASK);
 
-			/* Save ref/mod info */
-			if (data & TLB_ACCESS) 
-				firstpv->pv_va |= PV_REF;
-			if (data & (TLB_MODIFY))
-				firstpv->pv_va |= PV_MOD;
+			/* Save REF/MOD info */
+			firstpv->pv_va |= pmap_tte2flags(data);
 			if (data & TLB_TSB_LOCK) {
 #ifdef DIAGNOSTIC
 				printf("pmap_page_protect: wired page pm %p va %p not removed\n",
@@ -3068,11 +3144,8 @@ pmap_page_protect(pg, prot)
 			}
 #endif
 			data = pseg_get(pv->pv_pmap, pv->pv_va&PV_VAMASK);
-			/* Save ref/mod info */
-			if (data & TLB_ACCESS) 
-				pv->pv_va |= PV_REF;
-			if (data & (TLB_MODIFY))
-				pv->pv_va |= PV_MOD;
+			/* Save REF/MOD info */
+			pv->pv_va |= pmap_tte2flags(data);
 			if (pseg_set(pv->pv_pmap, pv->pv_va&PV_VAMASK, 0, 0)) {
 				printf("pmap_page_protect: gotten pseg empty!\n");
 				Debugger();
@@ -3424,11 +3497,8 @@ pmap_remove_pv(pmap, va, pa)
 		pool_put(&pv_pool, npv);
 	}
 
-	/* Save ref/mod info */
-	if (data & TLB_ACCESS) 
-		opv->pv_va |= PV_REF;
-	if (data & (TLB_MODIFY))
-		opv->pv_va |= PV_MOD;
+	/* Save REF/MOD info */
+	opv->pv_va |= pmap_tte2flags(data);
 
 	/* Check to see if the alias went away */
 	if (opv->pv_va & PV_ALIAS) {
@@ -3459,6 +3529,9 @@ pmap_page_cache(pm, pa, mode)
 	pv_entry_t pv;
 	int s;
 
+	if (CPU_ISSUN4V)
+		return;
+
 #ifdef DEBUG
 	if (pmapdebug & (PDB_ENTER))
 		printf("pmap_page_uncache(%llx)\n", (unsigned long long)pa);
@@ -3477,8 +3550,8 @@ pmap_page_cache(pm, pa, mode)
 			simple_lock(&pv->pv_pmap->pm_lock);
 		if (pv->pv_va & PV_NC) {
 			/* Non-cached -- I/O mapping */
-			if (pseg_set(pv->pv_pmap, va, 
-				     pseg_get(pv->pv_pmap, va) & ~(TLB_CV|TLB_CP), 
+			if (pseg_set(pv->pv_pmap, va,
+			    pseg_get(pv->pv_pmap, va) & ~(SUN4U_TLB_CV|SUN4U_TLB_CP),
 				     0)) {
 				printf("pmap_page_cache: aliased pseg empty!\n");
 				Debugger();
@@ -3486,16 +3559,16 @@ pmap_page_cache(pm, pa, mode)
 			}
 		} else if (mode && (!(pv->pv_va & PV_NVC))) {
 			/* Enable caching */
-			if (pseg_set(pv->pv_pmap, va, 
-				     pseg_get(pv->pv_pmap, va) | TLB_CV, 0)) {
+			if (pseg_set(pv->pv_pmap, va,
+			    pseg_get(pv->pv_pmap, va) | SUN4U_TLB_CV, 0)) {
 				printf("pmap_page_cache: aliased pseg empty!\n");
 				Debugger();
 				/* panic? */
 			}
 		} else {
 			/* Disable caching */
-			if (pseg_set(pv->pv_pmap, va, 
-				     pseg_get(pv->pv_pmap, va) & ~TLB_CV, 0)) {
+			if (pseg_set(pv->pv_pmap, va,
+			    pseg_get(pv->pv_pmap, va) & ~SUN4U_TLB_CV, 0)) {
 				printf("pmap_page_cache: aliased pseg empty!\n");
 				Debugger();
 				/* panic? */