Add pmap support for user mappings of multiple page sizes (super pages).

This supports all hardware page sizes (8K, 64K, 512K, 4MB), but only 8k
pages are actually used as of yet.

10 files changed, 481 insertions, 380 deletions

diff --git a/sys/sparc64/include/asmacros.h b/sys/sparc64/include/asmacros.h
index a719efd7b004..540d28f3d0a8 100644
--- a/sys/sparc64/include/asmacros.h
+++ b/sys/sparc64/include/asmacros.h
@@ -44,11 +44,6 @@
  */
 #define	ASP_REG		%g5
 
-/*
- * MMU %g7 points to the user tsb.
- */
-#define	TSB_REG		%g7
-
 #ifdef LOCORE
 
 /*
diff --git a/sys/sparc64/include/tsb.h b/sys/sparc64/include/tsb.h
index 20db029aff9d..9a817c20376f 100644
--- a/sys/sparc64/include/tsb.h
+++ b/sys/sparc64/include/tsb.h
@@ -55,9 +55,9 @@ tsb_vpntobucket(pmap_t pm, vm_offset_t vpn)
 }
 
 static __inline struct tte *
-tsb_vtobucket(pmap_t pm, vm_offset_t va)
+tsb_vtobucket(pmap_t pm, u_long sz, vm_offset_t va)
 {
-	return (tsb_vpntobucket(pm, va >> PAGE_SHIFT));
+	return (tsb_vpntobucket(pm, va >> TTE_PAGE_SHIFT(sz)));
 }
 
 static __inline struct tte *
@@ -77,7 +77,7 @@ typedef int (tsb_callback_t)(struct pmap *, struct pmap *, struct tte *,
 
 struct	tte *tsb_tte_lookup(pmap_t pm, vm_offset_t va);
 void	tsb_tte_remove(struct tte *stp);
-struct	tte *tsb_tte_enter(pmap_t pm, vm_page_t m, vm_offset_t va,
+struct	tte *tsb_tte_enter(pmap_t pm, vm_page_t m, vm_offset_t va, u_long sz,
 			   u_long data);
 void	tsb_tte_local_remove(struct tte *tp);
 void	tsb_foreach(pmap_t pm1, pmap_t pm2, vm_offset_t start, vm_offset_t end,
diff --git a/sys/sparc64/include/tte.h b/sys/sparc64/include/tte.h
index 13a445a8b87d..e86c722f8793 100644
--- a/sys/sparc64/include/tte.h
+++ b/sys/sparc64/include/tte.h
@@ -52,27 +52,27 @@
 #define	TD_PA_MASK	((1UL << TD_PA_BITS) - 1)
 #define	TD_SOFT_MASK	((1UL << TD_SOFT_BITS) - 1)
 
-#define	TD_SIZE_SPREAD	(3)
+#define	TS_8K		(0UL)
+#define	TS_64K		(1UL)
+#define	TS_512K		(2UL)
+#define	TS_4M		(3UL)
 
-#define	TS_EXEC		(1UL << 4)
-#define	TS_REF		(1UL << 3)
-#define	TS_PV		(1UL << 2)
-#define	TS_W		(1UL << 1)
-#define	TS_WIRED	(1UL << 0)
+#define	TS_MIN		TS_8K
+#define	TS_MAX		TS_4M
 
 #define	TD_V		(1UL << 63)
-#define	TD_8K		(0UL << TD_SIZE_SHIFT)
-#define	TD_64K		(1UL << TD_SIZE_SHIFT)
-#define	TD_512K		(2UL << TD_SIZE_SHIFT)
-#define	TD_4M		(3UL << TD_SIZE_SHIFT)
+#define	TD_8K		(TS_8K << TD_SIZE_SHIFT)
+#define	TD_64K		(TS_64K << TD_SIZE_SHIFT)
+#define	TD_512K		(TS_512K << TD_SIZE_SHIFT)
+#define	TD_4M		(TS_4M << TD_SIZE_SHIFT)
 #define	TD_NFO		(1UL << 60)
 #define	TD_IE		(1UL << 59)
 #define	TD_PA(pa)	((pa) & (TD_PA_MASK << TD_PA_SHIFT))
-#define	TD_EXEC		(TS_EXEC << TD_SOFT_SHIFT)
-#define	TD_REF		(TS_REF << TD_SOFT_SHIFT)
-#define	TD_PV		(TS_PV << TD_SOFT_SHIFT)
-#define	TD_SW		(TS_W << TD_SOFT_SHIFT)
-#define	TD_WIRED	(TS_WIRED << TD_SOFT_SHIFT)
+#define	TD_EXEC		((1UL << 4) << TD_SOFT_SHIFT)
+#define	TD_REF		((1UL << 3) << TD_SOFT_SHIFT)
+#define	TD_PV		((1UL << 2) << TD_SOFT_SHIFT)
+#define	TD_SW		((1UL << 1) << TD_SOFT_SHIFT)
+#define	TD_WIRED	((1UL << 0) << TD_SOFT_SHIFT)
 #define	TD_L		(1UL << 6)
 #define	TD_CP		(1UL << 5)
 #define	TD_CV		(1UL << 4)
@@ -81,12 +81,17 @@
 #define	TD_W		(1UL << 1)
 #define	TD_G		(1UL << 0)
 
-#define	TV_VPN(va)	((va) >> PAGE_SHIFT)
+#define	TV_SIZE_BITS	(TD_SIZE_BITS)
+#define	TV_VPN(va, sz)	((((va) >> TTE_PAGE_SHIFT(sz)) << TV_SIZE_BITS) | sz)
+
+#define	TTE_SIZE_SPREAD	(3)
+#define	TTE_PAGE_SHIFT(sz) \
+	(PAGE_SHIFT + ((sz) * TTE_SIZE_SPREAD))
 
 #define	TTE_GET_SIZE(tp) \
 	(((tp)->tte_data >> TD_SIZE_SHIFT) & TD_SIZE_MASK)
 #define	TTE_GET_PAGE_SHIFT(tp) \
-	(PAGE_SHIFT + (TTE_GET_SIZE(tp) * TD_SIZE_SPREAD))
+	TTE_PAGE_SHIFT(TTE_GET_SIZE(tp))
 #define	TTE_GET_PAGE_SIZE(tp) \
 	(1 << TTE_GET_PAGE_SHIFT(tp))
 #define	TTE_GET_PAGE_MASK(tp) \
@@ -94,13 +99,17 @@
 
 #define	TTE_GET_PA(tp) \
 	((tp)->tte_data & (TD_PA_MASK << TD_PA_SHIFT))
+#define	TTE_GET_VPN(tp) \
+	((tp)->tte_vpn >> TV_SIZE_BITS)
 #define	TTE_GET_VA(tp) \
-	((tp)->tte_vpn << PAGE_SHIFT)
+	(TTE_GET_VPN(tp) << TTE_GET_PAGE_SHIFT(tp))
 #define	TTE_GET_PMAP(tp) \
 	((tp)->tte_pmap)
 #define	TTE_ZERO(tp) \
 	bzero(tp, sizeof(*tp))
 
+struct pmap;
+
 struct tte {
 	u_long	tte_vpn;
 	u_long	tte_data;
@@ -109,15 +118,10 @@ struct tte {
 };
 
 static __inline int
-tte_match_vpn(struct tte *tp, vm_offset_t vpn)
-{
-	return ((tp->tte_data & TD_V) != 0 && tp->tte_vpn == vpn);
-}
-
-static __inline int
 tte_match(struct tte *tp, vm_offset_t va)
 {
-	return (tte_match_vpn(tp, va >> PAGE_SHIFT));
+	return (((tp->tte_data & TD_V) != 0) &&
+	    (tp->tte_vpn == TV_VPN(va, TTE_GET_SIZE(tp))));
 }
 
 #endif /* !_MACHINE_TTE_H_ */
