can: mcp251xfd: mcp251xfd_chip_wait_for_osc_ready(): factor out into separate function

This patch factors out mcp251xfd_chip_wait_for_osc_ready() into a
separate function, it will be used in several places in the next
patches.

Link: https://lore.kernel.org/all/20220207131047.282110-7-mkl@pengutronix.de
Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>

diff --git a/drivers/net/can/spi/mcp251xfd/mcp251xfd-core.c b/drivers/net/can/spi/mcp251xfd/mcp251xfd-core.c
index e8e736e..6222633 100644 (file)
--- a/drivers/net/can/spi/mcp251xfd/mcp251xfd-core.c
+++ b/drivers/net/can/spi/mcp251xfd/mcp251xfd-core.c
@@ -246,6 +246,32 @@ mcp251xfd_chip_set_mode_nowait(const struct mcp251xfd_priv *priv,
        return __mcp251xfd_chip_set_mode(priv, mode_req, true);
 }
 
+static int
+mcp251xfd_chip_wait_for_osc_ready(const struct mcp251xfd_priv *priv,
+                                 u32 osc_reference, u32 osc_mask)
+{
+       u32 osc;
+       int err;
+
+       err = regmap_read_poll_timeout(priv->map_reg, MCP251XFD_REG_OSC, osc,
+                                      (osc & osc_mask) == osc_reference,
+                                      MCP251XFD_OSC_STAB_SLEEP_US,
+                                      MCP251XFD_OSC_STAB_TIMEOUT_US);
+       if (mcp251xfd_reg_invalid(osc)) {
+               netdev_err(priv->ndev,
+                          "Failed to detect %s (osc=0x%08x).\n",
+                          mcp251xfd_get_model_str(priv), osc);
+               return -ENODEV;
+       } else if (err == -ETIMEDOUT) {
+               netdev_err(priv->ndev,
+                          "Timeout waiting for Oscillator Ready (osc=0x%08x, osc_reference=0x%08x)\n",
+                          osc, osc_reference);
+               return -ETIMEDOUT;
+       }
+
+       return 0;
+}
+
 static int mcp251xfd_chip_clock_enable(const struct mcp251xfd_priv *priv)
 {
        u32 osc, osc_reference, osc_mask;
@@ -269,24 +295,7 @@ static int mcp251xfd_chip_clock_enable(const struct mcp251xfd_priv *priv)
        if (err)
                return err;
 
-       /* Wait for "Oscillator Ready" bit */
-       err = regmap_read_poll_timeout(priv->map_reg, MCP251XFD_REG_OSC, osc,
-                                      (osc & osc_mask) == osc_reference,
-                                      MCP251XFD_OSC_STAB_SLEEP_US,
-                                      MCP251XFD_OSC_STAB_TIMEOUT_US);
-       if (mcp251xfd_reg_invalid(osc)) {
-               netdev_err(priv->ndev,
-                          "Failed to detect %s (osc=0x%08x).\n",
-                          mcp251xfd_get_model_str(priv), osc);
-               return -ENODEV;
-       } else if (err == -ETIMEDOUT) {
-               netdev_err(priv->ndev,
-                          "Timeout waiting for Oscillator Ready (osc=0x%08x, osc_reference=0x%08x)\n",
-                          osc, osc_reference);
-               return -ETIMEDOUT;
-       }
-
-       return err;
+       return mcp251xfd_chip_wait_for_osc_ready(priv, osc_reference, osc_mask);
 }
 
 static inline int mcp251xfd_chip_sleep(const struct mcp251xfd_priv *priv)