[PATCH v3 4/9] clk: clocking-wizard: move clock registration to separate function

[Harry Austen <hpausten@protonmail.com>]

Provide clear separation of dynamic reconfiguration logic, by moving its
setup procedure to its own dedicated function.

Signed-off-by: Harry Austen <hpausten@protonmail.com>
---
 drivers/clk/xilinx/clk-xlnx-clock-wizard.c | 143 +++++++++++----------
 1 file changed, 75 insertions(+), 68 deletions(-)

diff --git a/drivers/clk/xilinx/clk-xlnx-clock-wizard.c b/drivers/clk/xilinx/clk-xlnx-clock-wizard.c
index f332e0eee56c8..1a65a7d153c35 100644
--- a/drivers/clk/xilinx/clk-xlnx-clock-wizard.c
+++ b/drivers/clk/xilinx/clk-xlnx-clock-wizard.c
@@ -962,72 +962,30 @@ static const struct versal_clk_data versal_data = {
 	.is_versal	= true,
 };
 
-static int clk_wzrd_probe(struct platform_device *pdev)
+static int clk_wzrd_register_output_clocks(struct device *dev, int nr_outputs)
 {
 	const char *clkout_name, *clk_name, *clk_mul_name;
+	struct clk_wzrd *clk_wzrd = dev_get_drvdata(dev);
 	u32 regl, regh, edge, regld, reghd, edged, div;
-	struct device_node *np = pdev->dev.of_node;
 	const struct versal_clk_data *data;
-	struct clk_wzrd *clk_wzrd;
 	unsigned long flags = 0;
+	bool is_versal = false;
 	void __iomem *ctrl_reg;
 	u32 reg, reg_f, mult;
-	bool is_versal = false;
-	unsigned long rate;
-	int nr_outputs;
-	int i, ret;
-
-	ret = of_property_read_u32(np, "xlnx,nr-outputs", &nr_outputs);
-	if (ret || nr_outputs > WZRD_NUM_OUTPUTS)
-		return -EINVAL;
-
-	clk_wzrd = devm_kzalloc(&pdev->dev, struct_size(clk_wzrd, clk_data.hws, nr_outputs),
-				GFP_KERNEL);
-	if (!clk_wzrd)
-		return -ENOMEM;
-	platform_set_drvdata(pdev, clk_wzrd);
-
-	clk_wzrd->base = devm_platform_ioremap_resource(pdev, 0);
-	if (IS_ERR(clk_wzrd->base))
-		return PTR_ERR(clk_wzrd->base);
-
-	ret = of_property_read_u32(np, "xlnx,speed-grade", &clk_wzrd->speed_grade);
-	if (!ret) {
-		if (clk_wzrd->speed_grade < 1 || clk_wzrd->speed_grade > 3) {
-			dev_warn(&pdev->dev, "invalid speed grade '%d'\n",
-				 clk_wzrd->speed_grade);
-			clk_wzrd->speed_grade = 0;
-		}
-	}
-
-	clk_wzrd->clk_in1 = devm_clk_get(&pdev->dev, "clk_in1");
-	if (IS_ERR(clk_wzrd->clk_in1))
-		return dev_err_probe(&pdev->dev, PTR_ERR(clk_wzrd->clk_in1),
-				     "clk_in1 not found\n");
+	int i;
 
-	clk_wzrd->axi_clk = devm_clk_get_enabled(&pdev->dev, "s_axi_aclk");
-	if (IS_ERR(clk_wzrd->axi_clk))
-		return dev_err_probe(&pdev->dev, PTR_ERR(clk_wzrd->axi_clk),
-				     "s_axi_aclk not found\n");
-	rate = clk_get_rate(clk_wzrd->axi_clk);
-	if (rate > WZRD_ACLK_MAX_FREQ) {
-		dev_err(&pdev->dev, "s_axi_aclk frequency (%lu) too high\n",
-			rate);
-		return -EINVAL;
-	}
-
-	data = device_get_match_data(&pdev->dev);
+	data = device_get_match_data(dev);
 	if (data)
 		is_versal = data->is_versal;
 
-	clkout_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_out0", dev_name(&pdev->dev));
+	clkout_name = devm_kasprintf(dev, GFP_KERNEL, "%s_out0", dev_name(dev));
 	if (!clkout_name)
 		return -ENOMEM;
 
 	if (is_versal) {
 		if (nr_outputs == 1) {
 			clk_wzrd->clk_data.hws[0] = clk_wzrd_ver_register_divider
-				(&pdev->dev, clkout_name,
+				(dev, clkout_name,
 				__clk_get_name(clk_wzrd->clk_in1), 0,
 				clk_wzrd->base, WZRD_CLK_CFG_REG(is_versal, 3),
 				WZRD_CLKOUT_DIVIDE_SHIFT,
@@ -1035,7 +993,7 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 				CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO,
 				DIV_ALL, &clkwzrd_lock);
 
-			goto out;
+			return 0;
 		}
 		/* register multiplier */
 		edge = !!(readl(clk_wzrd->base + WZRD_CLK_CFG_REG(is_versal, 0)) &
@@ -1060,7 +1018,7 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 	} else {
 		if (nr_outputs == 1) {
 			clk_wzrd->clk_data.hws[0] = clk_wzrd_register_divider
-				(&pdev->dev, clkout_name,
+				(dev, clkout_name,
 				__clk_get_name(clk_wzrd->clk_in1), 0,
 				clk_wzrd->base, WZRD_CLK_CFG_REG(is_versal, 3),
 				WZRD_CLKOUT_DIVIDE_SHIFT,
@@ -1068,7 +1026,7 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 				CLK_DIVIDER_ONE_BASED | CLK_DIVIDER_ALLOW_ZERO,
 				DIV_ALL, &clkwzrd_lock);
 
-			goto out;
+			return 0;
 		}
 		reg = readl(clk_wzrd->base + WZRD_CLK_CFG_REG(is_versal, 0));
 		reg_f = reg & WZRD_CLKFBOUT_FRAC_MASK;
@@ -1079,19 +1037,19 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 		mult = (reg * 1000) + reg_f;
 		div = 1000;
 	}
-	clk_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_mul", dev_name(&pdev->dev));
+	clk_name = devm_kasprintf(dev, GFP_KERNEL, "%s_mul", dev_name(dev));
 	if (!clk_name)
 		return -ENOMEM;
 	clk_wzrd->clks_internal[wzrd_clk_mul] = devm_clk_hw_register_fixed_factor
-			(&pdev->dev, clk_name,
+			(dev, clk_name,
 			 __clk_get_name(clk_wzrd->clk_in1),
 			0, mult, div);
 	if (IS_ERR(clk_wzrd->clks_internal[wzrd_clk_mul])) {
-		dev_err(&pdev->dev, "unable to register fixed-factor clock\n");
+		dev_err(dev, "unable to register fixed-factor clock\n");
 		return PTR_ERR(clk_wzrd->clks_internal[wzrd_clk_mul]);
 	}
 
-	clk_name = devm_kasprintf(&pdev->dev, GFP_KERNEL, "%s_mul_div", dev_name(&pdev->dev));
+	clk_name = devm_kasprintf(dev, GFP_KERNEL, "%s_mul_div", dev_name(dev));
 	if (!clk_name)
 		return -ENOMEM;
 
@@ -1108,31 +1066,29 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 
 		clk_mul_name = clk_hw_get_name(clk_wzrd->clks_internal[wzrd_clk_mul]);
 		clk_wzrd->clks_internal[wzrd_clk_mul_div] =
-			devm_clk_hw_register_fixed_factor(&pdev->dev, clk_name,
-							  clk_mul_name, 0, 1, div);
+			devm_clk_hw_register_fixed_factor(dev, clk_name, clk_mul_name, 0, 1, div);
 	} else {
 		ctrl_reg = clk_wzrd->base + WZRD_CLK_CFG_REG(is_versal, 0);
 		clk_wzrd->clks_internal[wzrd_clk_mul_div] = devm_clk_hw_register_divider
-			(&pdev->dev, clk_name,
+			(dev, clk_name,
 			 clk_hw_get_name(clk_wzrd->clks_internal[wzrd_clk_mul]),
 			flags, ctrl_reg, 0, 8, CLK_DIVIDER_ONE_BASED |
 			CLK_DIVIDER_ALLOW_ZERO, &clkwzrd_lock);
 	}
 	if (IS_ERR(clk_wzrd->clks_internal[wzrd_clk_mul_div])) {
-		dev_err(&pdev->dev, "unable to register divider clock\n");
+		dev_err(dev, "unable to register divider clock\n");
 		return PTR_ERR(clk_wzrd->clks_internal[wzrd_clk_mul_div]);
 	}
 
 	/* register div per output */
 	for (i = nr_outputs - 1; i >= 0 ; i--) {
-		clkout_name = devm_kasprintf(&pdev->dev, GFP_KERNEL,
-					     "%s_out%d", dev_name(&pdev->dev), i);
+		clkout_name = devm_kasprintf(dev, GFP_KERNEL, "%s_out%d", dev_name(dev), i);
 		if (!clkout_name)
 			return -ENOMEM;
 
 		if (is_versal) {
 			clk_wzrd->clk_data.hws[i] = clk_wzrd_ver_register_divider
-						(&pdev->dev,
+						(dev,
 						 clkout_name, clk_name, 0,
 						 clk_wzrd->base,
 						 (WZRD_CLK_CFG_REG(is_versal, 3) + i * 8),
@@ -1144,7 +1100,7 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 		} else {
 			if (!i)
 				clk_wzrd->clk_data.hws[i] = clk_wzrd_register_divf
-					(&pdev->dev, clkout_name, clk_name, flags, clk_wzrd->base,
+					(dev, clkout_name, clk_name, flags, clk_wzrd->base,
 					(WZRD_CLK_CFG_REG(is_versal, 2) + i * 12),
 					WZRD_CLKOUT_DIVIDE_SHIFT,
 					WZRD_CLKOUT_DIVIDE_WIDTH,
@@ -1152,7 +1108,7 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 					DIV_O, &clkwzrd_lock);
 			else
 				clk_wzrd->clk_data.hws[i] = clk_wzrd_register_divider
-					(&pdev->dev, clkout_name, clk_name, 0, clk_wzrd->base,
+					(dev, clkout_name, clk_name, 0, clk_wzrd->base,
 					(WZRD_CLK_CFG_REG(is_versal, 2) + i * 12),
 					WZRD_CLKOUT_DIVIDE_SHIFT,
 					WZRD_CLKOUT_DIVIDE_WIDTH,
@@ -1160,13 +1116,64 @@ static int clk_wzrd_probe(struct platform_device *pdev)
 					DIV_O, &clkwzrd_lock);
 		}
 		if (IS_ERR(clk_wzrd->clk_data.hws[i])) {
-			dev_err(&pdev->dev,
-				"unable to register divider clock\n");
+			dev_err(dev, "unable to register divider clock\n");
 			return PTR_ERR(clk_wzrd->clk_data.hws[i]);
 		}
 	}
 
-out:
+	return 0;
+}
+
+static int clk_wzrd_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct clk_wzrd *clk_wzrd;
+	unsigned long rate;
+	int nr_outputs;
+	int ret;
+
+	ret = of_property_read_u32(np, "xlnx,nr-outputs", &nr_outputs);
+	if (ret || nr_outputs > WZRD_NUM_OUTPUTS)
+		return -EINVAL;
+
+	clk_wzrd = devm_kzalloc(&pdev->dev, struct_size(clk_wzrd, clk_data.hws, nr_outputs),
+				GFP_KERNEL);
+	if (!clk_wzrd)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, clk_wzrd);
+
+	clk_wzrd->base = devm_platform_ioremap_resource(pdev, 0);
+	if (IS_ERR(clk_wzrd->base))
+		return PTR_ERR(clk_wzrd->base);
+
+	ret = of_property_read_u32(np, "xlnx,speed-grade", &clk_wzrd->speed_grade);
+	if (!ret) {
+		if (clk_wzrd->speed_grade < 1 || clk_wzrd->speed_grade > 3) {
+			dev_warn(&pdev->dev, "invalid speed grade '%d'\n",
+				 clk_wzrd->speed_grade);
+			clk_wzrd->speed_grade = 0;
+		}
+	}
+
+	clk_wzrd->clk_in1 = devm_clk_get(&pdev->dev, "clk_in1");
+	if (IS_ERR(clk_wzrd->clk_in1))
+		return dev_err_probe(&pdev->dev, PTR_ERR(clk_wzrd->clk_in1),
+				     "clk_in1 not found\n");
+
+	clk_wzrd->axi_clk = devm_clk_get_enabled(&pdev->dev, "s_axi_aclk");
+	if (IS_ERR(clk_wzrd->axi_clk))
+		return dev_err_probe(&pdev->dev, PTR_ERR(clk_wzrd->axi_clk),
+				     "s_axi_aclk not found\n");
+	rate = clk_get_rate(clk_wzrd->axi_clk);
+	if (rate > WZRD_ACLK_MAX_FREQ) {
+		dev_err(&pdev->dev, "s_axi_aclk frequency (%lu) too high\n", rate);
+		return -EINVAL;
+	}
+
+	ret = clk_wzrd_register_output_clocks(&pdev->dev, nr_outputs);
+	if (ret)
+		return ret;
+
 	clk_wzrd->clk_data.num = nr_outputs;
 	ret = devm_of_clk_add_hw_provider(&pdev->dev, of_clk_hw_onecell_get, &clk_wzrd->clk_data);
 	if (ret) {
-- 
2.46.0