diff --git a/sys/sparc64/sparc64/exception.S b/sys/sparc64/sparc64/exception.S
index 7de0f1ffbc74..fa19ad7e3d71 100644
--- a/sys/sparc64/sparc64/exception.S
+++ b/sys/sparc64/sparc64/exception.S
@@ -554,45 +554,62 @@ END(tl0_sfsr_trap)
 
 	.macro	immu_miss_user
 	/*
+	 * Initialize the page size walker.
+	 */
+	mov	TS_MIN, %g2
+
+	/*
+	 * Loop over all supported page sizes.
+	 */
+
+	/*
+	 * Compute the page shift for the page size we are currently looking
+	 * for.
+	 */
+1:	add	%g2, %g2, %g3
+	add	%g3, %g2, %g3
+	add	%g3, PAGE_SHIFT, %g3
+
+	/*
 	 * Extract the virtual page number from the contents of the tag
 	 * access register.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+	srlx	%g1, %g3, %g3
 
 	/*
 	 * Compute the tte bucket address.
 	 */
-	set	(1 << TSB_BUCKET_ADDRESS_BITS) - 1, %g1
-	and	%g1, %g3, %g1
-	sllx	%g1, TSB_BUCKET_SHIFT + TTE_SHIFT, %g1
-	add	%g1, TSB_REG, %g1
+	ldxa	[%g0 + AA_IMMU_TSB] %asi, %g5
+	and	%g3, TSB_BUCKET_MASK, %g4
+	sllx	%g4, TSB_BUCKET_SHIFT + TTE_SHIFT, %g4
+	add	%g4, %g5, %g4
 
 	/*
-	 * Loop over the ttes in this bucket
+	 * Compute the tte tag target.
 	 */
+	sllx	%g3, TV_SIZE_BITS, %g3
+	or	%g3, %g2, %g3
 
 	/*
-	 * Load the tte.  Note that this instruction may fault, clobbering
-	 * the context of the tag access register (at least), and the contents
-	 * of %g3, %g4, %g5, and %g6.  Luckily we can recover %g3, and we do
-	 * not use %g4 or %g5 until this instruction completes successfully.
+	 * Loop over the ttes in this bucket
 	 */
-1:	ldda	[%g1] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
 
 	/*
-	 * Recover the virtual page number, which may have been clobbered.
+	 * Load the tte.  Note that this instruction may fault, clobbering
+	 * the contents of the tag access register, %g5, %g6, and %g7.  We
+	 * do not use %g5, and %g6 and %g7 are not used until this instruction
+	 * completes successfully.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+2:	ldda	[%g4] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
-	 * Check that its valid and executable and that the virtual page
-	 * numbers match.
+	 * Check that its valid and executable and that the tte tags match.
 	 */
-	brgez,pn %g5, 2f
-	 andcc	%g5, TD_EXEC, %g0
-	bz,pn	%xcc, 2f
-	 cmp	%g3, %g4
-	bne,pn %xcc, 2f
+	brgez,pn %g7, 3f
+	 andcc	%g7, TD_EXEC, %g0
+	bz,pn	%xcc, 3f
+	 cmp	%g3, %g6
+	bne,pn	%xcc, 3f
 	 EMPTY
 
 	/*
@@ -602,43 +619,52 @@ END(tl0_sfsr_trap)
 	/*
 	 * Set the reference bit, if it's currently clear.
 	 */
-	 andcc	%g5, TD_REF, %g0
+	 andcc	%g7, TD_REF, %g0
 	bz,a,pn	%xcc, tl0_immu_miss_set_ref
 	 nop
 
 	/*
 	 * Load the tte tag and data into the tlb and retry the instruction.
 	 */
-	stxa	%g2, [%g0 + AA_IMMU_TAR] %asi
-	stxa	%g5, [%g0] ASI_ITLB_DATA_IN_REG
+	stxa	%g1, [%g0 + AA_IMMU_TAR] %asi
+	stxa	%g7, [%g0] ASI_ITLB_DATA_IN_REG
 	retry
 
 	/*
-	 * Check the low bits to see if we've finished the bucket.
+	 * Advance to the next tte in this bucket, and check the low bits
+	 * of the bucket pointer to see if we've finished the bucket.
 	 */
-2:	add	%g1, 1 << TTE_SHIFT, %g1
-	andcc	%g1, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
-	bnz,a,pt %xcc, 1b
-	 nop
-	.endm
+3:	add	%g4, 1 << TTE_SHIFT, %g4
+	andcc	%g4, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
+	bnz,pt	%xcc, 2b
+	 EMPTY
 
-	.macro	tl0_immu_miss
 	/*
-	 * Force kernel store order.
+	 * See if we just checked the largest page size, and advance to the
+	 * next one if not.
 	 */
-	wrpr	%g0, PSTATE_MMU, %pstate
+	 cmp	%g2, TS_MAX
+	bne,pt	%xcc, 1b
+	 add	%g2, 1, %g2
+	.endm
 
+	.macro	tl0_immu_miss
 	/*
 	 * Load the virtual page number and context from the tag access
 	 * register.  We ignore the context.
 	 */
 	wr	%g0, ASI_IMMU, %asi
-	ldxa	[%g0 + AA_IMMU_TAR] %asi, %g2
+	ldxa	[%g0 + AA_IMMU_TAR] %asi, %g1
 
+	/*
+	 * Try a fast inline lookup of the user tsb.
+	 */
 	immu_miss_user
 
-	b,a	%xcc, tl0_immu_miss_trap
-	 nop
+	/*
+	 * Not in user tsb, call c code.
+	 */
+	ba,a	%xcc, tl0_immu_miss_trap
 	.align	128
 	.endm
 
@@ -646,19 +672,19 @@ ENTRY(tl0_immu_miss_set_ref)
 	/*
 	 * Set the reference bit.
 	 */
-	TTE_SET_REF(%g1, %g4, %g5)
+	TTE_SET_REF(%g4, %g2, %g3)
 
 	/*
-	 * May have become invalid, in which case start over.
+	 * May have become invalid during casxa, in which case start over.
 	 */
-	brgez,pn %g4, 1f
-	 or	%g4, TD_REF, %g4
+	brgez,pn %g2, 1f
+	 nop
 
 	/*
 	 * Load the tte tag and data into the tlb and retry the instruction.
 	 */
-	stxa	%g2, [%g0 + AA_IMMU_TAR] %asi
-	stxa	%g4, [%g0] ASI_ITLB_DATA_IN_REG
+	stxa	%g1, [%g0 + AA_IMMU_TAR] %asi
+	stxa	%g2, [%g0] ASI_ITLB_DATA_IN_REG
 1:	retry
 END(tl0_immu_miss_set_ref)
 
@@ -667,7 +693,7 @@ ENTRY(tl0_immu_miss_trap)
 	 * Put back the contents of the tag access register, in case we
 	 * faulted.
 	 */
-	stxa	%g2, [%g0 + AA_IMMU_TAR] %asi
+	stxa	%g1, [%g0 + AA_IMMU_TAR] %asi
 	membar	#Sync
 
 	/*
@@ -691,42 +717,60 @@ END(tl0_immu_miss_trap)
 
 	.macro	dmmu_miss_user
 	/*
+	 * Initialize the page size walker.
+	 */
+	mov	TS_MIN, %g2
+
+	/*
+	 * Loop over all supported page sizes.
+	 */
+
+	/*
+	 * Compute the page shift for the page size we are currently looking
+	 * for.
+	 */
+1:	add	%g2, %g2, %g3
+	add	%g3, %g2, %g3
+	add	%g3, PAGE_SHIFT, %g3
+
+	/*
 	 * Extract the virtual page number from the contents of the tag
 	 * access register.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+	srlx	%g1, %g3, %g3
 
 	/*
 	 * Compute the tte bucket address.
 	 */
-	set	(1 << TSB_BUCKET_ADDRESS_BITS) - 1, %g1
-	and	%g1, %g3, %g1
-	sllx	%g1, TSB_BUCKET_SHIFT + TTE_SHIFT, %g1
-	add	%g1, TSB_REG, %g1
+	ldxa	[%g0 + AA_DMMU_TSB] %asi, %g5
+	and	%g3, TSB_BUCKET_MASK, %g4
+	sllx	%g4, TSB_BUCKET_SHIFT + TTE_SHIFT, %g4
+	add	%g4, %g5, %g4
 
 	/*
-	 * Loop over the ttes in this bucket
+	 * Compute the tte tag target.
 	 */
