2 files changed, 159 insertions, 195 deletions

diff --git a/drivers/sh/clk/cpg.c b/drivers/sh/clk/cpg.c
index f0d015dd0fef..07e9fb4f8041 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)
@@ -66,71 +68,43 @@ int __init sh_clk_mstp_register(struct clk *clks, int nr)
         return ret;
 }
 
-static long sh_clk_div_round_rate(struct clk *clk, unsigned long rate)
+/*
+ * Div/mult table lookup helpers
+ */
+static inline struct clk_div_table *clk_to_div_table(struct clk *clk)
 {
-        return clk_rate_table_round(clk, clk->freq_table, rate);
+        return clk->priv;
 }
 
-static int sh_clk_div6_divisors[64] = {
-        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
-        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
-        33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
-        49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64
-};
+static inline struct clk_div_mult_table *clk_to_div_mult_table(struct clk *clk)
+{
+        return clk_to_div_table(clk)->div_mult_table;
+}
 
-static struct clk_div_mult_table sh_clk_div6_table = {
-        .divisors = sh_clk_div6_divisors,
-        .nr_divisors = ARRAY_SIZE(sh_clk_div6_divisors),
-};
+/*
+ * Common div ops
+ */
+static long sh_clk_div_round_rate(struct clk *clk, unsigned long rate)
+{
+        return clk_rate_table_round(clk, clk->freq_table, rate);
+}
 
-static unsigned long sh_clk_div6_recalc(struct clk *clk)
+static unsigned long sh_clk_div_recalc(struct clk *clk)
 {
-        struct clk_div_mult_table *table = &sh_clk_div6_table;
+        struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
         unsigned int idx;
 
         clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-                             table, NULL);
+                             table, clk->arch_flags ? &clk->arch_flags : NULL);
 
-        idx = sh_clk_read(clk) & 0x003f;
+        idx = (sh_clk_read(clk) >> clk->enable_bit) & clk->div_mask;
 
         return clk->freq_table[idx].frequency;
 }
 
-static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
-{
-        struct clk_div_mult_table *table = &sh_clk_div6_table;
-        u32 value;
-        int ret, i;
-
-        if (!clk->parent_table || !clk->parent_num)
-                return -EINVAL;
-
-        /* Search the parent */
-        for (i = 0; i < clk->parent_num; i++)
-                if (clk->parent_table[i] == parent)
-                        break;
-
-        if (i == clk->parent_num)
-                return -ENODEV;
-
-        ret = clk_reparent(clk, parent);
-        if (ret < 0)
-                return ret;
-
-        value = sh_clk_read(clk) &
-                ~(((1 << clk->src_width) - 1) << clk->src_shift);
-
-        sh_clk_write(value | (i << clk->src_shift), clk);
-
-        /* Rebuild the frequency table */
-        clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-                             table, NULL);
-
-        return 0;
-}
-
-static int sh_clk_div6_set_rate(struct clk *clk, unsigned long rate)
+static int sh_clk_div_set_rate(struct clk *clk, unsigned long rate)
 {
+        struct clk_div_table *dt = clk_to_div_table(clk);
         unsigned long value;
         int idx;
 
@@ -139,51 +113,53 @@ static int sh_clk_div6_set_rate(struct clk *clk, unsigned long rate)
                 return idx;
 
         value = sh_clk_read(clk);
-        value &= ~0x3f;
-        value |= idx;
+        value &= ~(clk->div_mask << clk->enable_bit);
+        value |= (idx << clk->enable_bit);
         sh_clk_write(value, clk);
+
+        /* XXX: Should use a post-change notifier */
+        if (dt->kick)
+                dt->kick(clk);
+
         return 0;
 }
 
-static int sh_clk_div6_enable(struct clk *clk)
+static int sh_clk_div_enable(struct clk *clk)
 {
-        unsigned long value;
-        int ret;
-
-        ret = sh_clk_div6_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;
+        sh_clk_write(sh_clk_read(clk) & ~CPG_CKSTP_BIT, clk);
+        return 0;
 }
 
-static void sh_clk_div6_disable(struct clk *clk)
+static void sh_clk_div_disable(struct clk *clk)
 {
-        unsigned long value;
+        unsigned int val;
 
-        value = sh_clk_read(clk);
-        value |= 0x100; /* stop clock */
-        value |= 0x3f; /* VDIV bits must be non-zero, overwrite divider */
-        sh_clk_write(value, clk);
+        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);
 }
 
