1 files changed, 87 insertions, 0 deletions

diff --git a/drivers/clk/mvebu/kirkwood.c b/drivers/clk/mvebu/kirkwood.c
index ddb666a86500..f73a2fa8a561 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"
 
 /*
@@ -225,6 +227,91 @@ 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;
+};
+
+#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 =