+	sllx	%g3, TV_SIZE_BITS, %g3
+	or	%g3, %g2, %g3
 
 	/*
-	 * Load the tte.  Note that this instruction may fault, clobbering
-	 * the contents of the tag access register (at least), and the contents
-	 * of %g3, %g4, %g5, and %g6.  Luckily we can recover %g3, and we do
-	 * not use %g4 or %g5 until this instruction completes successfully.
+	 * Loop over the ttes in this bucket
 	 */
-1:	ldda	[%g1] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
 
 	/*
-	 * Recover the virtual page number, which may have been clobbered.
+	 * Load the tte.  Note that this instruction may fault, clobbering
+	 * the contents of the tag access register, %g5, %g6, and %g7.  We
+	 * do not use %g5, and %g6 and %g7 are not used until this instruction
+	 * completes successfully.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+2:	ldda	[%g4] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
 	 * Check that its valid and that the virtual page numbers match.
 	 */
-	brgez,pn %g5, 2f
-	 cmp	%g3, %g4
-	bne,pn %xcc, 2f
+	brgez,pn %g7, 3f
+	 cmp	%g3, %g6
+	bne,pn	%xcc, 3f
 	 EMPTY
 
 	/*
@@ -736,58 +780,62 @@ END(tl0_immu_miss_trap)
 	/*
 	 * Set the reference bit, if it's currently clear.
 	 */
-	 andcc	%g5, TD_REF, %g0
+	 andcc	%g7, TD_REF, %g0
 	bz,a,pn	%xcc, dmmu_miss_user_set_ref
 	 nop
 
 	/*
 	 * Load the tte tag and data into the tlb and retry the instruction.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
-	stxa	%g5, [%g0] ASI_DTLB_DATA_IN_REG
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g7, [%g0] ASI_DTLB_DATA_IN_REG
 	retry
 
 	/*
-	 * Check the low bits to see if we've finished the bucket.
+	 * Advance to the next tte in this bucket, and check the low bits
+	 * of the bucket pointer to see if we've finished the bucket.
 	 */
-2:	add	%g1, 1 << TTE_SHIFT, %g1
-	andcc	%g1, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
-	bnz,a,pt %xcc, 1b
-	 nop
+3:	add	%g4, 1 << TTE_SHIFT, %g4
+	andcc	%g4, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
+	bnz,pt	%xcc, 2b
+	 EMPTY
+
+	/*
+	 * See if we just checked the largest page size, and advance to the
+	 * next one if not.
+	 */
+	 cmp	%g2, TS_MAX
+	bne,pt	%xcc, 1b
+	 add	%g2, 1, %g2
 	.endm
 
 ENTRY(dmmu_miss_user_set_ref)
 	/*
 	 * Set the reference bit.
 	 */
-	TTE_SET_REF(%g1, %g4, %g5)
+	TTE_SET_REF(%g4, %g2, %g3)
 
 	/*
-	 * May have become invalid, in which case start over.
+	 * May have become invalid during casxa, in which case start over.
 	 */
-	brgez,pn %g4, 1f
-	 or	%g4, TD_REF, %g4
+	brgez,pn %g2, 1f
+	 nop
 
 	/*
 	 * Load the tte tag and data into the tlb and retry the instruction.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
-	stxa	%g4, [%g0] ASI_DTLB_DATA_IN_REG
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g2, [%g0] ASI_DTLB_DATA_IN_REG
 1:	retry
 END(dmmu_miss_user_set_ref)
 
 	.macro	tl0_dmmu_miss
 	/*
-	 * Force kernel store order.
-	 */
-	wrpr	%g0, PSTATE_MMU, %pstate
-
-	/*
 	 * Load the virtual page number and context from the tag access
 	 * register.  We ignore the context.
 	 */
 	wr	%g0, ASI_DMMU, %asi
-	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g2
+	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g1
 
 	/*
 	 * Try a fast inline lookup of the primary tsb.
@@ -795,10 +843,9 @@ END(dmmu_miss_user_set_ref)
 	dmmu_miss_user
 
 	/*
-	 * Not in primary tsb, call c code.  Not much else fits inline.
+	 * Not in user tsb, call c code.
 	 */
-	b,a	%xcc, tl0_dmmu_miss_trap
-	 nop
+	ba,a	%xcc, tl0_dmmu_miss_trap
 	.align	128
 	.endm
 
@@ -807,7 +854,7 @@ ENTRY(tl0_dmmu_miss_trap)
 	 * Put back the contents of the tag access register, in case we
 	 * faulted.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
 	membar	#Sync
 
 	/*
@@ -831,122 +878,141 @@ END(tl0_dmmu_miss_trap)
 
 	.macro	dmmu_prot_user
 	/*
+	 * Initialize the page size walker.
+	 */
+	mov	TS_MIN, %g2
+
+	/*
+	 * Loop over all supported page sizes.
+	 */
+
+	/*
+	 * Compute the page shift for the page size we are currently looking
+	 * for.
+	 */
+1:	add	%g2, %g2, %g3
+	add	%g3, %g2, %g3
+	add	%g3, PAGE_SHIFT, %g3
+
+	/*
 	 * Extract the virtual page number from the contents of the tag
 	 * access register.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+	srlx	%g1, %g3, %g3
 
 	/*
 	 * Compute the tte bucket address.
 	 */
-	set	(1 << TSB_BUCKET_ADDRESS_BITS) - 1, %g1
-	and	%g1, %g3, %g1
-	sllx	%g1, TSB_BUCKET_SHIFT + TTE_SHIFT, %g1
-	add	%g1, TSB_REG, %g1
+	ldxa	[%g0 + AA_DMMU_TSB] %asi, %g5
+	and	%g3, TSB_BUCKET_MASK, %g4
+	sllx	%g4, TSB_BUCKET_SHIFT + TTE_SHIFT, %g4
+	add	%g4, %g5, %g4
 
 	/*
-	 * Loop over the ttes in this bucket
+	 * Compute the tte tag target.
 	 */
+	sllx	%g3, TV_SIZE_BITS, %g3
+	or	%g3, %g2, %g3
 
 	/*
-	 * Load the tte.  Note that this instruction may fault, clobbering
-	 * the context of the tag access register (at least), and the contents
-	 * of %g3, %g4, %g5, and %g6.  Luckily we can recover %g3, and we do
-	 * not use %g4 or %g5 until this instruction completes successfully.
+	 * Loop over the ttes in this bucket
 	 */
-1:	ldda	[%g1] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
 
 	/*
-	 * Recover the virtual page number, which may have been clobbered.
+	 * Load the tte.  Note that this instruction may fault, clobbering
+	 * the contents of the tag access register, %g5, %g6, and %g7.  We
+	 * do not use %g5, and %g6 and %g7 are not used until this instruction
+	 * completes successfully.
 	 */
-	srlx	%g2, TAR_VPN_SHIFT, %g3
+2:	ldda	[%g4] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
 	 * Check that its valid and writable and that the virtual page
 	 * numbers match.
 	 */
-	brgez,pn %g5, 2f
-	 andcc	%g5, TD_SW, %g0
-	bz,pn	%xcc, 2f
-	 cmp	%g3, %g4
-	bne,pn %xcc, 2f
+	brgez,pn %g7, 4f
+	 andcc	%g7, TD_SW, %g0
+	bz,pn	%xcc, 4f
+	 cmp	%g3, %g6
+	bne,pn	%xcc, 4f
 	 nop
 
 	/*
 	 * Set the hardware write bit.
 	 */
-	b,a	%xcc, dmmu_prot_set_w
-	 nop
+	TTE_SET_W(%g4, %g2, %g3)
 
 	/*
-	 * Check the low bits to see if we've finished the bucket.
+	 * Delete the old TLB entry and clear the sfsr.
 	 */
-2:	add	%g1, 1 << TTE_SHIFT, %g1
-	andcc	%g1, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
-	bnz,a,pt %xcc, 1b
-	 nop
-	.endm
+	srlx	%g1, PAGE_SHIFT, %g3
+	sllx	%g3, PAGE_SHIFT, %g3
+	stxa	%g0, [%g3] ASI_DMMU_DEMAP
+	stxa	%g0, [%g0 + AA_DMMU_SFSR] %asi
+	membar	#Sync
 
-	.macro	tl0_dmmu_prot
 	/*
-	 * Force kernel store order.
+	 * May have become invalid during casxa, in which case start over.
 	 */
