1 files changed, 66 insertions, 11 deletions

diff --git a/drivers/clk/ti/clk-ctrl.c b/drivers/clk/ti/clk-ctrl.c
index c5c97dc35c4..e3564277982 100644
--- a/drivers/clk/ti/clk-ctrl.c
+++ b/drivers/clk/ti/clk-ctrl.c
@@ -8,7 +8,11 @@
 #include <dm.h>
 #include <dm/device_compat.h>
 #include <clk-uclass.h>
-#include <asm/arch-am33xx/clock.h>
+#include <asm/ti-common/omap_clock.h>
+#include <asm/io.h>
+#include <linux/iopoll.h>
+
+#define LDELAY	1000000
 
 struct clk_ti_ctrl_offs {
 	fdt_addr_t start;
@@ -33,10 +37,66 @@ static int clk_ti_ctrl_check_offs(struct clk *clk, fdt_addr_t offs)
 	return -EFAULT;
 }
 
+#define IDLEST_DISABLED		(MODULE_CLKCTRL_IDLEST_DISABLED << MODULE_CLKCTRL_IDLEST_SHIFT)
+#define IDLEST_TRANSITION	(MODULE_CLKCTRL_IDLEST_TRANSITIONING << MODULE_CLKCTRL_IDLEST_SHIFT)
+static inline void wait_for_clk_disable(u32 addr)
+{
+	u32 bound = LDELAY;
+	int val = 0;
+
+	while ((val & MODULE_CLKCTRL_IDLEST_MASK) != IDLEST_DISABLED) {
+		val = readl(addr);
+
+		if (--bound == 0) {
+			printf("Clock disable failed for 0x%x idlest 0x%x\n",
+			       addr, val);
+			return;
+		}
+	}
+}
+
+static int clk_ti_ctrl_disable_clock_module(u32 addr)
+{
+	clrsetbits_le32(addr, MODULE_CLKCTRL_MODULEMODE_MASK,
+			MODULE_CLKCTRL_MODULEMODE_SW_DISABLE <<
+			MODULE_CLKCTRL_MODULEMODE_SHIFT);
+
+	wait_for_clk_disable(addr);
+
+	return 0;
+}
+
+static inline void wait_for_clk_enable(u32 addr)
+{
+	int val = IDLEST_DISABLED;
+	u32 bound = LDELAY;
+
+	while (((val & MODULE_CLKCTRL_IDLEST_MASK) == IDLEST_DISABLED) ||
+	       ((val & MODULE_CLKCTRL_IDLEST_MASK) == IDLEST_TRANSITION)) {
+		val = readl(addr);
+
+		if (--bound == 0) {
+			printf("Clock enable failed for 0x%x idlest 0x%x\n",
+			       addr, val);
+			return;
+		}
+	}
+}
+
+static int clk_ti_ctrl_enable_clock_module(u32 addr)
+{
+	clrsetbits_le32(addr, MODULE_CLKCTRL_MODULEMODE_MASK,
+			MODULE_CLKCTRL_MODULEMODE_SW_EXPLICIT_EN <<
+			MODULE_CLKCTRL_MODULEMODE_SHIFT);
+
+	wait_for_clk_enable(addr);
+
+	return 0;
+}
+
 static int clk_ti_ctrl_disable(struct clk *clk)
 {
 	struct clk_ti_ctrl_priv *priv = dev_get_priv(clk->dev);
-	u32 *clk_modules[2] = { };
 	fdt_addr_t offs;
 	int err;
 
@@ -47,16 +107,13 @@ static int clk_ti_ctrl_disable(struct clk *clk)
 		return err;
 	}
 
-	clk_modules[0] = (u32 *)(offs);
-	dev_dbg(clk->dev, "disable module @ %p\n", clk_modules[0]);
-	do_disable_clocks(NULL, clk_modules, 1);
-	return 0;
+	dev_dbg(clk->dev, "disable module @ %x\n", offs);
+	return clk_ti_ctrl_disable_clock_module(offs);
 }
 
 static int clk_ti_ctrl_enable(struct clk *clk)
 {
 	struct clk_ti_ctrl_priv *priv = dev_get_priv(clk->dev);
-	u32 *clk_modules[2] = { };
 	fdt_addr_t offs;
 	int err;
 
@@ -67,10 +124,8 @@ static int clk_ti_ctrl_enable(struct clk *clk)
 		return err;
 	}
 
-	clk_modules[0] = (u32 *)(offs);
-	dev_dbg(clk->dev, "enable module @ %p\n", clk_modules[0]);
-	do_enable_clocks(NULL, clk_modules, 1);
-	return 0;
+	dev_dbg(clk->dev, "enable module @ %x\n", offs);
+	return clk_ti_ctrl_enable_clock_module(offs);
 }
 
 static ulong clk_ti_ctrl_get_rate(struct clk *clk)