[PATCH v5 1/4] armv8: mmu: add a function to help debug TLB lookups

[Casey Connolly]

Implement a super basic software TLB walk which can look up a single
address in the TLB and print each stage of the translation. This is
helpful for debugging TLB issues and will be compiled out if unused.

Example output on QEMU aarch64:

Performing software TLB lookup of address 0x50100000 va_bits: 40
  PTE: 0x47fe0000. addr[47:39]: 0x000 (offset 0x00000)
  L0: 0x47fe0000 -> TABLE (0x47fe1000)
    PTE: 0x47fe1000. addr[38:30]: 0x001 (offset 0x00008)
    L1: 0x47fe1008 -> BLOCK (0x40000000)
      [0x40000000 - 0x80000000]

Signed-off-by: Casey Connolly <casey.connolly at linaro.org>
---
 arch/arm/cpu/armv8/cache_v8.c    | 60 ++++++++++++++++++++++++++++++++++++++++
 arch/arm/include/asm/armv8/mmu.h |  7 +++++
 2 files changed, 67 insertions(+)

diff --git a/arch/arm/cpu/armv8/cache_v8.c b/arch/arm/cpu/armv8/cache_v8.c
index 39479df7b21f..97a1c39ed268 100644
--- a/arch/arm/cpu/armv8/cache_v8.c
+++ b/arch/arm/cpu/armv8/cache_v8.c
@@ -733,8 +733,68 @@ void dump_pagetable(u64 ttbr, u64 tcr)
 	       va_bits, va_bits < 39 ? 3 : 4);
 	walk_pagetable(ttbr, tcr, pagetable_print_entry, NULL);
 }
 
+/* Do a software pagetable walk for the given address */
+void tlb_debug_lookup(u64 addr)
+{
+	u64 va_bits;
+	u64 ttbr = gd->arch.tlb_addr, *pte;
+	int lshift, level;
+
+	get_tcr(NULL, &va_bits);
+	level = va_bits < 39 ? 1 : 0;
+
+	printf("Performing software TLB lookup of address %#010llx va_bits: %lld\n",
+	       addr, va_bits);
+
+	addr = ALIGN_DOWN(addr, 0x1000);
+	pte = ((u64 *)ttbr);
+	for (int i = level; i < 4; i++) {
+		int indent = (i - level + 1) * 2;
+		u32 idx;
+		u64 _addr;
+
+		lshift = level2shift(i);
+		idx = (addr >> lshift) & 0x1FF;
+
+		printf("%*sPTE: %#010llx. addr[%d:%d]: %#05x (offset %#07x)\n", indent, "", (u64)pte,
+		       lshift + 8, lshift, idx, idx * 8);
+		printf("%*sL%d: %#010llx -> ", indent, "", i, (u64)(&pte[idx]));
+
+		pte = &pte[idx];
+		_addr = *pte & GENMASK_ULL(va_bits, PAGE_SHIFT);
+
+		/*
+		 * Check the PTE and either descend if it's a table or print
+		 * the mapping and return.
+		 */
+		switch (pte_type(pte)) {
+		case PTE_TYPE_FAULT:
+			printf("UNMAPPED!\n");
+			return;
+		case PTE_TYPE_BLOCK:
+			printf("BLOCK (%#010llx)\n", _addr);
+			break;
+		case PTE_TYPE_TABLE:
+			if (i < 3) {
+				printf("TABLE (%#010llx)\n", _addr);
+				pte = (u64 *)_addr;
+				continue;
+			} else { /* PTE_TYPE_PAGE */
+				printf("PAGE (%#010llx)\n", _addr);
+			}
+			break;
+		default:
+			printf("Unknown (%#010llx)\n", _addr);
+			break;
+		}
+
+		printf("%*s[%#010llx - %#010llx]\n", indent + 2, "", _addr, _addr + (1 << lshift));
+		return;
+	}
+}
+
 /* Returns the estimated required size of all page tables */
 __weak u64 get_page_table_size(void)
 {
 	u64 one_pt = MAX_PTE_ENTRIES * sizeof(u64);
diff --git a/arch/arm/include/asm/armv8/mmu.h b/arch/arm/include/asm/armv8/mmu.h
index 8aa5f9721c4a..ee81619a1843 100644
--- a/arch/arm/include/asm/armv8/mmu.h
+++ b/arch/arm/include/asm/armv8/mmu.h
@@ -186,8 +186,15 @@ void walk_pagetable(u64 ttbr, u64 tcr, pte_walker_cb_t cb, void *priv);
  * @tcr: TCR value to use
  */
 void dump_pagetable(u64 ttbr, u64 tcr);
 
+/**
+ * tlb_debug_lookup() - Perform a software TLB walk printing each stage
+ *
+ * @addr: the address to look-up in the TLB.
+ */
+void tlb_debug_lookup(u64 addr);
+
 struct mm_region {
 	u64 virt;
 	u64 phys;
 	u64 size;

-- 
2.53.0