iommu: msm: simplify address discovery in secure register dump code

Currently in the secure register dump code, we try to determine the
register in question by doing some reverse arithmetic on the full
address that TZ returns and matching that against known register
offsets. However, support was recently added for storing the base
physical address of the Iommu in `struct msm_iommu_drvdata'. Simplify
the code by calculating the offset of the registers being returned by TZ
by subtracting their values from the base address of the Iommu.

Change-Id: Icd6e4a35a48b808b0523ab4971cb9fb5b00125f5
Signed-off-by: Mitchel Humpherys <mitchelh@codeaurora.org>

2 files changed, 20 insertions, 16 deletions

diff --git a/drivers/iommu/msm_iommu-v1.c b/drivers/iommu/msm_iommu-v1.c
index 447952cc7871..54354a6d9fa5 100644
--- a/drivers/iommu/msm_iommu-v1.c
+++ b/drivers/iommu/msm_iommu-v1.c
@@ -1192,7 +1192,8 @@ static void __print_ctx_regs(void __iomem *base, int ctx, unsigned int fsr)
 	unsigned int i;
 
 	for (i = DUMP_REG_FIRST; i < MAX_DUMP_REGS; ++i) {
-		regs[i].val = GET_CTX_REG(dump_regs_tbl[i].key, base, ctx);
+		regs[i].val = GET_CTX_REG(dump_regs_tbl[i].reg_offset,
+					base, ctx);
 		regs[i].valid = 1;
 	}
 	print_ctx_regs(regs);
@@ -1342,11 +1343,11 @@ static phys_addr_t msm_iommu_get_pt_base_addr(struct iommu_domain *domain)
 	return __pa(priv->pt.fl_table);
 }
 
-#define DUMP_REG_INIT(dump_reg, cb_reg, mbp)			\
-	do {							\
-		dump_regs_tbl[dump_reg].key = cb_reg;		\
-		dump_regs_tbl[dump_reg].name = #cb_reg;		\
-		dump_regs_tbl[dump_reg].must_be_present = mbp;	\
+#define DUMP_REG_INIT(dump_reg, cb_reg, mbp)				\
+	do {								\
+		dump_regs_tbl[dump_reg].reg_offset = cb_reg;	\
+		dump_regs_tbl[dump_reg].name = #cb_reg;			\
+		dump_regs_tbl[dump_reg].must_be_present = mbp;		\
 	} while (0)
 
 static void msm_iommu_build_dump_regs_table(void)
diff --git a/drivers/iommu/msm_iommu_sec.c b/drivers/iommu/msm_iommu_sec.c
index 9a78c187aa66..accd63548558 100644
--- a/drivers/iommu/msm_iommu_sec.c
+++ b/drivers/iommu/msm_iommu_sec.c
@@ -143,11 +143,9 @@ static int msm_iommu_dump_fault_regs(int smmu_id, int cb_num,
 	return ret;
 }
 
-#define EXTRACT_DUMP_REG_KEY(addr, ctx) (addr & ((1 << CTX_SHIFT) - 1))
-
 static int msm_iommu_reg_dump_to_regs(
 	struct msm_iommu_context_reg ctx_regs[],
-	struct msm_scm_fault_regs_dump *dump, int cb_num)
+	struct msm_scm_fault_regs_dump *dump, struct msm_iommu_drvdata *drvdata)
 {
 	int i, j, ret = 0;
 	const uint32_t nvals = (dump->dump_size / sizeof(uint32_t));
@@ -155,21 +153,27 @@ static int msm_iommu_reg_dump_to_regs(
 	const uint32_t * const end = ((uint32_t *) dump) + nvals;
 
 	for (i = 1; it < end; it += 2, i += 2) {
+		unsigned long reg_offset;
 		uint32_t addr	= *it;
 		uint32_t val	= *(it + 1);
 		struct msm_iommu_context_reg *reg = NULL;
+		if (drvdata->phys_base > addr) {
+			pr_err("Bogus-looking register (0x%x) for Iommu with base at %pa. Skipping.\n",
+				addr, &drvdata->phys_base);
+			continue;
+		}
+		reg_offset = addr - drvdata->phys_base;
 
 		for (j = 0; j < MAX_DUMP_REGS; ++j) {
-			if (dump_regs_tbl[j].key ==
-				EXTRACT_DUMP_REG_KEY(addr, cb_num)) {
+			if (dump_regs_tbl[j].reg_offset == reg_offset) {
 				reg = &ctx_regs[j];
 				break;
 			}
 		}
 
 		if (reg == NULL) {
-			pr_debug("Unknown register in secure CB dump: %x (%x)\n",
-				addr, EXTRACT_DUMP_REG_KEY(addr, cb_num));
+			pr_debug("Unknown register in secure CB dump: %x\n",
+				addr);
 			continue;
 		}
 
@@ -194,7 +198,7 @@ static int msm_iommu_reg_dump_to_regs(
 			if (dump_regs_tbl[i].must_be_present) {
 				pr_err("Register missing from dump: %s, %lx\n",
 					dump_regs_tbl[i].name,
-					dump_regs_tbl[i].key);
+					dump_regs_tbl[i].reg_offset);
 				ret = 1;
 			}
 			ctx_regs[i].val = 0;
@@ -253,8 +257,7 @@ irqreturn_t msm_iommu_secure_fault_handler_v2(int irq, void *dev_id)
 	} else {
 		struct msm_iommu_context_reg ctx_regs[MAX_DUMP_REGS];
 		memset(ctx_regs, 0, sizeof(ctx_regs));
-		tmp = msm_iommu_reg_dump_to_regs(ctx_regs, regs,
-						ctx_drvdata->num);
+		tmp = msm_iommu_reg_dump_to_regs(ctx_regs, regs, drvdata);
 		if (!tmp && ctx_regs[DUMP_REG_FSR].val) {
 			if (!ctx_drvdata->attached_domain) {
 				pr_err("Bad domain in interrupt handler\n");