1 files changed, 197 insertions, 30 deletions

diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index e9cb614005..1d94e09167 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -1,18 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
 /*
  * clk-mux.c - generic barebox clock support. Based on Linux clk support
  *
  * Copyright (c) 2012 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License as
- * published by the Free Software Foundation; either version 2 of
- * the License, or (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
  */
 #include <common.h>
 #include <io.h>
@@ -20,44 +10,153 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 
-static int clk_mux_get_parent(struct clk *clk)
+int clk_mux_val_to_index(struct clk_hw *hw, u32 *table, unsigned int flags,
+			 unsigned int val)
+{
+	int num_parents = clk_hw_get_num_parents(hw);
+
+	if (table) {
+		int i;
+
+		for (i = 0; i < num_parents; i++)
+			if (table[i] == val)
+				return i;
+		return -EINVAL;
+	}
+
+	return val;
+}
+EXPORT_SYMBOL_GPL(clk_mux_val_to_index);
+
+unsigned int clk_mux_index_to_val(u32 *table, unsigned int flags, u8 index)
+{
+	return table ? table[index] : index;
+}
+EXPORT_SYMBOL_GPL(clk_mux_index_to_val);
+
+static int clk_mux_get_parent(struct clk_hw *hw)
 {
-	struct clk_mux *m = container_of(clk, struct clk_mux, clk);
+	struct clk_mux *m = to_clk_mux(hw);
 	int idx = readl(m->reg) >> m->shift & ((1 << m->width) - 1);
 
-	return idx;
+	return clk_mux_val_to_index(hw, m->table, m->flags, idx);
 }
 
-static int clk_mux_set_parent(struct clk *clk, u8 idx)
+static int clk_mux_set_parent(struct clk_hw *hw, u8 idx)
 {
-	struct clk_mux *m = container_of(clk, struct clk_mux, clk);
+	struct clk_mux *m = to_clk_mux(hw);
 	u32 val;
 
 	if (m->flags & CLK_MUX_READ_ONLY) {
-		if (clk_mux_get_parent(clk) != idx)
+		if (clk_mux_get_parent(hw) != idx)
 			return -EPERM;
 		else
 			return 0;
 	}
 
+	idx = clk_mux_index_to_val(m->table, m->flags, idx);
+
 	val = readl(m->reg);
 	val &= ~(((1 << m->width) - 1) << m->shift);
 	val |= idx << m->shift;
 
-	if (clk->flags & CLK_MUX_HIWORD_MASK)
+	if (m->flags & CLK_MUX_HIWORD_MASK)
 		val |= ((1 << m->width) - 1) << (m->shift + 16);
 	writel(val, m->reg);
 
 	return 0;
 }
 
