[PATCH v2 06/21] ARM: Layerscape: icid: move re-usable code to separate functions

[Sascha Hauer <s.hauer@pengutronix.de>]

Create separate functions from code that can be re-used by upcoming
LS1028a support.

Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>
---
 arch/arm/mach-layerscape/icid.c | 98 +++++++++++++++++++--------------
 1 file changed, 57 insertions(+), 41 deletions(-)

diff --git a/arch/arm/mach-layerscape/icid.c b/arch/arm/mach-layerscape/icid.c
index f0a9dc8063..dd0e160bde 100644
--- a/arch/arm/mach-layerscape/icid.c
+++ b/arch/arm/mach-layerscape/icid.c
@@ -68,6 +68,58 @@ struct icid_id_table {
 	phys_addr_t reg_addr;
 };
 
+static void of_set_iommu_prop(struct device_node *np, phandle iommu_handle,
+			       int stream_id)
+{
+	u32 prop[2];
+
+	prop[0] = cpu_to_fdt32(iommu_handle);
+	prop[1] = cpu_to_fdt32(stream_id);
+
+	of_set_property(np, "iommus", prop, sizeof(prop), 1);
+}
+
+static phandle of_get_iommu_handle(struct device_node *root)
+{
+	struct device_node *iommu;
+
+	iommu = of_find_compatible_node(root, NULL, "arm,mmu-500");
+	if (!iommu) {
+		pr_info("No \"arm,mmu-500\" node found, won't fixup\n");
+		return 0;
+	}
+
+	return of_node_create_phandle(iommu);
+}
+
+static int of_fixup_icid(struct device_node *root, phandle iommu_handle,
+			 const struct icid_id_table *icid_table, int num_icid)
+{
+	int i;
+
+	for (i = 0; i < num_icid; i++) {
+		const struct icid_id_table *icid = &icid_table[i];
+		struct device_node *np;
+
+		if (!icid->compat)
+			continue;
+
+		for_each_compatible_node_from(np, root, NULL, icid->compat) {
+			struct resource res;
+
+			if (of_address_to_resource(np, 0, &res))
+				continue;
+
+			if (res.start == icid->compat_addr) {
+				of_set_iommu_prop(np, iommu_handle, icid->id);
+				break;
+			}
+		}
+	}
+
+	return 0;
+}
+
 struct fman_icid_id_table {
 	u32 port_id;
 	u32 icid;
@@ -292,17 +344,6 @@ static int get_fman_port_icid(int port_id, const struct fman_icid_id_table *tbl,
 	return -ENODEV;
 }
 
-static void of_set_iommu_prop(struct device_node *np, phandle iommu_handle,
-			       int stream_id)
-{
-	u32 prop[2];
-
-	prop[0] = cpu_to_fdt32(iommu_handle);
-	prop[1] = cpu_to_fdt32(stream_id);
-
-	of_set_property(np, "iommus", prop, sizeof(prop), 1);
-}
-
 static void of_fixup_fman_port_icid_by_compat(struct device_node *root,
 					       phandle iommu_handle,
 					       const char *compat)
@@ -491,40 +532,15 @@ static void of_fixup_qportals(struct device_node *root, phandle iommu_handle)
 	}
 }
 
-static int icid_of_fixup(struct device_node *root, void *context)
+static int of_fixup_ls1046a(struct device_node *root, void *context)
 {
-	int i;
-	struct device_node *iommu;
 	phandle iommu_handle;
 
-	iommu = of_find_compatible_node(root, NULL, "arm,mmu-500");
-	if (!iommu) {
-		pr_info("No \"arm,mmu-500\" node found, won't fixup\n");
+	iommu_handle = of_get_iommu_handle(root);
+	if (!iommu_handle)
 		return 0;
-	}
-
-	iommu_handle = of_node_create_phandle(iommu);
-
-	for (i = 0; i < ARRAY_SIZE(icid_tbl_ls1046a); i++) {
-		const struct icid_id_table *icid = &icid_tbl_ls1046a[i];
-		struct device_node *np;
-
-		if (!icid->compat)
-			continue;
-
-		for_each_compatible_node_from(np, root, NULL, icid->compat) {
-			struct resource res;
-
-			if (of_address_to_resource(np, 0, &res))
-				continue;
-
-			if (res.start == icid->compat_addr) {
-				of_set_iommu_prop(np, iommu_handle, icid->id);
-				break;
-			}
-		}
-	}
 
+	of_fixup_icid(root, iommu_handle, icid_tbl_ls1046a, ARRAY_SIZE(icid_tbl_ls1046a));
 	of_fixup_fman_icids(root, iommu_handle);
 	of_fixup_qportals(root, iommu_handle);
 
@@ -553,5 +569,5 @@ void ls1046a_setup_icids(void)
 
 	setup_qbman_portals();
 
-	of_register_fixup(icid_of_fixup, NULL);
+	of_register_fixup(of_fixup_ls1046a, NULL);
 }
-- 
2.39.2