3 files changed, 108 insertions, 5 deletions

diff --git a/drivers/clk/mvebu/common.c b/drivers/clk/mvebu/common.c
index f902c18f4552..b7fcb469c87a 100644
--- a/drivers/clk/mvebu/common.c
+++ b/drivers/clk/mvebu/common.c
@@ -171,8 +171,10 @@ void __init mvebu_coreclk_setup(struct device_node *np,
  * Clock Gating Control
  */
 
+DEFINE_SPINLOCK(ctrl_gating_lock);
+
 struct clk_gating_ctrl {
-	spinlock_t lock;
+	spinlock_t *lock;
 	struct clk **gates;
 	int num_gates;
 };
@@ -220,7 +222,8 @@ void __init mvebu_clk_gating_setup(struct device_node *np,
 	if (WARN_ON(!ctrl))
 		goto ctrl_out;
 
-	spin_lock_init(&ctrl->lock);
+	/* lock must already be initialized */
+	ctrl->lock = &ctrl_gating_lock;
 
 	/* Count, allocate, and register clock gates */
 	for (n = 0; desc[n].name;)
@@ -237,7 +240,7 @@ void __init mvebu_clk_gating_setup(struct device_node *np,
 			(desc[n].parent) ? desc[n].parent : default_parent;
 		ctrl->gates[n] = clk_register_gate(NULL, desc[n].name, parent,
 					desc[n].flags, base, desc[n].bit_idx,
-					0, &ctrl->lock);
+					0, ctrl->lock);
 		WARN_ON(IS_ERR(ctrl->gates[n]));
 	}
 
diff --git a/drivers/clk/mvebu/common.h b/drivers/clk/mvebu/common.h
index 989ab1464617..783b5631a453 100644
--- a/drivers/clk/mvebu/common.h
+++ b/drivers/clk/mvebu/common.h
@@ -17,6 +17,8 @@
 
 #include <linux/kernel.h>
 
+extern spinlock_t ctrl_gating_lock;
+
 struct device_node;
 
 struct coreclk_ratio {
diff --git a/drivers/clk/mvebu/kirkwood.c b/drivers/clk/mvebu/kirkwood.c
index ddb666a86500..99550f25975e 100644
--- a/drivers/clk/mvebu/kirkwood.c
+++ b/drivers/clk/mvebu/kirkwood.c
@@ -13,9 +13,11 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/slab.h>
 #include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include "common.h"
 
 /*
@@ -214,7 +216,6 @@ static const struct clk_gating_soc_desc kirkwood_gating_desc[] __initconst = {
 	{ "runit", NULL, 7, 0 },
 	{ "xor0", NULL, 8, 0 },
 	{ "audio", NULL, 9, 0 },
-	{ "powersave", "cpuclk", 11, 0 },
 	{ "sata0", NULL, 14, 0 },
 	{ "sata1", NULL, 15, 0 },
 	{ "xor1", NULL, 16, 0 },
@@ -225,6 +226,101 @@ static const struct clk_gating_soc_desc kirkwood_gating_desc[] __initconst = {
 	{ }
 };
 
+
+/*
+ * Clock Muxing Control
+ */
+
+struct clk_muxing_soc_desc {
+	const char *name;
+	const char **parents;
+	int num_parents;
+	int shift;
+	int width;
+	unsigned long flags;
+};
+
+struct clk_muxing_ctrl {
+	spinlock_t *lock;
+	struct clk **muxes;
+	int num_muxes;
+};
+
+static const char *powersave_parents[] = {
+	"cpuclk",
+	"ddrclk",
+};
+
+static const struct clk_muxing_soc_desc kirkwood_mux_desc[] __initconst = {
+	{ "powersave", powersave_parents, ARRAY_SIZE(powersave_parents),
+		11, 1, 0 },
+};
+
+#define to_clk_mux(_hw) container_of(_hw, struct clk_mux, hw)
+
+static struct clk *clk_muxing_get_src(
+	struct of_phandle_args *clkspec, void *data)
+{
+	struct clk_muxing_ctrl *ctrl = (struct clk_muxing_ctrl *)data;
+	int n;
+
+	if (clkspec->args_count < 1)
+		return ERR_PTR(-EINVAL);
+
+	for (n = 0; n < ctrl->num_muxes; n++) {
+		struct clk_mux *mux =
+			to_clk_mux(__clk_get_hw(ctrl->muxes[n]));
+		if (clkspec->args[0] == mux->shift)
+			return ctrl->muxes[n];
+	}
+	return ERR_PTR(-ENODEV);
+}
+
+static void __init kirkwood_clk_muxing_setup(struct device_node *np,
+				   const struct clk_muxing_soc_desc *desc)
+{
+	struct clk_muxing_ctrl *ctrl;
+	void __iomem *base;
+	int n;
+
+	base = of_iomap(np, 0);
+	if (WARN_ON(!base))
+		return;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (WARN_ON(!ctrl))
+		goto ctrl_out;
+
+	/* lock must already be initialized */
+	ctrl->lock = &ctrl_gating_lock;
+
+	/* Count, allocate, and register clock muxes */
+	for (n = 0; desc[n].name;)
+		n++;
+
+	ctrl->num_muxes = n;
+	ctrl->muxes = kcalloc(ctrl->num_muxes, sizeof(struct clk *),
+			GFP_KERNEL);
+	if (WARN_ON(!ctrl->muxes))
+		goto muxes_out;
+
+	for (n = 0; n < ctrl->num_muxes; n++) {
+		ctrl->muxes[n] = clk_register_mux(NULL, desc[n].name,
+				desc[n].parents, desc[n].num_parents,
+				desc[n].flags, base, desc[n].shift,
+				desc[n].width, desc[n].flags, ctrl->lock);
+		WARN_ON(IS_ERR(ctrl->muxes[n]));
+	}
+
+	of_clk_add_provider(np, clk_muxing_get_src, ctrl);
+
+	return;
+muxes_out:
+	kfree(ctrl);
+ctrl_out:
+	iounmap(base);
+}
+
 static void __init kirkwood_clk_init(struct device_node *np)
 {
 	struct device_node *cgnp =
@@ -236,8 +332,10 @@ static void __init kirkwood_clk_init(struct device_node *np)
 	else
 		mvebu_coreclk_setup(np, &kirkwood_coreclks);
 
-	if (cgnp)
+	if (cgnp) {
 		mvebu_clk_gating_setup(cgnp, kirkwood_gating_desc);
+		kirkwood_clk_muxing_setup(cgnp, kirkwood_mux_desc);
+	}
 }
 CLK_OF_DECLARE(kirkwood_clk, "marvell,kirkwood-core-clock",
 	       kirkwood_clk_init);