[PATCH 10/12] soc: xilinx: vcu: make the PLL configurable

[Michael Tretter <m.tretter@pengutronix.de>]

Do not configure the PLL when probing the driver, but register the clock
in the clock framework and do the configuration based on the respective
callbacks.

This is necessary to allow the consumers, i.e., encoder and decoder
drivers, of the xlnx_vcu clock provider to set the clock rate and
actually enable the clocks without relying on some pre-configuration.

Signed-off-by: Michael Tretter <m.tretter@pengutronix.de>
---
 drivers/soc/xilinx/xlnx_vcu.c | 137 ++++++++++++++++++++++++++--------
 1 file changed, 106 insertions(+), 31 deletions(-)

diff --git a/drivers/soc/xilinx/xlnx_vcu.c b/drivers/soc/xilinx/xlnx_vcu.c
index cf8456b4ef78..84d7c46cd42f 100644
--- a/drivers/soc/xilinx/xlnx_vcu.c
+++ b/drivers/soc/xilinx/xlnx_vcu.c
@@ -257,9 +257,18 @@ static void xvcu_write_field_reg(void __iomem *iomem, int offset,
 	xvcu_write(iomem, offset, val);
 }
 
-static int xvcu_pll_wait_for_lock(struct xvcu_device *xvcu)
+#define to_vcu_pll(_hw) container_of(_hw, struct vcu_pll, hw)
+
+struct vcu_pll {
+	struct clk_hw hw;
+	void __iomem *reg_base;
+	unsigned long fvco_min;
+	unsigned long fvco_max;
+};
+
+static int xvcu_pll_wait_for_lock(struct vcu_pll *pll)
 {
-	void __iomem *base = xvcu->vcu_slcr_ba;
+	void __iomem *base = pll->reg_base;
 	unsigned long timeout;
 	u32 lock_status;
 
@@ -307,9 +316,9 @@ static const struct xvcu_pll_cfg *xvcu_find_cfg(int div)
 	return cfg;
 }
 
-static int xvcu_pll_set_div(struct xvcu_device *xvcu, int div)
+static int xvcu_pll_set_div(struct vcu_pll *pll, int div)
 {
-	void __iomem *base = xvcu->vcu_slcr_ba;
+	void __iomem *base = pll->reg_base;
 	const struct xvcu_pll_cfg *cfg = NULL;
 	u32 vcu_pll_ctrl;
 	u32 cfg_val;
@@ -334,24 +343,49 @@ static int xvcu_pll_set_div(struct xvcu_device *xvcu, int div)
 	return 0;
 }
 
-static int xvcu_pll_set_rate(struct xvcu_device *xvcu,
+static long xvcu_pll_round_rate(struct clk_hw *hw,
+				unsigned long rate, unsigned long *parent_rate)
+{
+	struct vcu_pll *pll = to_vcu_pll(hw);
+	unsigned int feedback_div;
+
+	rate = clamp_t(unsigned long, rate, pll->fvco_min, pll->fvco_max);
+
+	feedback_div = DIV_ROUND_CLOSEST_ULL(rate, *parent_rate);
+	feedback_div = clamp_t(unsigned int, feedback_div, 25, 125);
+
+	return *parent_rate * feedback_div;
+}
+
+static unsigned long xvcu_pll_recalc_rate(struct clk_hw *hw,
+					  unsigned long parent_rate)
+{
+	struct vcu_pll *pll = to_vcu_pll(hw);
+	void __iomem *base = pll->reg_base;
+	unsigned int div;
+	u32 vcu_pll_ctrl;
+
+	vcu_pll_ctrl = xvcu_read(base, VCU_PLL_CTRL);
+	div = (vcu_pll_ctrl >> VCU_PLL_CTRL_FBDIV_SHIFT) & VCU_PLL_CTRL_FBDIV_MASK;
+
+	return div * parent_rate;
+}
+
+static int xvcu_pll_set_rate(struct clk_hw *hw,
 			     unsigned long rate, unsigned long parent_rate)
 {
-	return xvcu_pll_set_div(xvcu, rate / parent_rate);
+	struct vcu_pll *pll = to_vcu_pll(hw);
+
+	return xvcu_pll_set_div(pll, rate / parent_rate);
 }
 
-static int xvcu_pll_enable(struct xvcu_device *xvcu)
+static int xvcu_pll_enable(struct clk_hw *hw)
 {
-	void __iomem *base = xvcu->vcu_slcr_ba;
+	struct vcu_pll *pll = to_vcu_pll(hw);
+	void __iomem *base = pll->reg_base;
 	u32 vcu_pll_ctrl;
 	int ret;
 
-	ret = clk_prepare_enable(xvcu->pll_ref);
-	if (ret) {
-		dev_err(xvcu->dev, "failed to enable pll_ref clock source\n");
-		return ret;
-	}
-
 	xvcu_write_field_reg(base, VCU_PLL_CTRL,
 			     1, VCU_PLL_CTRL_BYPASS_MASK,
 			     VCU_PLL_CTRL_BYPASS_SHIFT);
@@ -371,9 +405,9 @@ static int xvcu_pll_enable(struct xvcu_device *xvcu)
 	vcu_pll_ctrl |= (0 & VCU_PLL_CTRL_RESET_MASK) << VCU_PLL_CTRL_RESET_SHIFT;
 	xvcu_write(base, VCU_PLL_CTRL, vcu_pll_ctrl);
 
-	ret = xvcu_pll_wait_for_lock(xvcu);
+	ret = xvcu_pll_wait_for_lock(pll);
 	if (ret) {
-		dev_err(xvcu->dev, "PLL is not locked\n");
+		pr_err("VCU PLL is not locked\n");
 		goto err;
 	}
 
@@ -381,15 +415,14 @@ static int xvcu_pll_enable(struct xvcu_device *xvcu)
 			     0, VCU_PLL_CTRL_BYPASS_MASK,
 			     VCU_PLL_CTRL_BYPASS_SHIFT);
 
-	return ret;
 err:
-	clk_disable_unprepare(xvcu->pll_ref);
 	return ret;
 }
 
