clk: imx6q: configure ldb clock selectors

A hardware bug prevents LDB clock selectors from being configured later
on non-plus i.MX6QD variants, so let's set the desired configuration in
the probe before we register them. We also have to make the necessary
clock functions available in XPL builds.

Signed-off-by: Brian Ruley <brian.ruley@gehealthcare.com>

diff --git a/drivers/clk/imx/clk-imx6q.c b/drivers/clk/imx/clk-imx6q.c
index 9ee3109bf1d5d56f2ea942c643ed297bda698af5..393b4215fe8beba4e89c42a6760d6f00a1cd174c 100644 (file)
--- a/drivers/clk/imx/clk-imx6q.c
+++ b/drivers/clk/imx/clk-imx6q.c
@@ -9,6 +9,7 @@
 #include <log.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/imx-regs.h>
+#include <dm/of_access.h>
 #include <dt-bindings/clock/imx6qdl-clock.h>
 
 #include "clk.h"
@@ -141,6 +142,121 @@ static const char *ipu2_di1_sels_2[] = {
 };
 
 static unsigned int share_count_mipi_core_cfg;
+
+static void of_assigned_ldb_sels(struct udevice *dev, int *ldb_di0_sel,
+                                int *ldb_di1_sel)
+{
+       struct ofnode_phandle_args clk_args, parent_args;
+       ofnode node = dev_ofnode(dev);
+       int count, err;
+
+       count = dev_count_phandle_with_args(dev, "assigned-clocks",
+                                           "#clock-cells", 0);
+       if (count <= 0) {
+               if (count == 0)
+                       debug("%s: no assigned_clocks found\n", dev->name);
+               else
+                       pr_err("%s: failed to get phandle count (%d)\n",
+                              dev->name, count);
+               return;
+       }
+
+       for (int i = 0; i < count; i++) {
+               err = dev_read_phandle_with_args(dev, "assigned-clocks",
+                                                "#clock-cells", 0, i,
+                                                &clk_args);
+               if (err == -ENOENT)
+                       /* Skip empty handles */
+                       continue;
+               else if (err < 0)
+                       return;
+
+               if (!ofnode_equal(clk_args.node, node) ||
+                   clk_args.args[0] >= IMX6QDL_CLK_END) {
+                       pr_err("%s: clock %d not in ccm\n", dev->name, i);
+                       return;
+               }
+
+               err = dev_read_phandle_with_args(dev, "assigned-clock-parents",
+                                                "#clock-cells", 0, i,
+                                                &parent_args);
+               if (err < 0)
+                       return;
+
+               if (!ofnode_equal(parent_args.node, node) ||
+                   parent_args.args[0] >= IMX6QDL_CLK_END) {
+                       pr_err("%s: parent clock %d not in ccm\n", dev->name,
+                              i);
+                       return;
+               }
+
+               if (clk_args.args[0] == IMX6QDL_CLK_LDB_DI0_SEL)
+                       *ldb_di0_sel = parent_args.args[0];
+               else if (clk_args.args[0] == IMX6QDL_CLK_LDB_DI1_SEL)
+                       *ldb_di1_sel = parent_args.args[0];
+       }
+}
+
+static void imx6q_init_ldb_clks(struct udevice *dev)
+{
+       int ldb_di_sel[] = { IMX6QDL_CLK_END, IMX6QDL_CLK_END };
+       enum ldb_di_clock ldb_di_clk[] = { MXC_MMDC_CH1_CLK, MXC_MMDC_CH1_CLK };
+
+       of_assigned_ldb_sels(dev, &ldb_di_sel[0], &ldb_di_sel[1]);
+       for (int i = 0; i < 2; i++) {
+               switch (ldb_di_sel[i]) {
+               case IMX6QDL_CLK_PLL5_VIDEO_DIV:
+                       ldb_di_clk[i] = MXC_PLL5_CLK;
+                       break;
+               case IMX6QDL_CLK_PLL2_PFD0_352M:
+                       ldb_di_clk[i] = MXC_PLL2_PFD0_CLK;
+                       break;
+               case IMX6QDL_CLK_PLL2_PFD2_396M: {
+                       struct clk *clk, *parent;
+
+                       int err = clk_get_by_id(IMX6QDL_CLK_PERIPH_PRE, &clk);
+
+                       if (err) {
+                               pr_err("%s: failed to get periph_pre clock "
+                                      "(%d)\n",
+                                      dev->name, err);
+                               return;
+                       }
+
+                       err = clk_get_by_id(IMX6QDL_CLK_PLL2_PFD2_396M,
+                                           &parent);
+                       if (err) {
+                               pr_err("%s: failed to get pll2_pfd2_396m clock"
+                                      " (%d)\n",
+                                      dev->name, err);
+                               return;
+                       }
+
+                       if (parent == clk) {
+                               pr_err("%s: ldb_di%d_sel: couldn't disable "
+                                      "pll2_pfd2_396m clock\n",
+                                      dev->name, i);
+                               return;
+                       }
+
+                       ldb_di_clk[i] = MXC_PLL2_PFD2_CLK;
+                       break;
+               }
+               case IMX6QDL_CLK_MMDC_CH1_AXI:
+               case IMX6QDL_CLK_END:
+                       /* use the default clock */
+                       break;
+               case IMX6QDL_CLK_PLL3_USB_OTG:
+                       ldb_di_clk[i] = MXC_PLL3_SW_CLK;
+                       break;
+               default:
+                       pr_err("%s: invalid LDB clock parent\n", dev->name);
+                       return;
+               }
+       }
+
+       select_ldb_di_clock_source(ldb_di_clk[0], ldb_di_clk[1]);
+}
 #endif /* CONFIG_IS_ENABLED(VIDEO) */
 
 static int imx6q_clk_probe(struct udevice *dev)
@@ -356,7 +472,10 @@ static int imx6q_clk_probe(struct udevice *dev)
                /*
                 * Need to set these as read-only due to a hardware bug.
                 * Keeping default mux values. Fixed on the i.MX6 QuadPlus
+                * Need to set the clocks now and make them read-only due to a
+                * hardware bug. Fixed on the i.MX6 QuadPlus
                 */
+               imx6q_init_ldb_clks(dev);
                clk_dm(IMX6QDL_CLK_LDB_DI0_SEL,
                       imx_clk_mux_flags(dev, "ldb_di0_sel", base + 0x2c, 9, 3,
                                         ldb_di_sels, ARRAY_SIZE(ldb_di_sels),