From 31abcd3f6829eb3630af2044c6595428ee02b1eb Mon Sep 17 00:00:00 2001
From: Janghyuck Kim <janghyuck.kim@samsung.com>
Date: Thu, 10 May 2018 17:31:37 +0900
Subject: [PATCH] [COMMON] iommu/exynos: add checking TLB entries

TLB checking is added when SysMMU fault occurred.
It compares TLB value to page table entry and displays message if it
doesn't refered different entries.

Change-Id: I6599f7bb04f548093dc60e528466fd7f6a81cb30
Signed-off-by: Janghyuck Kim <janghyuck.kim@samsung.com>
---
 drivers/iommu/exynos-iommu-reg.h | 51 ++++++++++++++++++++++++++++----
 drivers/iommu/exynos-iommu.h     |  2 ++
 2 files changed, 47 insertions(+), 6 deletions(-)

diff --git a/drivers/iommu/exynos-iommu-reg.h b/drivers/iommu/exynos-iommu-reg.h
index f1cc558f9d2b..24fbac3d1ce8 100644
--- a/drivers/iommu/exynos-iommu-reg.h
+++ b/drivers/iommu/exynos-iommu-reg.h
@@ -66,15 +66,53 @@ static inline unsigned int dump_tlb_entry_way_type(void __iomem *sfrbase,
 	return 0;
 }
 
+static inline void sysmmu_tlb_compare(phys_addr_t pgtable,
+				int idx_sub, u32 vpn, u32 ppn)
+{
+	sysmmu_pte_t *entry;
+	unsigned long vaddr = MMU_VADDR_FROM_TLB((unsigned long)vpn, idx_sub);
+	unsigned long paddr = MMU_PADDR_FROM_TLB((unsigned long)ppn);
+	unsigned long phys = 0;
+
+	if (!pgtable)
+		return;
+
+	entry = section_entry(phys_to_virt(pgtable), vaddr);
+
+	if (lv1ent_section(entry)) {
+		phys = section_phys(entry);
+	} else if (lv1ent_page(entry)) {
+		entry = page_entry(entry, vaddr);
+
+		if (lv2ent_large(entry))
+			phys = lpage_phys(entry);
+		else if (lv2ent_small(entry))
+			phys = spage_phys(entry);
+	} else {
+		pr_crit(">> Invalid address detected! entry: %#lx",
+						(unsigned long)*entry);
+		return;
+	}
+
+	if (paddr != phys) {
+		pr_crit(">> TLB mismatch detected!\n");
+		pr_crit("   TLB: %#010lx, PT entry: %#010lx\n", paddr, phys);
+	}
+}
+
 static inline unsigned int dump_tlb_entry_port_type(void __iomem *sfrbase,
-						int idx_way, int idx_set)
+		phys_addr_t pgtable, int idx_way, int idx_set, int idx_sub)
 {
 	if (MMU_TLB_ENTRY_VALID(__raw_readl(sfrbase + REG_CAPA1_TLB_ATTR))) {
+		u32 vpn, ppn, attr;
+
+		vpn = __raw_readl(sfrbase + REG_CAPA1_TLB_VPN);
+		ppn = __raw_readl(sfrbase + REG_CAPA1_TLB_PPN);
+		attr = __raw_readl(sfrbase + REG_CAPA1_TLB_ATTR);
+
 		pr_crit("[%02d][%02d] VPN: %#010x, PPN: %#010x, ATTR: %#010x\n",
-			idx_way, idx_set,
-			__raw_readl(sfrbase + REG_CAPA1_TLB_VPN),
-			__raw_readl(sfrbase + REG_CAPA1_TLB_PPN),
-			__raw_readl(sfrbase + REG_CAPA1_TLB_ATTR));
+			idx_way, idx_set, vpn, ppn, attr);
+		sysmmu_tlb_compare(pgtable, idx_sub, vpn, ppn);
 		return 1;
 	}
 	return 0;
@@ -183,7 +221,8 @@ static inline void dump_sysmmu_tlb_port(struct sysmmu_drvdata *drvdata,
 				for (k = 0; k < MMU_NUM_TLB_SUBLINE; k++) {
 					__raw_writel(MMU_CAPA1_SET_TLB_READ_ENTRY(t, j, i, k),
 							sfrbase + REG_CAPA1_TLB_READ);
-					cnt += dump_tlb_entry_port_type(sfrbase, i, j);
+					cnt += dump_tlb_entry_port_type(
+						sfrbase, pgtable, i, j, k);
 				}
 			}
 		}
diff --git a/drivers/iommu/exynos-iommu.h b/drivers/iommu/exynos-iommu.h
index 03d71aba4b61..fb0efbdc39f8 100644
--- a/drivers/iommu/exynos-iommu.h
+++ b/drivers/iommu/exynos-iommu.h
@@ -200,6 +200,8 @@ typedef u32 sysmmu_pte_t;
 #define MMU_TLB_ENTRY_VALID(reg)	((reg) >> 28)
 #define MMU_SBB_ENTRY_VALID(reg)	((reg) >> 28)
 
+#define MMU_VADDR_FROM_TLB(reg, idx)	((((reg) & 0xFFFFC) | ((idx) & 0x3)) << 12)
+#define MMU_PADDR_FROM_TLB(reg)		(((reg) & 0xFFFFFF) << 12)
 #define MMU_VADDR_FROM_SBB(reg)		(((reg) & 0xFFFFF) << 12)
 #define MMU_PADDR_FROM_SBB(reg)		(((reg) & 0x3FFFFFF) << 10)
 
-- 
2.20.1