clk: mux: add clk_hw registration functions

Save users the hassle of opencoding by providing wrappers with the same
Linux semantics: names are duplicated, same arguments and struct clk_hw
is returned.

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
Link: https://lore.barebox.org/20220131075725.1873026-8-a.fatoum@pengutronix.de
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

2 files changed, 108 insertions, 1 deletions

diff --git a/drivers/clk/clk-mux.c b/drivers/clk/clk-mux.c
index d8c09e4a75..d608e0e2c5 100644
--- a/drivers/clk/clk-mux.c
+++ b/drivers/clk/clk-mux.c
@@ -10,12 +10,36 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 
+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 = 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_hw *hw, u8 idx)
@@ -30,6 +54,8 @@ static int clk_mux_set_parent(struct clk_hw *hw, u8 idx)
 			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;
@@ -186,3 +212,57 @@ struct clk *clk_register_mux(struct device_d *dev, const char *name,
 	return clk_mux(name, flags, reg, shift, width, parent_names,
 		       num_parents, clk_mux_flags);
 }
+
+struct clk_hw *__clk_hw_register_mux(struct device_d *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;
+}
diff --git a/include/linux/clk.h b/include/linux/clk.h
index 9f2069dc9e..44c0cc7b1f 100644
--- a/include/linux/clk.h
+++ b/include/linux/clk.h
@@ -619,6 +619,7 @@ struct clk_mux {
 	int shift;
 	int width;
 	unsigned flags;
+	u32 *table;
 	spinlock_t *lock;
 };
 
@@ -641,6 +642,32 @@ struct clk *clk_register_mux(struct device_d *dev, const char *name,
 		void __iomem *reg, u8 shift, u8 width,
 		u8 clk_mux_flags, spinlock_t *lock);
 
+struct clk_hw *__clk_hw_register_mux(struct device_d *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);
+
+#define clk_hw_register_mux(dev, name, parent_names,                  \
+		num_parents, flags, reg, shift, mask,                 \
+		clk_mux_flags, lock)                                  \
+	__clk_hw_register_mux((dev), (name), (num_parents),           \
+				     (parent_names),                  \
+				     (flags), (reg), (shift), (mask), \
+				     (clk_mux_flags), NULL, (lock))
+
+#define clk_hw_register_mux_table(dev, name, parent_names, num_parents,	  \
+				  flags, reg, shift, mask, clk_mux_flags, \
+				  table, lock)				  \
+	__clk_hw_register_mux((dev), (name), (num_parents),	          \
+			      (parent_names), (flags), (reg),             \
+			      (shift), (mask), (clk_mux_flags), (table),  \
+			      (lock))
+
+int clk_mux_val_to_index(struct clk_hw *hw, u32 *table, unsigned int flags,
+			 unsigned int val);
+unsigned int clk_mux_index_to_val(u32 *table, unsigned int flags, u8 index);
+
 struct clk_gate {
 	struct clk_hw hw;
 	void __iomem *reg;