[PATCH v3 05/14] clk: sunxi-ng: Add mux clock support

[Maxime Ripard <maxime.ripard@free-electrons.com>]

Some clocks in the Allwinner SoCs clocks unit are just muxes.

However, those muxes might also be found in some other complicated clocks
that would benefit from the code in there to deal with "advanced" features,
like pre-dividers.

Introduce a set of helpers to reduce the code duplication in such cases.

Signed-off-by: Maxime Ripard <maxime.ripard@free-electrons.com>
---
 drivers/clk/sunxi-ng/Kconfig   |   3 +
 drivers/clk/sunxi-ng/Makefile  |   1 +
 drivers/clk/sunxi-ng/ccu_mux.c | 187 +++++++++++++++++++++++++++++++++++++++++
 drivers/clk/sunxi-ng/ccu_mux.h |  91 ++++++++++++++++++++
 4 files changed, 282 insertions(+)
 create mode 100644 drivers/clk/sunxi-ng/ccu_mux.c
 create mode 100644 drivers/clk/sunxi-ng/ccu_mux.h

diff --git a/drivers/clk/sunxi-ng/Kconfig b/drivers/clk/sunxi-ng/Kconfig
index 2f135c8a61b6..4699a602e0c1 100644
--- a/drivers/clk/sunxi-ng/Kconfig
+++ b/drivers/clk/sunxi-ng/Kconfig
@@ -12,4 +12,7 @@ config SUNXI_CCU_FRAC
 config SUNXI_CCU_GATE
 	bool
 
+config SUNXI_CCU_MUX
+	bool
+
 endif
diff --git a/drivers/clk/sunxi-ng/Makefile b/drivers/clk/sunxi-ng/Makefile
index 48b885f2d8ff..eaa833721245 100644
--- a/drivers/clk/sunxi-ng/Makefile
+++ b/drivers/clk/sunxi-ng/Makefile
@@ -5,3 +5,4 @@ obj-$(CONFIG_SUNXI_CCU)		+= ccu_reset.o
 # Base clock types
 obj-$(CONFIG_SUNXI_CCU_FRAC)	+= ccu_frac.o
 obj-$(CONFIG_SUNXI_CCU_GATE)	+= ccu_gate.o
