3 files changed, 35 insertions, 19 deletions

diff --git a/sys/powerpc/booke/pmap.c b/sys/powerpc/booke/pmap.c
index 08516b151e6b..a76ef6a089fd 100644
--- a/sys/powerpc/booke/pmap.c
+++ b/sys/powerpc/booke/pmap.c
@@ -177,6 +177,7 @@ static struct mtx copy_page_mutex;
 #endif
 
 static struct mtx tlbivax_mutex;
+static bool mmuv2;
 
 /**************************************************************************/
 /* PMAP */
@@ -640,6 +641,9 @@ mmu_booke_bootstrap(vm_offset_t start, vm_offset_t kernelend)
 
 	debugf("mmu_booke_bootstrap: entered\n");
 
+	if ((mfspr(SPR_MMUCFG) & MMUCFG_MAVN_M) > 0)
+		mmuv2 = true;
+
 	/* Set interesting system properties */
 #ifdef __powerpc64__
 	hw_direct_map = 1;
@@ -2703,7 +2707,7 @@ tsize2size(unsigned int tsize)
 	 * size = 4^tsize * 2^10 = 2^(2 * tsize - 10)
 	 */
 
-	return ((1 << (2 * tsize)) * 1024);
+	return ((1UL << tsize) * 1024);
 }
 
 /*
@@ -2713,7 +2717,7 @@ static unsigned int
 size2tsize(vm_size_t size)
 {
 
-	return (ilog2(size) / 2 - 5);
+	return (ilog2(size) - 10);
 }
 
 /*
@@ -2772,23 +2776,29 @@ tlb1_mapin_region(vm_offset_t va, vm_paddr_t pa, vm_size_t size, int wimge)
 {
 	vm_offset_t base;
 	vm_size_t mapped, sz, ssize;
+	int shift;
 
 	mapped = 0;
 	base = va;
 	ssize = size;
 
+	if (mmuv2)
+		shift = 1;
+	else
+		shift = 2;
+
 	while (size > 0) {
-		sz = 1UL << (ilog2(size) & ~1);
+		sz = 1UL << (ilog2(size) & ~(shift - 1));
 		/* Align size to PA */
 		if (pa % sz != 0) {
 			do {
-				sz >>= 2;
+				sz >>= shift;
 			} while (pa % sz != 0);
 		}
 		/* Now align from there to VA */
 		if (va % sz != 0) {
 			do {
-				sz >>= 2;
+				sz >>= shift;
 			} while (va % sz != 0);
 		}
 #ifdef __powerpc64__
@@ -2805,7 +2815,8 @@ tlb1_mapin_region(vm_offset_t va, vm_paddr_t pa, vm_size_t size, int wimge)
 		 * For now, though, since we have plenty of space in TLB1,
 		 * always avoid creating entries larger than 4GB.
 		 */
-		sz = MIN(sz, 1UL << 32);
+		if (!mmuv2)
+			sz = MIN(sz, 1UL << 32);
 #endif
 		if (bootverbose)
 			printf("Wiring VA=%p to PA=%jx (size=%lx)\n",
diff --git a/sys/powerpc/include/spr.h b/sys/powerpc/include/spr.h
index 605b1be194d9..5c6e9d67fcb4 100644
--- a/sys/powerpc/include/spr.h
+++ b/sys/powerpc/include/spr.h
@@ -864,5 +864,10 @@
 #define	  BUCSR_BPEN		0x00000001	/* Branch Prediction Enable */
 #define	  BUCSR_BBFI		0x00000200	/* Branch Buffer Flash Invalidate */
 
+#define	SPR_MMUCFG		0x3f7	/* ..8 MMU Configuration Register */
+#define	  MMUCFG_PIDSIZE_M	  0x000007c0
+#define	  MMUCFG_PIDSIZE_S	  6
+#define	  MMUCFG_MAVN_M		  0x00000003
+
 #endif /* BOOKE */
 #endif /* !_POWERPC_SPR_H_ */
diff --git a/sys/powerpc/include/tlb.h b/sys/powerpc/include/tlb.h
index 0a4463e0b928..33d31efab604 100644
--- a/sys/powerpc/include/tlb.h
+++ b/sys/powerpc/include/tlb.h
@@ -50,20 +50,20 @@
 #define MAS1_TID_SHIFT		16
 #define MAS1_TS_MASK		0x00001000
 #define MAS1_TS_SHIFT		12
-#define MAS1_TSIZE_MASK		0x00000F00
-#define MAS1_TSIZE_SHIFT	8
+#define MAS1_TSIZE_MASK		0x00000F80
+#define MAS1_TSIZE_SHIFT	7
 
-#define	TLB_SIZE_4K		1
-#define	TLB_SIZE_16K		2
-#define	TLB_SIZE_64K		3
-#define	TLB_SIZE_256K		4
-#define	TLB_SIZE_1M		5
-#define	TLB_SIZE_4M		6
-#define	TLB_SIZE_16M		7
-#define	TLB_SIZE_64M		8
-#define	TLB_SIZE_256M		9
-#define	TLB_SIZE_1G		10
-#define	TLB_SIZE_4G		11
+#define	TLB_SIZE_4K		2
+#define	TLB_SIZE_16K		4
+#define	TLB_SIZE_64K		6
+#define	TLB_SIZE_256K		8
+#define	TLB_SIZE_1M		10
+#define	TLB_SIZE_4M		12
+#define	TLB_SIZE_16M		14
+#define	TLB_SIZE_64M		16
+#define	TLB_SIZE_256M		18
+#define	TLB_SIZE_1G		20
+#define	TLB_SIZE_4G		22
 
 #ifdef __powerpc64__
 #define	MAS2_EPN_MASK		0xFFFFFFFFFFFFF000UL