-static struct sh_clk_ops sh_clk_div6_clk_ops = {
-        .recalc         = sh_clk_div6_recalc,
+static struct sh_clk_ops sh_clk_div_clk_ops = {
+        .recalc         = sh_clk_div_recalc,
+        .set_rate       = sh_clk_div_set_rate,
         .round_rate     = sh_clk_div_round_rate,
-        .set_rate       = sh_clk_div6_set_rate,
-        .enable         = sh_clk_div6_enable,
-        .disable        = sh_clk_div6_disable,
 };
 
-static struct sh_clk_ops sh_clk_div6_reparent_clk_ops = {
-        .recalc         = sh_clk_div6_recalc,
+static struct sh_clk_ops sh_clk_div_enable_clk_ops = {
+        .recalc         = sh_clk_div_recalc,
+        .set_rate       = sh_clk_div_set_rate,
         .round_rate     = sh_clk_div_round_rate,
-        .set_rate       = sh_clk_div6_set_rate,
-        .enable         = sh_clk_div6_enable,
-        .disable        = sh_clk_div6_disable,
-        .set_parent     = sh_clk_div6_set_parent,
+        .enable         = sh_clk_div_enable,
+        .disable        = sh_clk_div_disable,
 };
 
 static int __init sh_clk_init_parent(struct clk *clk)
@@ -218,12 +194,12 @@ static int __init sh_clk_init_parent(struct clk *clk)
         return 0;
 }
 
-static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
-                                           struct sh_clk_ops *ops)
+static int __init sh_clk_div_register_ops(struct clk *clks, int nr,
+                        struct clk_div_table *table, struct sh_clk_ops *ops)
 {
         struct clk *clkp;
         void *freq_table;
-        int nr_divs = sh_clk_div6_table.nr_divisors;
+        int nr_divs = table->div_mult_table->nr_divisors;
         int freq_table_size = sizeof(struct cpufreq_frequency_table);
         int ret = 0;
         int k;
@@ -231,7 +207,7 @@ static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
         freq_table_size *= (nr_divs + 1);
         freq_table = kzalloc(freq_table_size * nr, GFP_KERNEL);
         if (!freq_table) {
-                pr_err("sh_clk_div6_register: unable to alloc memory\n");
+                pr_err("%s: unable to alloc memory\n", __func__);
                 return -ENOMEM;
         }
 
@@ -239,47 +215,98 @@ static int __init sh_clk_div6_register_ops(struct clk *clks, int nr,
                 clkp = clks + k;
 
                 clkp->ops = ops;
+                clkp->priv = table;
+
                 clkp->freq_table = freq_table + (k * freq_table_size);
                 clkp->freq_table[nr_divs].frequency = CPUFREQ_TABLE_END;
-                ret = clk_register(clkp);
-                if (ret < 0)
-                        break;
 
-                ret = sh_clk_init_parent(clkp);
+                ret = clk_register(clkp);
+                if (ret == 0)
+                        ret = sh_clk_init_parent(clkp);
         }
 
         return ret;
 }
 
-int __init sh_clk_div6_register(struct clk *clks, int nr)
-{
-        return sh_clk_div6_register_ops(clks, nr, &sh_clk_div6_clk_ops);
-}
+/*
+ * div6 support
+ */
+static int sh_clk_div6_divisors[64] = {
+        1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16,
+        17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32,
+        33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48,
+        49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64
+};
 
-int __init sh_clk_div6_reparent_register(struct clk *clks, int nr)
-{
-        return sh_clk_div6_register_ops(clks, nr,
-                                        &sh_clk_div6_reparent_clk_ops);
-}
+static struct clk_div_mult_table div6_div_mult_table = {
+        .divisors = sh_clk_div6_divisors,
+        .nr_divisors = ARRAY_SIZE(sh_clk_div6_divisors),
+};
 
