[U-Boot] [i.MX8MM+CCF 07/41] clk: mux: add set parent support

Peng Fan
2019-04-30T10:17:54Z

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 55fc97367a..1a937bf923 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) ?
(struct clk *)dev_get_driver_data(clk->dev) : clk);
@@ -72,8 +89,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) ?
+ (struct clk *)dev_get_driver_data(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) ?
+ (struct clk *)dev_get_driver_data(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 3458746a60..216095d28c 100644
--- a/include/linux/clk-provider.h
+++ b/include/linux/clk-provider.h
@@ -56,6 +56,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;
--
2.16.4