-	wrpr	%g0, PSTATE_MMU, %pstate
+	brgez,pn %g2, 3f
+	 or	%g2, TD_W, %g2
 
 	/*
-	 * Load the virtual page number and context from the tag access
-	 * register.  We ignore the context.
+	 * Load the tte data into the tlb and retry the instruction.
 	 */
-	wr	%g0, ASI_DMMU, %asi
-	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g2
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g2, [%g0] ASI_DTLB_DATA_IN_REG
+3:	retry
 
 	/*
-	 * Try a fast inline lookup of the tsb.
+	 * Check the low bits to see if we've finished the bucket.
 	 */
-	dmmu_prot_user
+4:	add	%g4, 1 << TTE_SHIFT, %g4
+	andcc	%g4, (1 << (TSB_BUCKET_SHIFT + TTE_SHIFT)) - 1, %g0
+	bnz,pt	%xcc, 2b
+	 EMPTY
 
 	/*
-	 * Not in tsb.  Call c code.
+	 * See if we just checked the largest page size, and advance to the
+	 * next one if not.
 	 */
-	b,a	%xcc, tl0_dmmu_prot_trap
+	 cmp	%g2, TS_MAX
+	bne,pt	%xcc, 1b
+	 add	%g2, 1, %g2
+	.endm
+
+	.macro	tl0_dmmu_prot
+	ba,a	%xcc, tl0_dmmu_prot_1
 	 nop
 	.align	128
 	.endm
 
-ENTRY(dmmu_prot_set_w)
-	/*
-	 * Set the hardware write bit in the tte.
-	 */
-	TTE_SET_W(%g1, %g4, %g5)
-
+ENTRY(tl0_dmmu_prot_1)
 	/*
-	 * Delete the old TLB entry and clear the sfsr.
+	 * Load the virtual page number and context from the tag access
+	 * register.  We ignore the context.
 	 */
-	sllx	%g3, TAR_VPN_SHIFT, %g3
-	stxa	%g0, [%g3] ASI_DMMU_DEMAP
-	stxa	%g0, [%g0 + AA_DMMU_SFSR] %asi
-	membar	#Sync
+	wr	%g0, ASI_DMMU, %asi
+	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g1
 
 	/*
-	 * May have become invalid in which case start over.
+	 * Try a fast inline lookup of the tsb.
 	 */
-	brgez,pn %g4, 1f
-	 or	%g4, TD_W, %g4
+	dmmu_prot_user
 
 	/*
-	 * Load the tte data into the tlb and retry the instruction.
+	 * Not in user tsb, call c code.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
-	stxa	%g4, [%g0] ASI_DTLB_DATA_IN_REG
-1:	retry
-END(dmmu_prot_set_w)
+	b,a	%xcc, tl0_dmmu_prot_trap
+	 nop
+END(tl0_dmmu_prot_1)
 
 ENTRY(tl0_dmmu_prot_trap)
 	/*
 	 * Put back the contents of the tag access register, in case we
 	 * faulted.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
 	membar	#Sync
 
 	/*
@@ -958,7 +1024,6 @@ ENTRY(tl0_dmmu_prot_trap)
 	 * Load the tar, sfar and sfsr.
 	 */
 	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g2
-
 	ldxa	[%g0 + AA_DMMU_SFAR] %asi, %g3
 	ldxa	[%g0 + AA_DMMU_SFSR] %asi, %g4
 	stxa	%g0, [%g0 + AA_DMMU_SFSR] %asi
@@ -1356,71 +1421,87 @@ END(intr_enqueue)
 	 * register.  We ignore the context.
 	 */
 	wr	%g0, ASI_IMMU, %asi
-	ldxa	[%g0 + AA_IMMU_TAR] %asi, %g6
-
-	/*
-	 * Extract the virtual page number from the contents of the tag access
-	 * register.
-	 */
-	srlx	%g6, TAR_VPN_SHIFT, %g6
+	ldxa	[%g0 + AA_IMMU_TAR] %asi, %g5
 
 	/*
-	 * Find the index into the kernel tsb.  The tsb mask gets patched at
-	 * startup.
+	 * Compute the address of the tte.  The tsb mask and address of the
+	 * tsb are patched at startup.
 	 */
-	.globl	tl1_immu_miss_load_tsb_mask
-tl1_immu_miss_load_tsb_mask:
-	sethi	%hi(TSB_KERNEL_MASK), %g4
-	or	%g4, %lo(TSB_KERNEL_MASK), %g4
-	and	%g6, %g4, %g3
+	.globl	tl1_immu_miss_patch_1
+tl1_immu_miss_patch_1:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
 
-	/*
-	 * Compute the tte address.  The address of the kernel tsb gets
-	 * patched at startup.
-	 */
-	.globl	tl1_immu_miss_load_tsb
-tl1_immu_miss_load_tsb:
-	sethi	%hi(TSB_KERNEL), %g4
-	sllx	%g3, TTE_SHIFT, %g3
-	add	%g3, %g4, %g3
+	srlx	%g5, TAR_VPN_SHIFT, %g5
+	and	%g5, %g6, %g6
+	sllx	%g6, TTE_SHIFT, %g6
+	add	%g6, %g7, %g6
 
 	/*
 	 * Load the tte.
 	 */
-	ldda	[%g3] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
+	ldda	[%g6] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
 	 * Check that its valid and executable and that the virtual page
 	 * numbers match.
 	 */
-	brgez,pn %g5, tl1_immu_miss_trap
-	 andcc	%g5, TD_EXEC, %g0
+	brgez,pn %g7, tl1_immu_miss_trap
+	 andcc	%g7, TD_EXEC, %g0
 	bz,pn	%xcc, tl1_immu_miss_trap
-	 cmp	%g4, %g6
+	 srlx	%g6, TV_SIZE_BITS, %g6
+	cmp	%g5, %g6
 	bne,pn	%xcc, tl1_immu_miss_trap
 	 EMPTY
 
 	/*
 	 * Set the reference bit if its currently clear.
 	 */
-	 andcc	%g5, TD_REF, %g0
-	bnz,a,pt %xcc, 1f
+	 andcc	%g7, TD_REF, %g0
+	bz,a,pn	%xcc, tl1_immu_miss_set_ref
 	 nop
-	TTE_SET_REF(%g3, %g5, %g4)
 
 	/*
-	 * May have become invalid, in which case start over.
+	 * Load the tte data into the TLB and retry the instruction.
+	 */
+	stxa	%g7, [%g0] ASI_ITLB_DATA_IN_REG
+	retry
+	.align	128
+	.endm
+
+ENTRY(tl1_immu_miss_set_ref)
+	/*
+	 * Recompute the tte address, which we clobbered loading the tte.  The
+	 * tsb mask and address of the tsb are patched at startup.
+	 */
+	.globl	tl1_immu_miss_patch_2
+tl1_immu_miss_patch_2:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
+
+	and	%g5, %g6, %g5
+	sllx	%g5, TTE_SHIFT, %g5
+	add	%g5, %g7, %g5
+
+	/*
+	 * Set the reference bit.
+	 */
+	TTE_SET_REF(%g5, %g6, %g7)
+
+	/*
+	 * May have become invalid during casxa, in which case start over.
 	 */
-	brgez,pn %g5, 2f
-	 or	%g5, TD_REF, %g5
+	brgez,pn %g6, 1f
+	 nop
 
 	/*
 	 * Load the tte data into the TLB and retry the instruction.
 	 */
-1:	stxa	%g5, [%g0] ASI_ITLB_DATA_IN_REG
-2:	retry
-	.align	128
-	.endm
+	stxa	%g6, [%g0] ASI_ITLB_DATA_IN_REG
+1:	retry
+END(tl1_immu_miss_set_ref)
 
 ENTRY(tl1_immu_miss_trap)
 	/*
@@ -1442,79 +1523,100 @@ END(tl1_immu_miss_trap)
 	 * register.
 	 */
 	wr	%g0, ASI_DMMU, %asi
-	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g6
+	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g5
 
 	/*
 	 * Extract the context from the contents of the tag access register.
 	 * If its non-zero this is a fault on a user address.  Note that the
 	 * faulting address is passed in %g2.
 	 */