-static unsigned long sh_clk_div4_recalc(struct clk *clk)
+static struct clk_div_table sh_clk_div6_table = {
+        .div_mult_table = &div6_div_mult_table,
+};
+
+static int sh_clk_div6_set_parent(struct clk *clk, struct clk *parent)
 {
-        struct clk_div4_table *d4t = clk->priv;
-        struct clk_div_mult_table *table = d4t->div_mult_table;
-        unsigned int idx;
+        struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
+        u32 value;
+        int ret, i;
 
+        if (!clk->parent_table || !clk->parent_num)
+                return -EINVAL;
+
+        /* Search the parent */
+        for (i = 0; i < clk->parent_num; i++)
+                if (clk->parent_table[i] == parent)
+                        break;
+
+        if (i == clk->parent_num)
+                return -ENODEV;
+
+        ret = clk_reparent(clk, parent);
+        if (ret < 0)
+                return ret;
+
+        value = sh_clk_read(clk) &
+                ~(((1 << clk->src_width) - 1) << clk->src_shift);
+
+        sh_clk_write(value | (i << clk->src_shift), clk);
+
+        /* Rebuild the frequency table */
         clk_rate_table_build(clk, clk->freq_table, table->nr_divisors,
-                             table, &clk->arch_flags);
+                             table, NULL);
 
-        idx = (sh_clk_read(clk) >> clk->enable_bit) & 0x000f;
+        return 0;
+}
 
-        return clk->freq_table[idx].frequency;
+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_div_enable,
+        .disable        = sh_clk_div_disable,
+        .set_parent     = sh_clk_div6_set_parent,
+};
+
+int __init sh_clk_div6_register(struct clk *clks, int nr)
+{
+        return sh_clk_div_register_ops(clks, nr, &sh_clk_div6_table,
+                                       &sh_clk_div_enable_clk_ops);
+}
+
+int __init sh_clk_div6_reparent_register(struct clk *clks, int nr)
+{
+        return sh_clk_div_register_ops(clks, nr, &sh_clk_div6_table,
+                                       &sh_clk_div6_reparent_clk_ops);
 }
 
+/*
+ * div4 support
+ */
 static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
 {
-        struct clk_div4_table *d4t = clk->priv;
-        struct clk_div_mult_table *table = d4t->div_mult_table;
+        struct clk_div_mult_table *table = clk_to_div_mult_table(clk);
         u32 value;
         int ret;
 
@@ -306,107 +333,31 @@ static int sh_clk_div4_set_parent(struct clk *clk, struct clk *parent)
         return 0;
 }
 
-static int sh_clk_div4_set_rate(struct clk *clk, unsigned long rate)
-{
-        struct clk_div4_table *d4t = clk->priv;
-        unsigned long value;
-        int idx = clk_rate_table_find(clk, clk->freq_table, rate);
-        if (idx < 0)
-                return idx;
-
-        value = sh_clk_read(clk);
-        value &= ~(0xf << clk->enable_bit);
-        value |= (idx << clk->enable_bit);
-        sh_clk_write(value, clk);
-
-        if (d4t->kick)
-                d4t->kick(clk);
-
-        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_div4_recalc,
-        .set_rate       = sh_clk_div4_set_rate,
-        .round_rate     = sh_clk_div_round_rate,
-};
-
-static struct sh_clk_ops sh_clk_div4_enable_clk_ops = {
-        .recalc         = sh_clk_div4_recalc,
-        .set_rate       = sh_clk_div4_set_rate,
-        .round_rate     = sh_clk_div_round_rate,
-        .enable         = sh_clk_div4_enable,
-        .disable        = sh_clk_div4_disable,
-};
-
 static struct sh_clk_ops sh_clk_div4_reparent_clk_ops = {
-        .recalc         = sh_clk_div4_recalc,
-        .set_rate       = sh_clk_div4_set_rate,
+        .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,
 };
 