+obj-$(CONFIG_SUNXI_CCU_MUX)	+= ccu_mux.o
diff --git a/drivers/clk/sunxi-ng/ccu_mux.c b/drivers/clk/sunxi-ng/ccu_mux.c
new file mode 100644
index 000000000000..58fc36e7dcce
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu_mux.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright (C) 2016 Maxime Ripard
+ * Maxime Ripard <maxime.ripard@free-electrons.com>
+ *
+ * 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.
+ */
+
+#include <linux/clk-provider.h>
+
+#include "ccu_gate.h"
+#include "ccu_mux.h"
+
+void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
+					     struct ccu_mux_internal *cm,
+					     int parent_index,
+					     unsigned long *parent_rate)
+{
+	u8 prediv = 1;
+	u32 reg;
+
+	if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
+	      (common->features & CCU_FEATURE_VARIABLE_PREDIV)))
+		return;
+
+	reg = readl(common->base + common->reg);
+	if (parent_index < 0) {
+		parent_index = reg >> cm->shift;
+		parent_index &= (1 << cm->width) - 1;
+	}
+
+	if (common->features & CCU_FEATURE_FIXED_PREDIV)
+		if (parent_index == cm->fixed_prediv.index)
+			prediv = cm->fixed_prediv.div;
+
+	if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
+		if (parent_index == cm->variable_prediv.index) {
+			u8 div;
+
+			div = reg >> cm->variable_prediv.shift;
+			div &= (1 << cm->variable_prediv.width) - 1;
+			prediv = div + 1;
+		}
+
+	*parent_rate = *parent_rate / prediv;
+}
+
+int ccu_mux_helper_determine_rate(struct ccu_common *common,
+				  struct ccu_mux_internal *cm,
+				  struct clk_rate_request *req,
+				  unsigned long (*round)(struct ccu_mux_internal *,
+							 unsigned long,
+							 unsigned long,
+							 void *),
+				  void *data)
+{
+	unsigned long best_parent_rate = 0, best_rate = 0;
+	struct clk_hw *best_parent, *hw = &common->hw;
+	unsigned int i;
+
+	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
+		unsigned long tmp_rate, parent_rate;
+		struct clk_hw *parent;
+
+		parent = clk_hw_get_parent_by_index(hw, i);
+		if (!parent)
+			continue;
+
+		parent_rate = clk_hw_get_rate(parent);
+		ccu_mux_helper_adjust_parent_for_prediv(common, cm, i,
+							&parent_rate);
+
+		tmp_rate = round(cm, clk_hw_get_rate(parent), req->rate, data);
+		if (tmp_rate == req->rate) {
+			best_parent = parent;
+			best_parent_rate = parent_rate;
+			best_rate = tmp_rate;
+			goto out;
+		}
+
+		if ((req->rate - tmp_rate) < (req->rate - best_rate)) {
+			best_rate = tmp_rate;
+			best_parent_rate = parent_rate;
+			best_parent = parent;
+		}
+	}
+
+	if (best_rate == 0)
+		return -EINVAL;
+
+out:
+	req->best_parent_hw = best_parent;
+	req->best_parent_rate = best_parent_rate;
+	req->rate = best_rate;
+	return 0;
+}
+
+u8 ccu_mux_helper_get_parent(struct ccu_common *common,
+			     struct ccu_mux_internal *cm)
+{
+	u32 reg;
+	u8 parent;
+
+	reg = readl(common->base + common->reg);
+	parent = reg >> cm->shift;
+	parent &= (1 << cm->width) - 1;
+
+	return parent;
+}
+
+int ccu_mux_helper_set_parent(struct ccu_common *common,
+			      struct ccu_mux_internal *cm,
+			      u8 index)
+{
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(common->lock, flags);
+
+	reg = readl(common->base + common->reg);
+	reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift);
+	writel(reg | (index << cm->shift), common->base + common->reg);
+
+	spin_unlock_irqrestore(common->lock, flags);
+
+	return 0;
+}
+
+static void ccu_mux_disable(struct clk_hw *hw)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	return ccu_gate_helper_disable(&cm->common, cm->enable);
+}
+
+static int ccu_mux_enable(struct clk_hw *hw)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	return ccu_gate_helper_enable(&cm->common, cm->enable);
+}
+
+static int ccu_mux_is_enabled(struct clk_hw *hw)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
+}
+
+static u8 ccu_mux_get_parent(struct clk_hw *hw)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
+}
+
+static int ccu_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
+}
+
+static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	struct ccu_mux *cm = hw_to_ccu_mux(hw);
+
+	ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
+						&parent_rate);
+
+	return parent_rate;
+}
+
+const struct clk_ops ccu_mux_ops = {
+	.disable	= ccu_mux_disable,
+	.enable		= ccu_mux_enable,
+	.is_enabled	= ccu_mux_is_enabled,
+
+	.get_parent	= ccu_mux_get_parent,
+	.set_parent	= ccu_mux_set_parent,
+
+	.determine_rate	= __clk_mux_determine_rate,
+	.recalc_rate	= ccu_mux_recalc_rate,
+};
diff --git a/drivers/clk/sunxi-ng/ccu_mux.h b/drivers/clk/sunxi-ng/ccu_mux.h
new file mode 100644
index 000000000000..945082631e7d
--- /dev/null
+++ b/drivers/clk/sunxi-ng/ccu_mux.h
@@ -0,0 +1,91 @@
+#ifndef _CCU_MUX_H_
+#define _CCU_MUX_H_
+
+#include <linux/clk-provider.h>
+
+#include "ccu_common.h"
+
+struct ccu_mux_internal {
+	u8	shift;
+	u8	width;
+
+	struct {
+		u8	index;
+		u8	div;
+	} fixed_prediv;
+
+	struct {
+		u8	index;
+		u8	shift;
+		u8	width;
+	} variable_prediv;
+};
+
+#define SUNXI_CLK_MUX(_shift, _width)	\
+	{					\
+		.shift	= _shift,		\
+		.width	= _width,		\
+	}
+
+struct ccu_mux {
+	u16			reg;
+	u32			enable;
+
+	struct ccu_mux_internal	mux;
+	struct ccu_common	common;
+};
+
+#define SUNXI_CCU_MUX(_struct, _name, _parents, _reg, _shift, _width, _flags) \
+	struct ccu_mux _struct = {					\
+		.mux	= SUNXI_CLK_MUX(_shift, _width),		\
+		.common	= {						\
+			.reg		= _reg,				\
+			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
+							      _parents, \
+							      &ccu_mux_ops, \
+							      _flags),	\
+		}							\
+	}
+
+#define SUNXI_CCU_MUX_WITH_GATE(_struct, _name, _parents, _reg,		\
+				_shift, _width, _gate, _flags)		\
+	struct ccu_mux _struct = {					\
+		.enable	= _gate,					\
+		.mux	= SUNXI_CLK_MUX(_shift, _width),		\
+		.common	= {						\
+			.reg		= _reg,				\
+			.hw.init	= CLK_HW_INIT_PARENTS(_name,	\
+							      _parents, \
+							      &ccu_mux_ops, \
+							      _flags),	\
+		}							\
+	}
+
+static inline struct ccu_mux *hw_to_ccu_mux(struct clk_hw *hw)
+{
+	struct ccu_common *common = hw_to_ccu_common(hw);
+
+	return container_of(common, struct ccu_mux, common);
+}
+
+extern const struct clk_ops ccu_mux_ops;
+
+void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
+					     struct ccu_mux_internal *cm,
+					     int parent_index,
+					     unsigned long *parent_rate);
+int ccu_mux_helper_determine_rate(struct ccu_common *common,
+				  struct ccu_mux_internal *cm,
+				  struct clk_rate_request *req,
+				  unsigned long (*round)(struct ccu_mux_internal *,
+							 unsigned long,
+							 unsigned long,
+							 void *),
+				  void *data);
+u8 ccu_mux_helper_get_parent(struct ccu_common *common,
+			     struct ccu_mux_internal *cm);
+int ccu_mux_helper_set_parent(struct ccu_common *common,
+			      struct ccu_mux_internal *cm,
+			      u8 index);
+
+#endif /* _CCU_MUX_H_ */
-- 
2.9.0