-	sllx	%g6, 64 - TAR_VPN_SHIFT, %g5
-	brnz,a,pn %g5, tl1_dmmu_miss_user
-	 mov	%g6, %g2
+	sllx	%g5, 64 - TAR_VPN_SHIFT, %g6
+	brnz,a,pn %g6, tl1_dmmu_miss_user
+	 mov	%g5, %g1
 
 	/*
 	 * Check for the direct mapped physical region.  These addresses have
 	 * the high bit set so they are negative.
 	 */
-	brlz,pn %g6, tl1_dmmu_miss_direct
+	brlz,pn %g5, tl1_dmmu_miss_direct
 	 EMPTY
 
 	/*
-	 * Find the index into the kernel tsb.  The tsb mask gets patched at
-	 * startup.
+	 * Compute the address of the tte.  The tsb mask and address of the
+	 * tsb are patched at startup.
 	 */
-	.globl	tl1_dmmu_miss_load_tsb_mask
-tl1_dmmu_miss_load_tsb_mask:
-	sethi	%hi(TSB_KERNEL_MASK), %g4
-	or	%g4, %lo(TSB_KERNEL_MASK), %g4
-	srlx	%g6, TAR_VPN_SHIFT, %g6
-	and	%g6, %g4, %g3
+	.globl	tl1_dmmu_miss_patch_1
+tl1_dmmu_miss_patch_1:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
 
-	/*
-	 * Compute the tte address.  The address of the kernel tsb gets
-	 * patched at startup.
-	 */
-	.globl	tl1_dmmu_miss_load_tsb
-tl1_dmmu_miss_load_tsb:
-	sethi	%hi(TSB_KERNEL), %g4
-	sllx	%g3, TTE_SHIFT, %g3
-	add	%g3, %g4, %g3
+	srlx	%g5, TAR_VPN_SHIFT, %g5
+	and	%g5, %g6, %g6
+	sllx	%g6, TTE_SHIFT, %g6
+	add	%g6, %g7, %g6
 
 	/*
 	 * Load the tte.
 	 */
-	ldda	[%g3] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
+	ldda	[%g6] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
 	 * Check that its valid and that the virtual page numbers match.
 	 */
-	brgez,pn %g5, tl1_dmmu_miss_trap
-	 cmp	%g4, %g6
+	brgez,pn %g7, tl1_dmmu_miss_trap
+	 srlx	%g6, TV_SIZE_BITS, %g6
+	cmp	%g5, %g6
 	bne,pn %xcc, tl1_dmmu_miss_trap
 	 EMPTY
 
 	/*
 	 * Set the reference bit if its currently clear.
 	 */
-	 andcc	%g5, TD_REF, %g0
-	bnz,a,pt %xcc, 1f
+	 andcc	%g7, TD_REF, %g0
+	bz,a,pt	%xcc, tl1_dmmu_miss_set_ref
 	 nop
-	TTE_SET_REF(%g3, %g5, %g4)
 
 	/*
-	 * May have become invalid, in which case start over.
+	 * Load the tte data into the TLB and retry the instruction.
 	 */
-	brgez,pn %g5, 2f
-	 or	%g5, TD_REF, %g5
+	stxa	%g7, [%g0] ASI_DTLB_DATA_IN_REG
+	retry
+	.align	128
+	.endm
+
+ENTRY(tl1_dmmu_miss_set_ref)
+	/*
+	 * Recompute the tte address, which we clobbered loading the tte.  The
+	 * tsb mask and address of the tsb are patched at startup.
+	 */
+	.globl	tl1_dmmu_miss_patch_2
+tl1_dmmu_miss_patch_2:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
+
+	and	%g5, %g6, %g5
+	sllx	%g5, TTE_SHIFT, %g5
+	add	%g5, %g7, %g5
+
+	/*
+	 * Set the reference bit.
+	 */
+	TTE_SET_REF(%g5, %g6, %g7)
+
+	/*
+	 * May have become invalid during casxa, in which case start over.
+	 */
+	brgez,pn %g6, 1f
+	 nop
 
 	/*
 	 * Load the tte data into the TLB and retry the instruction.
 	 */
-1:	stxa	%g5, [%g0] ASI_DTLB_DATA_IN_REG
-2:	retry
-	.align	128
-	.endm
+	stxa	%g6, [%g0] ASI_DTLB_DATA_IN_REG
+1:	retry
+END(tl1_dmmu_miss_set_ref)
 
 ENTRY(tl1_dmmu_miss_trap)
 	/*
@@ -1533,27 +1635,17 @@ ENTRY(tl1_dmmu_miss_trap)
 END(tl1_dmmu_miss_trap)
 
 ENTRY(tl1_dmmu_miss_direct)
-#if KTR_COMPILE & KTR_TRAP
-	CATR(KTR_TRAP, "tl1_dmmu_miss_direct: pc=%#lx sp=%#lx tar=%#lx"
-	    , %g1, %g2, %g3, 7, 8, 9)
-	rdpr	%tpc, %g2
-	stx	%g2, [%g1 + KTR_PARM1]
-	add	%sp, SPOFF, %g2
-	stx	%g2, [%g1 + KTR_PARM2]
-	stx	%g6, [%g1 + KTR_PARM3]
-9:
-#endif
-
 	/*
 	 * Check the cache bits in the virtual address to see if this mapping
 	 * is virtually cacheable.  We set this up so that the masks fit in
 	 * immediates...  Note that the arithmetic shift sign extends, keeping
 	 * all the top bits set.
 	 */
-	srax	%g6, TLB_DIRECT_SHIFT, %g6
-	andcc	%g6, TLB_DIRECT_UNCACHEABLE, %g0
-	mov	TD_CP | TD_CV | TD_W, %g1
-	movnz	%xcc, TD_CP | TD_W, %g1
+	srax	%g5, TLB_DIRECT_SHIFT, %g5
+	andcc	%g5, TLB_DIRECT_UNCACHEABLE, %g0
+	mov	TD_CP | TD_CV | TD_W, %g6
+	movnz	%xcc, TD_CP | TD_W, %g6
+	or	%g5, %g6, %g5
 
 	/*
 	 * Mask off the high bits of the virtual address to get the physical
@@ -1562,14 +1654,13 @@ ENTRY(tl1_dmmu_miss_direct)
 	 * we don't have to include it in the tte bits.  We ignore the cache
 	 * bits, since they get shifted into the soft tte bits anyway.
 	 */
-	setx	TLB_DIRECT_MASK & ~TD_V, %g3, %g2
-	andn	%g6, %g2, %g3
-	or	%g3, %g1, %g3
+	setx	TLB_DIRECT_MASK & ~TD_V, %g7, %g6
+	andn	%g5, %g6, %g5
 
 	/*
 	 * Load the tte data into the TLB and retry the instruction.
 	 */
-	stxa	%g3, [%g0] ASI_DTLB_DATA_IN_REG
+	stxa	%g5, [%g0] ASI_DTLB_DATA_IN_REG
 	retry
 END(tl1_dmmu_miss_direct)
 
@@ -1583,7 +1674,7 @@ ENTRY(tl1_dmmu_miss_user)
 	 * Put back the contents of the tag access register, in case we
 	 * faulted.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
 	membar	#Sync
 
 	/*
@@ -1608,86 +1699,100 @@ ENTRY(tl1_dmmu_miss_user)
 END(tl1_dmmu_miss_user)
 
 	.macro	tl1_dmmu_prot
+	ba,a	%xcc, tl1_dmmu_prot_1
+	 nop
+	.align	128
+	.endm
+
+ENTRY(tl1_dmmu_prot_1)
 	/*
 	 * Load the context and the virtual page number from the tag access
 	 * register.
 	 */
 	wr	%g0, ASI_DMMU, %asi
-	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g6
+	ldxa	[%g0 + AA_DMMU_TAR] %asi, %g5
 
 	/*
 	 * Extract the context from the contents of the tag access register.
 	 * If its non-zero this is a fault on a user address, otherwise get
 	 * the virtual page number.
 	 */