-static int __init sh_clk_div4_register_ops(struct clk *clks, int nr,
-                        struct clk_div4_table *table, struct sh_clk_ops *ops)
-{
-        struct clk *clkp;
-        void *freq_table;
-        int nr_divs = table->div_mult_table->nr_divisors;
-        int freq_table_size = sizeof(struct cpufreq_frequency_table);
-        int ret = 0;
-        int k;
-
-        freq_table_size *= (nr_divs + 1);
-        freq_table = kzalloc(freq_table_size * nr, GFP_KERNEL);
-        if (!freq_table) {
-                pr_err("sh_clk_div4_register: unable to alloc memory\n");
-                return -ENOMEM;
-        }
-
-        for (k = 0; !ret && (k < nr); k++) {
-                clkp = clks + k;
-
-                clkp->ops = ops;
-                clkp->priv = table;
-
-                clkp->freq_table = freq_table + (k * freq_table_size);
-                clkp->freq_table[nr_divs].frequency = CPUFREQ_TABLE_END;
-
-                ret = clk_register(clkp);
-        }
-
-        return ret;
-}
-
 int __init sh_clk_div4_register(struct clk *clks, int nr,
                                 struct clk_div4_table *table)
 {
-        return sh_clk_div4_register_ops(clks, nr, table, &sh_clk_div4_clk_ops);
+        return sh_clk_div_register_ops(clks, nr, table, &sh_clk_div_clk_ops);
 }
 
 int __init sh_clk_div4_enable_register(struct clk *clks, int nr,
                                 struct clk_div4_table *table)
 {
-        return sh_clk_div4_register_ops(clks, nr, table,
-                                        &sh_clk_div4_enable_clk_ops);
+        return sh_clk_div_register_ops(clks, nr, table,
+                                       &sh_clk_div_enable_clk_ops);
 }
 
 int __init sh_clk_div4_reparent_register(struct clk *clks, int nr,
                                 struct clk_div4_table *table)
 {
-        return sh_clk_div4_register_ops(clks, nr, table,
-                                        &sh_clk_div4_reparent_clk_ops);
+        return sh_clk_div_register_ops(clks, nr, table,
+                                       &sh_clk_div4_reparent_clk_ops);
 }
diff --git a/include/linux/sh_clk.h b/include/linux/sh_clk.h
index c513b73cd7cb..50910913b268 100644
--- a/include/linux/sh_clk.h
+++ b/include/linux/sh_clk.h
@@ -18,7 +18,6 @@ struct clk_mapping {
         struct kref             ref;
 };
 
-
 struct sh_clk_ops {
 #ifdef CONFIG_SH_CLK_CPG_LEGACY
         void (*init)(struct clk *clk);
@@ -31,6 +30,10 @@ struct sh_clk_ops {
         long (*round_rate)(struct clk *clk, unsigned long rate);
 };
 
+#define SH_CLK_DIV_MSK(div)     ((1 << (div)) - 1)
+#define SH_CLK_DIV4_MSK         SH_CLK_DIV_MSK(4)
+#define SH_CLK_DIV6_MSK         SH_CLK_DIV_MSK(6)
+
 struct clk {
         struct list_head        node;
         struct clk              *parent;
@@ -52,6 +55,7 @@ struct clk {
         unsigned int            enable_bit;
         void __iomem            *mapped_reg;
 
+        unsigned int            div_mask;
         unsigned long           arch_flags;
         void                    *priv;
         struct clk_mapping      *mapping;
@@ -65,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)
@@ -146,14 +152,17 @@ static inline int __deprecated sh_clk_mstp32_register(struct clk *clks, int nr)
         .enable_reg = (void __iomem *)_reg,                     \
         .enable_bit = _shift,                                   \
         .arch_flags = _div_bitmap,                              \
+        .div_mask = SH_CLK_DIV4_MSK,                            \
         .flags = _flags,                                        \
 }
 
-struct clk_div4_table {
+struct clk_div_table {
         struct clk_div_mult_table *div_mult_table;
         void (*kick)(struct clk *clk);
 };
 
+#define clk_div4_table clk_div_table
+
 int sh_clk_div4_register(struct clk *clks, int nr,
                          struct clk_div4_table *table);
 int sh_clk_div4_enable_register(struct clk *clks, int nr,
@@ -165,7 +174,9 @@ int sh_clk_div4_reparent_register(struct clk *clks, int nr,
                         _num_parents, _src_shift, _src_width)   \
 {                                                               \
         .enable_reg = (void __iomem *)_reg,                     \
-        .flags = _flags,                                        \
+        .enable_bit = 0, /* unused */                           \
+        .flags = _flags | CLK_MASK_DIV_ON_DISABLE,              \
+        .div_mask = SH_CLK_DIV6_MSK,                            \
         .parent_table = _parents,                               \
         .parent_num = _num_parents,                             \
         .src_shift = _src_shift,                                \
@@ -176,7 +187,9 @@ int sh_clk_div4_reparent_register(struct clk *clks, int nr,
 {                                                               \
         .parent         = _parent,                              \
         .enable_reg     = (void __iomem *)_reg,                 \
-        .flags          = _flags,                               \
+        .enable_bit     = 0,    /* unused */                    \
+        .div_mask       = SH_CLK_DIV6_MSK,                      \
+        .flags          = _flags | CLK_MASK_DIV_ON_DISABLE,     \
 }
 
 int sh_clk_div6_register(struct clk *clks, int nr);