-static void xvcu_pll_disable(struct xvcu_device *xvcu)
+static void xvcu_pll_disable(struct clk_hw *hw)
 {
-	void __iomem *base = xvcu->vcu_slcr_ba;
+	struct vcu_pll *pll = to_vcu_pll(hw);
+	void __iomem *base = pll->reg_base;
 	u32 vcu_pll_ctrl;
 
 	vcu_pll_ctrl = xvcu_read(base, VCU_PLL_CTRL);
@@ -400,8 +433,49 @@ static void xvcu_pll_disable(struct xvcu_device *xvcu)
 	vcu_pll_ctrl &= ~(VCU_PLL_CTRL_RESET_MASK << VCU_PLL_CTRL_RESET_SHIFT);
 	vcu_pll_ctrl |= (1 & VCU_PLL_CTRL_RESET_MASK) << VCU_PLL_CTRL_RESET_SHIFT;
 	xvcu_write(base, VCU_PLL_CTRL, vcu_pll_ctrl);
+}
+
+static const struct clk_ops vcu_pll_ops = {
+	.enable = xvcu_pll_enable,
+	.disable = xvcu_pll_disable,
+	.round_rate = xvcu_pll_round_rate,
+	.recalc_rate = xvcu_pll_recalc_rate,
+	.set_rate = xvcu_pll_set_rate,
+};
+
+static struct clk_hw *xvcu_register_pll(struct device *dev,
+					void __iomem *reg_base,
+					const char *name, const char *parent,
+					unsigned long flags)
+{
+	struct vcu_pll *pll;
+	struct clk_hw *hw;
+	struct clk_init_data init;
+	int ret;
+
+	init.name = name;
+	init.parent_names = &parent;
+	init.ops = &vcu_pll_ops;
+	init.num_parents = 1;
+	init.flags = flags;
+
+	pll = devm_kmalloc(dev, sizeof(*pll), GFP_KERNEL);
+	if (!pll)
+		return ERR_PTR(-ENOMEM);
+
+	pll->hw.init = &init;
+	pll->reg_base = reg_base;
+	pll->fvco_min = FVCO_MIN;
+	pll->fvco_max = FVCO_MAX;
+
+	hw = &pll->hw;
+	ret = devm_clk_hw_register(dev, hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	clk_hw_set_rate_range(hw, pll->fvco_min, pll->fvco_max);
 
-	clk_disable_unprepare(xvcu->pll_ref);
+	return hw;
 }
 
 /**
@@ -426,7 +500,6 @@ static int xvcu_set_vcu_pll_info(struct xvcu_device *xvcu)
 	u32 pll_clk;
 	u32 mod;
 	int i;
-	int ret;
 	const struct xvcu_pll_cfg *found = NULL;
 	struct clk_hw *hw;
 
@@ -486,13 +559,9 @@ static int xvcu_set_vcu_pll_info(struct xvcu_device *xvcu)
 	dev_dbg(xvcu->dev, "Actual Core clock freq is %uHz\n", coreclk);
 	dev_dbg(xvcu->dev, "Actual Mcu clock freq is %uHz\n", mcuclk);
 
-	ret = xvcu_pll_set_rate(xvcu, fvco, refclk);
-	if (ret)
-		return ret;
-
-	hw = clk_hw_register_fixed_rate(xvcu->dev, "vcu_pll",
-					__clk_get_name(xvcu->pll_ref),
-					0, fvco);
+	hw = xvcu_register_pll(xvcu->dev, xvcu,
+			       "vcu_pll", __clk_get_name(xvcu->pll_ref),
+			       CLK_SET_RATE_NO_REPARENT);
 	if (IS_ERR(hw))
 		return PTR_ERR(hw);
 	xvcu->pll = hw;
@@ -519,7 +588,7 @@ static int xvcu_set_pll(struct xvcu_device *xvcu)
 		return ret;
 	}
 
-	return xvcu_pll_enable(xvcu);
+	return 0;
 }
 
 static struct clk_hw *xvcu_clk_hw_register_leaf(struct device *dev,
@@ -630,6 +699,13 @@ static int xvcu_register_clock_provider(struct xvcu_device *xvcu)
 
 	xvcu->clk_data = data;
 
+	hw = xvcu_register_pll(dev, reg_base,
+			       "vcu_pll", __clk_get_name(xvcu->pll_ref),
+			       CLK_SET_RATE_NO_REPARENT | CLK_OPS_PARENT_ENABLE);
+	if (IS_ERR(hw))
+		return PTR_ERR(hw);
+	xvcu->pll = hw;
+
 	hw = xvcu_register_pll_post(dev, "vcu_pll_post",
 				    clk_hw_get_name(xvcu->pll), reg_base);
 	if (IS_ERR(hw))
@@ -803,7 +879,6 @@ static int xvcu_remove(struct platform_device *pdev)
 	/* Add the the Gasket isolation and put the VCU in reset. */
 	regmap_write(xvcu->logicore_reg_ba, VCU_GASKET_INIT, 0);
 
-	xvcu_pll_disable(xvcu);
 	clk_disable_unprepare(xvcu->aclk);
 
 	return 0;
-- 
2.20.1