1 files changed, 34 insertions, 43 deletions

diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index 29ee5f7072a4..06537f2b2fb8 100644
--- a/drivers/sh/clk/cpg.c
+++ b/drivers/sh/clk/cpg.c
@@ -14,6 +14,8 @@
 #include <linux/io.h>
 #include <linux/sh_clk.h>
 
+#define CPG_CKSTP_BIT	BIT(8)
+
 static unsigned int sh_clk_read(struct clk *clk)
 {
 	if (clk->flags & CLK_ENABLE_REG_8BIT)
@@ -122,6 +124,30 @@ static int sh_clk_div_set_rate(struct clk *clk, unsigned long rate)
 	return 0;
 }
 
+static int sh_clk_div_enable(struct clk *clk)
+{
+	sh_clk_write(sh_clk_read(clk) & ~CPG_CKSTP_BIT, clk);
+	return 0;
+}
+
+static void sh_clk_div_disable(struct clk *clk)
+{
+	unsigned int val;
+
+	val = sh_clk_read(clk);
+	val |= CPG_CKSTP_BIT;
+
+	/*
+	 * div6 clocks require the divisor field to be non-zero or the
+	 * above CKSTP toggle silently fails. Ensure that the divisor
+	 * array is reset to its initial state on disable.
+	 */
+	if (clk->flags & CLK_MASK_DIV_ON_DISABLE)
+		val |= clk->div_mask;
+
+	sh_clk_write(val, clk);
+}
+
 /*
  * div6 support
  */
@@ -174,44 +200,20 @@ static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
 	return 0;
 }
 
-static int sh_clk_div6_enable(struct clk *clk)
-{
-	unsigned long value;
-	int ret;
-
-	ret = sh_clk_div_set_rate(clk, clk->rate);
-	if (ret == 0) {
-		value = sh_clk_read(clk);
-		value &= ~0x100; /* clear stop bit to enable clock */
-		sh_clk_write(value, clk);
-	}
-	return ret;
-}
-
-static void sh_clk_div6_disable(struct clk *clk)
-{
-	unsigned long value;
-
-	value = sh_clk_read(clk);
-	value |= 0x100; /* stop clock */
-	value |= clk->div_mask; /* VDIV bits must be non-zero, overwrite divider */
-	sh_clk_write(value, clk);
-}
-
 static struct sh_clk_ops sh_clk_div6_clk_ops = {
 	.recalc		= sh_clk_div_recalc,
 	.round_rate	= sh_clk_div_round_rate,
 	.set_rate	= sh_clk_div_set_rate,
-	.enable		= sh_clk_div6_enable,
-	.disable	= sh_clk_div6_disable,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 };
 
 static struct sh_clk_ops sh_clk_div6_reparent_clk_ops = {
 	.recalc		= sh_clk_div_recalc,
 	.round_rate	= sh_clk_div_round_rate,
 	.set_rate	= sh_clk_div_set_rate,
-	.enable		= sh_clk_div6_enable,
-	.disable	= sh_clk_div6_disable,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 	.set_parent	= sh_clk_div6_set_parent,
 };
 
@@ -325,17 +327,6 @@ static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
 	return 0;
 }
 
-static int sh_clk_div4_enable(struct clk *clk)
-{
-	sh_clk_write(sh_clk_read(clk) & ~(1 << 8), clk);
-	return 0;
-}
-
-static void sh_clk_div4_disable(struct clk *clk)
-{
-	sh_clk_write(sh_clk_read(clk) | (1 << 8), clk);
-}
-
 static struct sh_clk_ops sh_clk_div4_clk_ops = {
 	.recalc		= sh_clk_div_recalc,
 	.set_rate	= sh_clk_div_set_rate,
@@ -346,16 +337,16 @@ static struct sh_clk_ops sh_clk_div4_enable_clk_ops = {
 	.recalc		= sh_clk_div_recalc,
 	.set_rate	= sh_clk_div_set_rate,
 	.round_rate	= sh_clk_div_round_rate,
-	.enable		= sh_clk_div4_enable,
-	.disable	= sh_clk_div4_disable,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 };
 
 static struct sh_clk_ops sh_clk_div4_reparent_clk_ops = {
 	.recalc		= sh_clk_div_recalc,
 	.set_rate	= sh_clk_div_set_rate,
 	.round_rate	= sh_clk_div_round_rate,
-	.enable		= sh_clk_div4_enable,
-	.disable	= sh_clk_div4_disable,
+	.enable		= sh_clk_div_enable,
+	.disable	= sh_clk_div_disable,
 	.set_parent	= sh_clk_div4_set_parent,
 };