alternate iommu fix

Signed-off-by: Peter Bailey <peter.bailey@raspberrypi.com>

Author: PineappleBeech

drivers/iommu/bcm2712-iommu.c

@@ -145,7 +145,7 @@ static void bcm2712_iommu_free_page(struct bcm2712_iommu *mmu, void *ptr)
 	}
 }
 
-static int bcm2712_iommu_init(struct bcm2712_iommu *mmu)
+static int bcm2712_iommu_hw_init(struct bcm2712_iommu *mmu)
 {
 	u32 u = MMU_RD(MMMU_DEBUG_INFO_OFFSET);
 
@@ -203,30 +203,11 @@ static int bcm2712_iommu_init(struct bcm2712_iommu *mmu)
 		MMU_WR(MMMU_BYPASS_END_OFFSET, 0);
 	}
 
-	/*
-	 * Configure the addresses of the top-level table (offset because
-	 * the aperture does not start from zero), and of the default page.
-	 * For simplicity, both these regions are whole Linux pages.
-	 */
-	if (mmu->top_table) {
-		u = (u32)(virt_to_phys(mmu->top_table) >> IOMMU_PAGE_SHIFT);
-	} else {
-		u = bcm2712_iommu_get_page(mmu, &mmu->top_table);
-		if (!u)
-			return -ENOMEM;
-	}
 	MMU_WR(MMMU_PT_PA_BASE_OFFSET,
-	       u - ((mmu->aperture_base - mmu->dma_iova_offset) >> L1_AP_BASE_SHIFT));
-	if (mmu->default_page) {
-		u = (u32)(virt_to_phys(mmu->default_page) >> IOMMU_PAGE_SHIFT);
-	} else {
-		u = bcm2712_iommu_get_page(mmu, &mmu->default_page);
-		if (!u) {
-			bcm2712_iommu_free_page(mmu, mmu->top_table);
-			return -ENOMEM;
-		}
-	}
-	MMU_WR(MMMU_ILLEGAL_ADR_OFFSET, MMMU_ILLEGAL_ADR_ENABLE + u);
+	       (u32)(virt_to_phys(mmu->top_table) >> IOMMU_PAGE_SHIFT) -
+	       ((mmu->aperture_base - mmu->dma_iova_offset) >> L1_AP_BASE_SHIFT));
+	MMU_WR(MMMU_ILLEGAL_ADR_OFFSET, MMMU_ILLEGAL_ADR_ENABLE +
+	       (u32)(virt_to_phys(mmu->default_page) >> IOMMU_PAGE_SHIFT));
 
 	/* Flush (and enable) the shared TLB cache; enable this MMU. */
 	if (mmu->cache)
@@ -242,6 +223,25 @@ static int bcm2712_iommu_init(struct bcm2712_iommu *mmu)
 	return 0;
 }
 
+static int bcm2712_iommu_init(struct bcm2712_iommu *mmu)
+{
+	/*
+	 * Configure the addresses of the top-level table (offset because
+	 * the aperture does not start from zero), and of the default page.
+	 * For simplicity, both these regions are whole Linux pages.
+	 */
+
+	if (!bcm2712_iommu_get_page(mmu, &mmu->top_table))
+		return -ENOMEM;
+
+	if (!bcm2712_iommu_get_page(mmu, &mmu->default_page)) {
+		bcm2712_iommu_free_page(mmu, mmu->top_table);
+		return -ENOMEM;
+	}
+
+	return bcm2712_iommu_hw_init(mmu);
+}
+
 static int bcm2712_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
 {
 	struct bcm2712_iommu *mmu = dev ? dev_iommu_priv_get(dev) : 0;
@@ -766,7 +766,7 @@ static int bcm2712_iommu_resume(struct device *dev)
 {
 	struct bcm2712_iommu *mmu = dev_get_drvdata(dev);
 
-	return bcm2712_iommu_init(mmu);
+	return bcm2712_iommu_hw_init(mmu);
 }
 
 static DEFINE_SIMPLE_DEV_PM_OPS(bcm2712_iommu_pm_ops, bcm2712_iommu_suspend,