[PATCH v2 4/7] clk: imx6q: configure ldb clock selectors

[Brian Ruley]

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.

Signed-off-by: Brian Ruley <brian.ruley at gehealthcare.com>
---
 drivers/clk/imx/clk-imx6q.c | 121 +++++++++++++++++++++++++++++++++++-
 1 file changed, 119 insertions(+), 2 deletions(-)

diff --git a/drivers/clk/imx/clk-imx6q.c b/drivers/clk/imx/clk-imx6q.c
index 6dadfd9b59c..ba039e56227 100644
--- 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"
@@ -140,6 +141,121 @@ static const char *const pll6_bypass_sels[] = {
 
 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]);
+}
+
 static int imx6q_clk_probe(struct udevice *dev)
 {
 	void *base;
@@ -350,9 +466,10 @@ static int imx6q_clk_probe(struct udevice *dev)
 				   ldb_di_sels, ARRAY_SIZE(ldb_di_sels)));
 	} else {
 		/*
-		 * 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),
-- 
2.47.3