-	sllx	%g6, 64 - TAR_VPN_SHIFT, %g5
-	brnz,a,pn %g5, tl1_dmmu_prot_user
-	 mov	%g6, %g2
+	sllx	%g5, 64 - TAR_VPN_SHIFT, %g6
+	brnz,a,pn %g6, tl1_dmmu_prot_user
+	 mov	%g5, %g1
 
 	/*
-	 * Find the index into the kernel tsb.  The tsb mask gets patched at
-	 * startup.
+	 * Compute the address of the tte.  The tsb mask and address of the
+	 * tsb are patched at startup.
 	 */
-	.globl	tl1_dmmu_prot_load_tsb_mask
-tl1_dmmu_prot_load_tsb_mask:
-	sethi	%hi(TSB_KERNEL_MASK), %g4
-	or	%g4, %lo(TSB_KERNEL_MASK), %g4
-	srlx	%g6, TAR_VPN_SHIFT, %g6
-	and	%g6, %g4, %g5
+	.globl	tl1_dmmu_prot_patch_1
+tl1_dmmu_prot_patch_1:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
 
-	/*
-	 * Compute the tte address.  The address of the kernel tsb gets
-	 * patched at startup.
-	 */
-	.globl	tl1_dmmu_prot_load_tsb
-tl1_dmmu_prot_load_tsb:
-	sethi	%hi(TSB_KERNEL), %g4
-	sllx	%g5, TTE_SHIFT, %g5
-	add	%g4, %g5, %g3
+	srlx	%g5, TAR_VPN_SHIFT, %g5
+	and	%g5, %g6, %g6
+	sllx	%g6, TTE_SHIFT, %g6
+	add	%g6, %g7, %g6
 
 	/*
 	 * Load the tte.
 	 */
-	ldda	[%g3] ASI_NUCLEUS_QUAD_LDD, %g4 /*, %g5 */
+	ldda	[%g6] ASI_NUCLEUS_QUAD_LDD, %g6 /*, %g7 */
 
 	/*
 	 * Check that its valid and writeable and that the virtual page
 	 * numbers match.
 	 */
-	brgez,pn %g5, tl1_dmmu_prot_trap
-	 andcc	%g5, TD_SW, %g0
+	brgez,pn %g7, tl1_dmmu_prot_trap
+	 andcc	%g7, TD_SW, %g0
 	bz,pn	%xcc, tl1_dmmu_prot_trap
-	 cmp	%g4, %g6
+	 srlx	%g6, TV_SIZE_BITS, %g6
+	cmp	%g5, %g6
 	bne,pn	%xcc, tl1_dmmu_prot_trap
 	 EMPTY
 
 	/*
 	 * Delete the old TLB entry and clear the sfsr.
 	 */
-	 sllx	%g6, TAR_VPN_SHIFT, %g6
+	 sllx	%g5, TAR_VPN_SHIFT, %g6
 	or	%g6, TLB_DEMAP_NUCLEUS, %g6
 	stxa	%g0, [%g6] ASI_DMMU_DEMAP
 	stxa	%g0, [%g0 + AA_DMMU_SFSR] %asi
 	membar	#Sync
 
-	ba,a	%xcc, tl1_dmmu_prot_cont
-	 nop
-	.align	128
-	.endm
+	/*
+	 * Recompute the tte address, which we clobbered loading the tte.  The
+	 * tsb mask and address of the tsb are patched at startup.
+	 */
+	.globl	tl1_dmmu_prot_patch_2
+tl1_dmmu_prot_patch_2:
+	sethi	%hi(TSB_KERNEL_MASK), %g6
+	or	%g6, %lo(TSB_KERNEL_MASK), %g6
+	sethi	%hi(TSB_KERNEL), %g7
+
+	and	%g5, %g6, %g5
+	sllx	%g5, TTE_SHIFT, %g5
+	add	%g5, %g7, %g5
 
-ENTRY(tl1_dmmu_prot_cont)
 	/*
 	 * Set the hardware write bit.
 	 */
-	TTE_SET_W(%g3, %g5, %g6)
+	TTE_SET_W(%g5, %g6, %g7)
+
+	/*
+	 * May have become invalid during casxa, in which case start over.
+	 */
+	brgez,pn %g6, 1f
+	 or	%g6, TD_W, %g6
 
 	/*
 	 * Load the tte data into the TLB and retry the instruction.
 	 */
-	or	%g5, TD_W, %g5
-	stxa	%g5, [%g0] ASI_DTLB_DATA_IN_REG
-	retry
-END(tl1_dmmu_prot_cont)
+	stxa	%g6, [%g0] ASI_DTLB_DATA_IN_REG
+1:	retry
+END(tl1_dmmu_prot_1)
 
 ENTRY(tl1_dmmu_prot_user)
 	/*
@@ -1699,7 +1804,7 @@ ENTRY(tl1_dmmu_prot_user)
 	 * Put back the contents of the tag access register, in case we
 	 * faulted.
 	 */
-	stxa	%g2, [%g0 + AA_DMMU_TAR] %asi
+	stxa	%g1, [%g0 + AA_DMMU_TAR] %asi
 	membar	#Sync
 
 	/*
diff --git a/sys/sparc64/sparc64/genassym.c b/sys/sparc64/sparc64/genassym.c
index 9d0dfa2bc71c..50e201b0b20c 100644
--- a/sys/sparc64/sparc64/genassym.c
+++ b/sys/sparc64/sparc64/genassym.c
@@ -77,7 +77,6 @@ ASSYM(KSTACK_PAGES, KSTACK_PAGES);
 ASSYM(KSTACK_GUARD_PAGES, KSTACK_GUARD_PAGES);
 ASSYM(PCPU_PAGES, PCPU_PAGES);
 ASSYM(UAREA_PAGES, UAREA_PAGES);
-ASSYM(PAGE_SIZE, PAGE_SIZE);
 
 ASSYM(PIL_TICK, PIL_TICK);
 
@@ -98,14 +97,16 @@ ASSYM(TLB_DIRECT_MASK, TLB_DIRECT_MASK);
 ASSYM(TLB_DIRECT_UNCACHEABLE, TLB_DIRECT_UNCACHEABLE);
 ASSYM(TLB_DIRECT_SHIFT, TLB_DIRECT_SHIFT);
 
-ASSYM(TSB_BUCKET_ADDRESS_BITS, TSB_BUCKET_ADDRESS_BITS);
+ASSYM(TSB_BUCKET_MASK, TSB_BUCKET_MASK);
 ASSYM(TSB_BUCKET_SHIFT, TSB_BUCKET_SHIFT);
 
 ASSYM(INT_SHIFT, INT_SHIFT);
 ASSYM(PTR_SHIFT, PTR_SHIFT);
 
 ASSYM(PAGE_SHIFT, PAGE_SHIFT);
-ASSYM(PAGE_MASK, PAGE_MASK);
+ASSYM(PAGE_SHIFT_8K, PAGE_SHIFT_8K);
+ASSYM(PAGE_SHIFT_4M, PAGE_SHIFT_4M);
+ASSYM(PAGE_SIZE, PAGE_SIZE);
 
 ASSYM(CPU_CLKSYNC, CPU_CLKSYNC);
 ASSYM(CPU_INIT, CPU_INIT);
@@ -174,6 +175,11 @@ ASSYM(TD_CV, TD_CV);
 ASSYM(TD_L, TD_L);
 ASSYM(TD_W, TD_W);
 
+ASSYM(TS_MIN, TS_MIN);
+ASSYM(TS_MAX, TS_MAX);
+
+ASSYM(TV_SIZE_BITS, TV_SIZE_BITS);
+
 ASSYM(V_INTR, offsetof(struct vmmeter, v_intr));
 
 ASSYM(PC_CURTHREAD, offsetof(struct pcpu, pc_curthread));
diff --git a/sys/sparc64/sparc64/locore.S b/sys/sparc64/sparc64/locore.S
index b1571cbd6cd3..4a4b5c941647 100644
--- a/sys/sparc64/sparc64/locore.S
+++ b/sys/sparc64/sparc64/locore.S
@@ -131,17 +131,6 @@ ENTRY(cpu_setregs)
 	mov	%o0, PCPU_REG
 
 	/*
-	 * MMU globals.
-	 */
-	wrpr	%g0, PSTATE_MMU, %pstate
-
-	/*
-	 * MMU %g7 points to the user tsb.  Initialize it to something sane
-	 * here to catch invalid use.
-	 */
-	mov	%g0, TSB_REG
-
-	/*
 	 * Normal globals again.
 	 */
 	wrpr	%g0, PSTATE_NORMAL, %pstate
