diff options
Diffstat (limited to 'drivers')
-rw-r--r-- | drivers/clk/mvebu/kirkwood.c | 87 |
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 = |