clk: mux: add set parent support

Add set parent support for clk mux

Signed-off-by: Peng Fan <peng.fan@nxp.com>

diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index 81d1e7e..5acc0b8 100644 (file)
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -60,7 +60,24 @@ int clk_mux_val_to_index(struct clk *clk, u32 *table, unsigned int flags,
        return val;
 }
 
-static u8 clk_mux_get_parent(struct clk *clk)
+unsigned int clk_mux_index_to_val(u32 *table, unsigned int flags, u8 index)
+{
+       unsigned int val = index;
+
+       if (table) {
+               val = table[index];
+       } else {
+               if (flags & CLK_MUX_INDEX_BIT)
+                       val = 1 << index;
+
+               if (flags & CLK_MUX_INDEX_ONE)
+                       val++;
+       }
+
+       return val;
+}
+
+u8 clk_mux_get_parent(struct clk *clk)
 {
        struct clk_mux *mux = to_clk_mux(clk_dev_binded(clk) ?
                        dev_get_clk_ptr(clk->dev) : clk);
@@ -77,8 +94,57 @@ static u8 clk_mux_get_parent(struct clk *clk)
        return clk_mux_val_to_index(clk, mux->table, mux->flags, val);
 }
 
+static int clk_fetch_parent_index(struct clk *clk,
+                                 struct clk *parent)
+{
+       struct clk_mux *mux = to_clk_mux(clk_dev_binded(clk) ?
+                       dev_get_clk_ptr(clk->dev) : clk);
+
+       int i;
+
+       if (!parent)
+               return -EINVAL;
+
+       for (i = 0; i < mux->num_parents; i++) {
+               if (!strcmp(parent->dev->name, mux->parent_names[i]))
+                       return i;
+       }
+
+       return -EINVAL;
+}
+
+static int clk_mux_set_parent(struct clk *clk, struct clk *parent)
+{
+       struct clk_mux *mux = to_clk_mux(clk_dev_binded(clk) ?
+                       dev_get_clk_ptr(clk->dev) : clk);
+       int index;
+       u32 val;
+       u32 reg;
+
+       index = clk_fetch_parent_index(clk, parent);
+       if (index < 0) {
+               printf("Could not fetch index\n");
+               return index;
+       }
+
+       val = clk_mux_index_to_val(mux->table, mux->flags, index);
+
+       if (mux->flags & CLK_MUX_HIWORD_MASK) {
+               reg = mux->mask << (mux->shift + 16);
+       } else {
+               reg = readl(mux->reg);
+               reg &= ~(mux->mask << mux->shift);
+       }
+       val = val << mux->shift;
+       reg |= val;
+       writel(reg, mux->reg);
+
+       return 0;
+}
+
 const struct clk_ops clk_mux_ops = {
-               .get_rate = clk_generic_get_rate,
+       .get_rate = clk_generic_get_rate,
+       .set_parent = clk_mux_set_parent,
 };
 
 struct clk *clk_hw_register_mux_table(struct device *dev, const char *name,

diff --git a/include/linux/clk-provider.h b/include/linux/clk-provider.h
index 43a25e9..7e44045 100644 (file)
--- a/include/linux/clk-provider.h
+++ b/include/linux/clk-provider.h
@@ -66,6 +66,8 @@ struct clk_mux {
 };
 
 #define to_clk_mux(_clk) container_of(_clk, struct clk_mux, clk)
+extern const struct clk_ops clk_mux_ops;
+u8 clk_mux_get_parent(struct clk *clk);
 
 struct clk_div_table {
        unsigned int    val;