soc: xilinx: vcu: add helper to wait for PLL locked

Extract a helper function to wait until the PLL is locked. Also,
disabling the bypass was buried in the exit path on the wait loop.
Separate the different steps and add a helper function to make the code
more readable.

Signed-off-by: Michael Tretter <m.tretter@pengutronix.de>
Acked-by: Michal Simek <michal.simek@xilinx.com>
Link: https://lore.kernel.org/r/20210121071659.1226489-5-m.tretter@pengutronix.de
Signed-off-by: Stephen Boyd <sboyd@kernel.org>

diff --git a/drivers/soc/xilinx/xlnx_vcu.c b/drivers/soc/xilinx/xlnx_vcu.c
index 7da9643..0fd8356 100644 (file)
--- a/drivers/soc/xilinx/xlnx_vcu.c
+++ b/drivers/soc/xilinx/xlnx_vcu.c
@@ -256,6 +256,22 @@ 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)
+{
+       void __iomem *base = xvcu->vcu_slcr_ba;
+       unsigned long timeout;
+       u32 lock_status;
+
+       timeout = jiffies + msecs_to_jiffies(2000);
+       do {
+               lock_status = xvcu_read(base, VCU_PLL_STATUS);
+               if (lock_status & VCU_PLL_STATUS_LOCK_STATUS_MASK)
+                       return 0;
+       } while (!time_after(jiffies, timeout));
+
+       return -ETIMEDOUT;
+}
+
 /**
  * xvcu_set_vcu_pll_info - Set the VCU PLL info
  * @xvcu:      Pointer to the xvcu_device structure
@@ -428,8 +444,6 @@ static int xvcu_set_vcu_pll_info(struct xvcu_device *xvcu)
  */
 static int xvcu_set_pll(struct xvcu_device *xvcu)
 {
-       u32 lock_status;
-       unsigned long timeout;
        int ret;
 
        ret = xvcu_set_vcu_pll_info(xvcu);
@@ -447,24 +461,18 @@ static int xvcu_set_pll(struct xvcu_device *xvcu)
        xvcu_write_field_reg(xvcu->vcu_slcr_ba, VCU_PLL_CTRL,
                             0, VCU_PLL_CTRL_RESET_MASK,
                             VCU_PLL_CTRL_RESET_SHIFT);
-       /*
-        * Defined the timeout for the max time to wait the
-        * PLL_STATUS to be locked.
-        */
-       timeout = jiffies + msecs_to_jiffies(2000);
-       do {
-               lock_status = xvcu_read(xvcu->vcu_slcr_ba, VCU_PLL_STATUS);
-               if (lock_status & VCU_PLL_STATUS_LOCK_STATUS_MASK) {
-                       xvcu_write_field_reg(xvcu->vcu_slcr_ba, VCU_PLL_CTRL,
-                                            0, VCU_PLL_CTRL_BYPASS_MASK,
-                                            VCU_PLL_CTRL_BYPASS_SHIFT);
-                       return 0;
-               }
-       } while (!time_after(jiffies, timeout));
 
-       /* PLL is not locked even after the timeout of the 2sec */
-       dev_err(xvcu->dev, "PLL is not locked\n");
-       return -ETIMEDOUT;
+       ret = xvcu_pll_wait_for_lock(xvcu);
+       if (ret) {
+               dev_err(xvcu->dev, "PLL is not locked\n");
+               return ret;
+       }
+
+       xvcu_write_field_reg(xvcu->vcu_slcr_ba, VCU_PLL_CTRL,
+                            0, VCU_PLL_CTRL_BYPASS_MASK,
+                            VCU_PLL_CTRL_BYPASS_SHIFT);
+
+       return ret;
 }
 
 /**