-struct clk_ops clk_mux_ops = {
-	.set_rate = clk_parent_set_rate,
-	.round_rate = clk_parent_round_rate,
+static struct clk *clk_get_parent_index(struct clk *clk, int num)
+{
+	if (num >= clk->num_parents)
+		return NULL;
+
+	if (clk->parents[num])
+		return clk->parents[num];
+
+	clk->parents[num] = clk_lookup(clk->parent_names[num]);
+
+	return clk->parents[num];
+}
+
+static struct clk *clk_mux_best_parent(struct clk *mux, unsigned long rate,
+	unsigned long *rrate)
+{
+	struct clk *bestparent = NULL;
+	long bestrate = LONG_MAX;
+	int i;
+
+	for (i = 0; i < mux->num_parents; i++) {
+		struct clk *parent = clk_get_parent_index(mux, i);
+		unsigned long r;
+
+		if (IS_ERR_OR_NULL(parent))
+			continue;
+
+		if (mux->flags & CLK_SET_RATE_PARENT)
+			r = clk_round_rate(parent, rate);
+		else
+			r = clk_get_rate(parent);
+
+		if (abs((long)rate - r) < abs((long)rate - bestrate)) {
+			bestrate = r;
+			bestparent = parent;
+		}
+	}
+
+	*rrate = bestrate;
+
+	return bestparent;
+}
+
+long clk_mux_round_rate(struct clk_hw *hw, unsigned long rate,
+			unsigned long *prate)
+{
+	struct clk *clk = clk_hw_to_clk(hw);
+	unsigned long rrate;
+	struct clk *bestparent;
+
+	if (clk->flags & CLK_SET_RATE_NO_REPARENT)
+		return clk_parent_round_rate(hw, rate, prate);
+
+	bestparent = clk_mux_best_parent(clk, rate, &rrate);
+
+	return rrate;
+}
+EXPORT_SYMBOL_GPL(clk_mux_round_rate);
+
+static int clk_mux_set_rate(struct clk_hw *hw, unsigned long rate,
+			unsigned long parent_rate)
+{
+	struct clk *clk = clk_hw_to_clk(hw);
+	struct clk *parent;
+	unsigned long rrate;
+	int ret;
+
+	if (clk->flags & CLK_SET_RATE_NO_REPARENT)
+		return clk_parent_set_rate(hw, rate, parent_rate);
+
+	parent = clk_mux_best_parent(clk, rate, &rrate);
+
+	ret = clk_set_parent(clk, parent);
+	if (ret)
+		return ret;
+
+	return clk_set_rate(parent, rate);
+}
+
+const struct clk_ops clk_mux_ops = {
+	.set_rate = clk_mux_set_rate,
+	.round_rate = clk_mux_round_rate,
 	.get_parent = clk_mux_get_parent,
 	.set_parent = clk_mux_set_parent,
 };
 
+const struct clk_ops clk_mux_ro_ops = {
+	.get_parent = clk_mux_get_parent,
+};
+
 struct clk *clk_mux_alloc(const char *name, unsigned clk_flags, void __iomem *reg,
 		u8 shift, u8 width, const char * const *parents, u8 num_parents,
 		unsigned mux_flags)
@@ -68,18 +167,19 @@ struct clk *clk_mux_alloc(const char *name, unsigned clk_flags, void __iomem *re
 	m->shift = shift;
 	m->width = width;
 	m->flags = mux_flags;
-	m->clk.ops = &clk_mux_ops;
-	m->clk.name = name;
-	m->clk.flags = clk_flags;
-	m->clk.parent_names = parents;
-	m->clk.num_parents = num_parents;
+	m->hw.clk.ops = &clk_mux_ops;
+	m->hw.clk.name = name;
+	m->hw.clk.flags = clk_flags;
+	m->hw.clk.parent_names = parents;
+	m->hw.clk.num_parents = num_parents;
 
-	return &m->clk;
+	return &m->hw.clk;
 }
 
 void clk_mux_free(struct clk *clk_mux)
 {
-	struct clk_mux *m = to_clk_mux(clk_mux);
+	struct clk_hw *hw = clk_to_clk_hw(clk_mux);
+	struct clk_mux *m = to_clk_mux(hw);
 
 	free(m);
 }
@@ -94,11 +194,78 @@ struct clk *clk_mux(const char *name, unsigned clk_flags, void __iomem *reg,
 	m = clk_mux_alloc(name, clk_flags, reg, shift, width, parents,
 			  num_parents, mux_flags);
 
-	ret = clk_register(m);
+	ret = bclk_register(m);
 	if (ret) {
-		free(to_clk_mux(m));
+		struct clk_hw *hw = clk_to_clk_hw(m);
+		free(to_clk_mux(hw));
 		return ERR_PTR(ret);
 	}
 
 	return m;
 }
+
+struct clk *clk_register_mux(struct device *dev, const char *name,
+			     const char * const *parent_names, u8 num_parents,
+			     unsigned long flags,
+			     void __iomem *reg, u8 shift, u8 width,
+			     u8 clk_mux_flags, spinlock_t *lock)
+{
+	return clk_mux(name, flags, reg, shift, width, parent_names,
+		       num_parents, clk_mux_flags);
+}
+
+struct clk_hw *__clk_hw_register_mux(struct device *dev,
+				     const char *name, u8 num_parents,
+				     const char * const *parent_names,
+				     unsigned long flags, void __iomem *reg,
+				     u8 shift, u32 mask,
+				     u8 clk_mux_flags, u32 *table,
+				     spinlock_t *lock)
+{
+	struct clk_mux *mux;
+	struct clk_hw *hw;
+	struct clk_init_data init = {};
+	u8 width = 0;
+	int ret = -EINVAL;
+
+	width = fls(mask) - ffs(mask) + 1;
+
+	if (clk_mux_flags & CLK_MUX_HIWORD_MASK) {
+		if (width + shift > 16) {
+			pr_err("mux value exceeds LOWORD field\n");
+			return ERR_PTR(-EINVAL);
+		}
+	}
+
+	/* allocate the mux */
+	mux = kzalloc(sizeof(*mux), GFP_KERNEL);
+	if (!mux)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	if (clk_mux_flags & CLK_MUX_READ_ONLY)
+		init.ops = &clk_mux_ro_ops;
+	else
+		init.ops = &clk_mux_ops;
+	init.flags = flags;
+	init.parent_names = parent_names;
+	init.num_parents = num_parents;
+
+	/* struct clk_mux assignments */
+	mux->reg = reg;
+	mux->shift = shift;
+	mux->width = width;
+	mux->flags = clk_mux_flags;
+	mux->lock = lock;
+	mux->table = table;
+	mux->hw.init = &init;
+
+	hw = &mux->hw;
+	ret = clk_hw_register(dev, hw);
+	if (ret) {
+		kfree(mux);
+		hw = ERR_PTR(ret);
+	}
+
+	return hw;
+}