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

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 4657009d81f1..ed15c25f2e79 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");