2 files changed, 38 insertions, 45 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,
 };
 
diff --git a/include/linux/sh_clk.h b/include/linux/sh_clk.h
index 35a04f19fb53..50910913b268 100644
--- a/include/linux/sh_clk.h
+++ b/include/linux/sh_clk.h
@@ -69,6 +69,8 @@ struct clk {
 #define CLK_ENABLE_REG_16BIT    BIT(2)
 #define CLK_ENABLE_REG_8BIT     BIT(3)
 
+#define CLK_MASK_DIV_ON_DISABLE BIT(4)
+
 #define CLK_ENABLE_REG_MASK     (CLK_ENABLE_REG_32BIT | \
                                  CLK_ENABLE_REG_16BIT | \
                                  CLK_ENABLE_REG_8BIT)
@@ -173,7 +175,7 @@ int sh_clk_div4_reparent_register(struct clk *clks, int nr,
 {                                                               \
         .enable_reg = (void __iomem *)_reg,                     \
         .enable_bit = 0, /* unused */                           \
-        .flags = _flags,                                        \
+        .flags = _flags | CLK_MASK_DIV_ON_DISABLE,              \
         .div_mask = SH_CLK_DIV6_MSK,                            \
         .parent_table = _parents,                               \
         .parent_num = _num_parents,                             \
@@ -187,7 +189,7 @@ int sh_clk_div4_reparent_register(struct clk *clks, int nr,
         .enable_reg     = (void __iomem *)_reg,                 \
         .enable_bit     = 0,    /* unused */                    \
         .div_mask       = SH_CLK_DIV6_MSK,                      \
-        .flags          = _flags,                               \
+        .flags          = _flags | CLK_MASK_DIV_ON_DISABLE,     \
 }
 
 int sh_clk_div6_register(struct clk *clks, int nr);