diff --git a/sys/sparc64/sparc64/mp_locore.S b/sys/sparc64/sparc64/mp_locore.S
index cf97464ac363..998b25ef541f 100644
--- a/sys/sparc64/sparc64/mp_locore.S
+++ b/sys/sparc64/sparc64/mp_locore.S
@@ -48,7 +48,8 @@
 	 nop
 	ldx	[%l2 + TTE_VPN], %l4
 	ldx	[%l2 + TTE_DATA], %l5
-	sllx	%l4, PAGE_SHIFT, %l4
+	srlx	%l4, TV_SIZE_BITS, %l4
+	sllx	%l4, PAGE_SHIFT_4M, %l4
 	wr	%g0, ASI_DMMU, %asi
 	stxa	%l4, [%g0 + AA_DMMU_TAR] %asi
 	stxa	%l5, [%g0] ASI_DTLB_DATA_IN_REG
@@ -145,7 +146,8 @@ ENTRY(mp_startup)
 	ldx	[%l3 + TTE_DATA], %l5
 
 	wr	%g0, ASI_DMMU, %asi
-	sllx	%l4, PAGE_SHIFT, %l4
+	srlx	%l4, TV_SIZE_BITS, %l4
+	sllx	%l4, PAGE_SHIFT_8K, %l4
 	stxa	%l4, [%g0 + AA_DMMU_TAR] %asi
 	stxa	%l5, [%g0] ASI_DTLB_DATA_IN_REG
 	membar	#Sync
diff --git a/sys/sparc64/sparc64/pmap.c b/sys/sparc64/sparc64/pmap.c
index 2e497fb19c77..789130bee9b7 100644
--- a/sys/sparc64/sparc64/pmap.c
+++ b/sys/sparc64/sparc64/pmap.c
@@ -161,12 +161,12 @@ static vm_offset_t pmap_bootstrap_alloc(vm_size_t size);
 
 static vm_offset_t pmap_map_direct(vm_page_t m);
 
-extern int tl1_immu_miss_load_tsb[];
-extern int tl1_immu_miss_load_tsb_mask[];
-extern int tl1_dmmu_miss_load_tsb[];
-extern int tl1_dmmu_miss_load_tsb_mask[];
-extern int tl1_dmmu_prot_load_tsb[];
-extern int tl1_dmmu_prot_load_tsb_mask[];
+extern int tl1_immu_miss_patch_1[];
+extern int tl1_immu_miss_patch_2[];
+extern int tl1_dmmu_miss_patch_1[];
+extern int tl1_dmmu_miss_patch_2[];
+extern int tl1_dmmu_prot_patch_1[];
+extern int tl1_dmmu_prot_patch_2[];
 
 /*
  * If user pmap is processed with pmap_remove and with pmap_remove and the
@@ -337,34 +337,34 @@ pmap_bootstrap(vm_offset_t ekva)
 	/*
 	 * Patch the virtual address and the tsb mask into the trap table.
 	 */
-#define	SETHI_G4(x) \
-	EIF_OP(IOP_FORM2) | EIF_F2_RD(4) | EIF_F2_OP2(INS0_SETHI) | \
-	    EIF_IMM((x) >> 10, 22)
-#define	OR_G4_I_G4(x) \
-	EIF_OP(IOP_MISC) | EIF_F3_RD(4) | EIF_F3_OP3(INS2_OR) | \
-	    EIF_F3_RS1(4) | EIF_F3_I(1) | EIF_IMM(x, 10)
-
-	tl1_immu_miss_load_tsb[0] = SETHI_G4((vm_offset_t)tsb_kernel);
-	tl1_immu_miss_load_tsb_mask[0] = SETHI_G4(tsb_kernel_mask);
-	tl1_immu_miss_load_tsb_mask[1] = OR_G4_I_G4(tsb_kernel_mask);
-	flush(tl1_immu_miss_load_tsb);
-	flush(tl1_immu_miss_load_tsb_mask);
-	flush(tl1_immu_miss_load_tsb_mask + 1);
-
-	tl1_dmmu_miss_load_tsb[0] = SETHI_G4((vm_offset_t)tsb_kernel);
-	tl1_dmmu_miss_load_tsb_mask[0] = SETHI_G4(tsb_kernel_mask);
-	tl1_dmmu_miss_load_tsb_mask[1] = OR_G4_I_G4(tsb_kernel_mask);
-	flush(tl1_dmmu_miss_load_tsb);
-	flush(tl1_dmmu_miss_load_tsb_mask);
-	flush(tl1_dmmu_miss_load_tsb_mask + 1);
-
-	tl1_dmmu_prot_load_tsb[0] = SETHI_G4((vm_offset_t)tsb_kernel);
-	tl1_dmmu_prot_load_tsb_mask[0] = SETHI_G4(tsb_kernel_mask);
-	tl1_dmmu_prot_load_tsb_mask[1] = OR_G4_I_G4(tsb_kernel_mask);
-	flush(tl1_dmmu_prot_load_tsb);
-	flush(tl1_dmmu_prot_load_tsb_mask);
-	flush(tl1_dmmu_prot_load_tsb_mask + 1);
 
+#define	SETHI(rd, imm22) \
+	(EIF_OP(IOP_FORM2) | EIF_F2_RD(rd) | EIF_F2_OP2(INS0_SETHI) | \
+	    EIF_IMM((imm22) >> 10, 22))
+#define	OR_R_I_R(rd, imm13, rs1) \
+	(EIF_OP(IOP_MISC) | EIF_F3_RD(rd) | EIF_F3_OP3(INS2_OR) | \
+	    EIF_F3_RS1(rs1) | EIF_F3_I(1) | EIF_IMM(imm13, 13))
+
+#define	PATCH(addr) do { \
+	if (addr[0] != SETHI(IF_F2_RD(addr[0]), 0x0) || \
+	    addr[1] != OR_R_I_R(IF_F3_RD(addr[1]), 0x0, IF_F3_RS1(addr[1])) || \
+	    addr[2] != SETHI(IF_F2_RD(addr[2]), 0x0)) \
+		panic("pmap_boostrap: patched instructions have changed"); \
+	addr[0] |= EIF_IMM((tsb_kernel_mask) >> 10, 22); \
+	addr[1] |= EIF_IMM(tsb_kernel_mask, 10); \
+	addr[2] |= EIF_IMM(((vm_offset_t)tsb_kernel) >> 10, 22); \
+	flush(addr); \
+	flush(addr + 1); \
+	flush(addr + 2); \
+} while (0)
+
+	PATCH(tl1_immu_miss_patch_1);
+	PATCH(tl1_immu_miss_patch_2);
+	PATCH(tl1_dmmu_miss_patch_1);
+	PATCH(tl1_dmmu_miss_patch_2);
+	PATCH(tl1_dmmu_prot_patch_1);
+	PATCH(tl1_dmmu_prot_patch_2);
+	
 	/*
 	 * Lock it in the tlb.
 	 */
@@ -380,7 +380,7 @@ pmap_bootstrap(vm_offset_t ekva)
 		va = kernel_tlbs[i].te_va;
 		for (off = 0; off < PAGE_SIZE_4M; off += PAGE_SIZE) {
 			tp = tsb_kvtotte(va + off);
-			tp->tte_vpn = TV_VPN(va + off);
+			tp->tte_vpn = TV_VPN(va + off, TS_8K);
 			tp->tte_data = TD_V | TD_8K | TD_PA(pa + off) |
 			    TD_REF | TD_SW | TD_CP | TD_CV | TD_P | TD_W;
 		}
@@ -398,7 +398,7 @@ pmap_bootstrap(vm_offset_t ekva)
 		pa = kstack0_phys + i * PAGE_SIZE;
 		va = kstack0 + i * PAGE_SIZE;
 		tp = tsb_kvtotte(va);
-		tp->tte_vpn = TV_VPN(va);
+		tp->tte_vpn = TV_VPN(va, TS_8K);
 		tp->tte_data = TD_V | TD_8K | TD_PA(pa) | TD_REF | TD_SW |
 		    TD_CP | TD_CV | TD_P | TD_W;
 	}
