Index: kvm_aarch64.c
===================================================================
RCS file: /cvsroot/src/lib/libkvm/kvm_aarch64.c,v
retrieving revision 1.5
diff -u -p -r1.5 kvm_aarch64.c
--- kvm_aarch64.c	9 Nov 2018 04:06:40 -0000	1.5
+++ kvm_aarch64.c	17 Dec 2018 19:46:50 -0000
@@ -64,6 +64,8 @@ _kvm_initvtop(kvm_t *kd)
 	return (0);
 }
 
+#define LOG(fmt, ...) printf("%s:%d: " fmt, __func__, __LINE__, ##__VA_ARGS__)
+
 int
 _kvm_kvatop(kvm_t *kd, vaddr_t va, paddr_t *pa)
 {
@@ -83,56 +85,65 @@ lose:
 	}
 
 	const cpu_kcore_hdr_t * const cpu_kh = kd->cpu_data;
-	const uint64_t tg1 = cpu_kh->kh_tcr1 & TCR_TG1; 
+	const uint64_t tg1 = cpu_kh->kh_tcr1 & TCR_TG1;
 	const u_int t1siz = __SHIFTOUT(cpu_kh->kh_tcr1, TCR_T1SZ);
+	const u_int inputsz = 64 - t1siz;
 
 	/*
 	 * Real kernel virtual address: do the translation.
 	 */
 
-	u_int va_bits;
+	// AArch64.FullTranslate (page 5920)
+
+	// Appendix K6.1 page 6180
+//	u_int va_bits;
 	u_int page_shift;
+//	u_int firstbl;
 
 	switch (tg1) {
 	case TCR_TG1_4KB:
-		va_bits = t1siz + 36;
+//		firstbl = 1;
 		page_shift = 12;
 		break;
 	case TCR_TG1_16KB:
-		va_bits = 48;
+//		firstbl = 2;
 		page_shift = 14;
 		break;
 	case TCR_TG1_64KB:
-		va_bits = t1siz + 38;
 		page_shift = 16;
+//		firstbl = 2;
 		break;
 	default:
 		goto lose;
 	}
 
 	const size_t page_size = 1 << page_shift;
-	const uint64_t page_mask = (page_size - 1);
-	const uint64_t page_addr = __BITS(47, 0) & ~page_mask;
+	const uint64_t page_mask = __BITS(page_shift - 1, 0);
+	const uint64_t page_addr = __BITS(47, page_shift);
 	const uint64_t pte_mask = page_mask >> 3;
 	const u_int pte_shift = page_shift - 3;
 
-	/* how many level of page tables do we have? */
-	u_int level = (48 + page_shift - 1) / page_shift;
+	/* how many levels of page tables do we have? */
+	u_int levels = howmany(inputsz - page_shift, pte_shift);
+
+	/*
+	 * Input address width of (n+1) bits or 64 - TnSZ
+	 * n = 63 - TnSz
 
+	 */
 	/* restrict va to the valid VA bits */
-	va &= (1LL << va_bits) - 1;
+	va &= __BITS(inputsz - 1, 0);
 
-	u_int addr_shift = page_shift + (level - 1) * pte_shift;
+	u_int addr_shift = page_shift + (levels - 1) * pte_shift;
 
 	/* clear out the unused low bits of the table address */
 	paddr_t pte_addr = (cpu_kh->kh_ttbr1 & TTBR_BADDR);
-	pte_addr &= ~((8L << (va_bits - addr_shift)) - 1);
 
 	for (;;) {
 		pt_entry_t pte;
 
 		/* now index into the pte table */
-		pte_addr += 8 * ((va >> addr_shift) & pte_mask);
+		pte_addr += 8 * __SHIFTOUT(va, __BITS(addr_shift + pte_shift - 1, addr_shift));
 
 		/* Find and read the PTE. */
 		if (_kvm_pread(kd, kd->pmfd, &pte, sizeof(pte),
@@ -148,15 +159,14 @@ lose:
 		}
 
 		if ((pte & LX_TYPE) == LX_TYPE_BLK) {
-			const paddr_t blk_mask = ((1L << addr_shift) - 1);
+			const paddr_t blk_mask = __BITS(addr_shift - 1, 0);
 
 			*pa = (pte & page_addr & ~blk_mask) | (va & blk_mask);
-			return 0;
+			return __BIT(addr_shift) - (va & blk_mask);
 		}
-
-		if (level == page_shift) {
+		if (--levels == 0) {
 			*pa = (pte & page_addr) | (va & page_mask);
-			return 0;
+			return page_size - (va & page_mask); 
 		}
 
 		/*