Add set parent support for clk mux
Signed-off-by: Peng Fan peng.fan@nxp.com --- drivers/clk/clk-mux.c | 70 ++++++++++++++++++++++++++++++++++++++++++-- include/linux/clk-provider.h | 2 ++ 2 files changed, 70 insertions(+), 2 deletions(-)
diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c index 81d1e7ebee..5acc0b8cbd 100644 --- 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 43a25e9c6a..7e44045c16 100644 --- 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;