@@ -436,7 +436,7 @@ pmap_bootstrap(vm_offset_t ekva)
 		    off += PAGE_SIZE) {
 			va = translations[i].om_start + off;
 			tp = tsb_kvtotte(va);
-			tp->tte_vpn = TV_VPN(va);
+			tp->tte_vpn = TV_VPN(va, TS_8K);
 			tp->tte_data = translations[i].om_tte + off;
 		}
 	}
@@ -783,7 +783,7 @@ pmap_kenter(vm_offset_t va, vm_offset_t pa)
 	data = TD_V | TD_8K | TD_PA(pa) | TD_REF | TD_SW | TD_CP | TD_P | TD_W;
 	if (pmap_cache_enter(m, va) != 0)
 		data |= TD_CV;
-	tp->tte_vpn = TV_VPN(va);
+	tp->tte_vpn = TV_VPN(va, TS_8K);
 	tp->tte_data = data;
 	STAILQ_INSERT_TAIL(&m->md.tte_list, tp, tte_link);
 	tp->tte_pmap = kernel_pmap;
@@ -804,7 +804,7 @@ pmap_kenter_flags(vm_offset_t va, vm_offset_t pa, u_long flags)
 	tp = tsb_kvtotte(va);
 	CTR4(KTR_PMAP, "pmap_kenter_flags: va=%#lx pa=%#lx tp=%p data=%#lx",
 	    va, pa, tp, tp->tte_data);
-	tp->tte_vpn = TV_VPN(va);
+	tp->tte_vpn = TV_VPN(va, TS_8K);
 	tp->tte_data = TD_V | TD_8K | TD_PA(pa) | TD_REF | TD_P | flags;
 }
 
@@ -873,7 +873,7 @@ pmap_map(vm_offset_t *virt, vm_offset_t pa_start, vm_offset_t pa_end, int prot)
 	va = sva;
 	for (; pa < pa_end; pa += PAGE_SIZE, va += PAGE_SIZE) {
 		tp = tsb_kvtotte(va);
-		tp->tte_vpn = TV_VPN(va);
+		tp->tte_vpn = TV_VPN(va, TS_8K);
 		tp->tte_data = TD_V | TD_8K | TD_PA(pa) | TD_REF | TD_SW |
 		    TD_CP | TD_CV | TD_P | TD_W;
 	}
@@ -1467,7 +1467,7 @@ pmap_enter(pmap_t pm, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 				data |= TD_W;
 		}
 
-		tsb_tte_enter(pm, m, va, data);
+		tsb_tte_enter(pm, m, va, TS_8K, data);
 	}
 }
 
@@ -1516,7 +1516,7 @@ pmap_copy_tte(pmap_t src_pmap, pmap_t dst_pmap, struct tte *tp, vm_offset_t va)
 		data = tp->tte_data &
 		    ~(TD_PV | TD_REF | TD_SW | TD_CV | TD_W);
 		m = PHYS_TO_VM_PAGE(TTE_GET_PA(tp));
-		tsb_tte_enter(dst_pmap, m, va, data);
+		tsb_tte_enter(dst_pmap, m, va, TS_8K, data);
 	}
 	return (1);
 }
@@ -1799,10 +1799,6 @@ pmap_activate(struct thread *td)
 	u_long context;
 	pmap_t pm;
 
-	/*
-	 * Load all the data we need up front to encourage the compiler to
-	 * not issue any loads while we have interrupts disable below.
-	 */
 	vm = td->td_proc->p_vmspace;
 	pm = &vm->vm_pmap;
 	tsb = (vm_offset_t)pm->pm_tsb;
@@ -1812,9 +1808,9 @@ pmap_activate(struct thread *td)
 	    ("pmap_activate: activating nucleus context?"));
 
 	mtx_lock_spin(&sched_lock);
-	wrpr(pstate, 0, PSTATE_MMU);
-	mov(tsb, TSB_REG);
-	wrpr(pstate, 0, PSTATE_KERNEL);
+	stxa(AA_DMMU_TSB, ASI_DMMU, tsb);
+	stxa(AA_IMMU_TSB, ASI_IMMU, tsb);
+	membar(Sync);
 	context = pmap_context_alloc();
 	pm->pm_context[PCPU_GET(cpuid)] = context;
 	pm->pm_active |= PCPU_GET(cpumask);
diff --git a/sys/sparc64/sparc64/swtch.S b/sys/sparc64/sparc64/swtch.S
index 429e961d2567..b94f5acf5458 100644
--- a/sys/sparc64/sparc64/swtch.S
+++ b/sys/sparc64/sparc64/swtch.S
@@ -243,13 +243,14 @@ ENTRY(cpu_switch)
 	stx	%o2, [PCPU(VMSPACE)]
 
 	/*
-	 * Load the address of the tsb, switch to mmu globals, and install
-	 * the preloaded tsb pointer.
+	 * Load the tsb registers.
 	 */
 	ldx	[%o2 + VM_PMAP + PM_TSB], %o3
-	wrpr	%g0, PSTATE_MMU, %pstate
-	mov	%o3, TSB_REG
-	wrpr	%g0, PSTATE_KERNEL, %pstate
+	mov	AA_DMMU_TSB, %o4
+	stxa	%o3, [%o4] ASI_DMMU
+	mov	AA_IMMU_TSB, %o4
+	stxa	%o3, [%o4] ASI_IMMU
+	membar	#Sync
 
 4:
 #if KTR_COMPILE & KTR_PROC
diff --git a/sys/sparc64/sparc64/tsb.c b/sys/sparc64/sparc64/tsb.c
index 5083073ad58f..68ba11d44a1b 100644
--- a/sys/sparc64/sparc64/tsb.c
+++ b/sys/sparc64/sparc64/tsb.c
@@ -63,6 +63,7 @@
 #include <machine/tte.h>
 
 CTASSERT((1 << TTE_SHIFT) == sizeof(struct tte));
+CTASSERT(TSB_BUCKET_MASK < (1 << 12));
 
 #ifdef PMAP_STATS
 static long tsb_nrepl;
@@ -101,6 +102,7 @@ tsb_tte_lookup(pmap_t pm, vm_offset_t va)
 {
 	struct tte *bucket;
 	struct tte *tp;
+	u_long sz;
 	u_int i;
 
 	if (pm == kernel_pmap) {
@@ -110,19 +112,20 @@ tsb_tte_lookup(pmap_t pm, vm_offset_t va)
 			return (tp);
 	} else {
 		TSB_STATS_INC(tsb_nlookup_u);
-		va = trunc_page(va);
-		bucket = tsb_vtobucket(pm, va);
-		for (i = 0; i < TSB_BUCKET_SIZE; i++) {
-			tp = &bucket[i];
-			if (tte_match(tp, va))
-				return (tp);
+		for (sz = TS_MIN; sz <= TS_MAX; sz++) {
+			bucket = tsb_vtobucket(pm, sz, va);
+			for (i = 0; i < TSB_BUCKET_SIZE; i++) {
+				tp = &bucket[i];
+				if (tte_match(tp, va))
+					return (tp);
+			}
 		}
 	}
 	return (NULL);
 }
 
 struct tte *
-tsb_tte_enter(pmap_t pm, vm_page_t m, vm_offset_t va, u_long data)
+tsb_tte_enter(pmap_t pm, vm_page_t m, vm_offset_t va, u_long sz, u_long data)
 {
 	struct tte *bucket;
 	struct tte *rtp;
@@ -140,7 +143,7 @@ tsb_tte_enter(pmap_t pm, vm_page_t m, vm_offset_t va, u_long data)
 	}
 
 	TSB_STATS_INC(tsb_nenter_u);
-	bucket = tsb_vtobucket(pm, va);
+	bucket = tsb_vtobucket(pm, sz, va);
 
 	tp = NULL;
 	rtp = NULL;
@@ -176,7 +179,7 @@ enter:
 	if (pmap_cache_enter(m, va) != 0)
 		data |= TD_CV;
 
-	tp->tte_vpn = TV_VPN(va);
+	tp->tte_vpn = TV_VPN(va, sz);
 	tp->tte_data = data;
 	STAILQ_INSERT_TAIL(&m->md.tte_list, tp, tte_link);
 	tp->tte_pmap = pm;