diff --git a/drivers/clk/imx/Makefile b/drivers/clk/imx/Makefile index 539add92be47..ea8c4bed7709 100644 --- a/drivers/clk/imx/Makefile +++ b/drivers/clk/imx/Makefile @@ -14,6 +14,7 @@ mxc-clk-objs += clk-frac-pll.o mxc-clk-objs += clk-gate.o mxc-clk-objs += clk-gate2.o mxc-clk-objs += clk-gate-exclusive.o +mxc-clk-objs += clk-mux.o mxc-clk-objs += clk-pfd.o mxc-clk-objs += clk-pfdv2.o mxc-clk-objs += clk-pllv1.o diff --git a/drivers/clk/imx/clk-mux.c b/drivers/clk/imx/clk-mux.c new file mode 100644 index 000000000000..a54129756318 --- /dev/null +++ b/drivers/clk/imx/clk-mux.c @@ -0,0 +1,239 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2022 Amarula Solutions + * + * Dario Binacchi + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "clk.h" + +#define to_clk_imx_mux(_hw) container_of(_hw, struct clk_imx_mux, hw) + +struct clk_imx_mux { + struct clk_hw hw; + struct imx_clk_iomap iomap; + u32 mask; + u8 shift; + u8 saved_parent; +}; + +static u8 imx_clk_mux_get_parent(struct clk_hw *hw) +{ + + struct clk_imx_mux *mux = to_clk_imx_mux(hw); + struct imx_clk_iomap *io = &mux->iomap; + int num_parents = clk_hw_get_num_parents(hw); + unsigned int val; + + if (regmap_read(io->regmap, io->offset, &val)) + return -EIO; + + val = (val >> mux->shift) && mux->mask; + + if (val >= num_parents) + return -EINVAL; + + return val; +} + +static int imx_clk_mux_set_parent(struct clk_hw *hw, u8 index) +{ + struct clk_imx_mux *mux = to_clk_imx_mux(hw); + struct imx_clk_iomap *io = &mux->iomap; + unsigned int val; + + if (regmap_read(io->regmap, io->offset, &val)) + return -EIO; + + val &= ~(mux->mask << mux->shift); + val |= index << mux->shift; + return regmap_write(io->regmap, io->offset, val); +} + +/** + * imx_clk_mux_save_context - Save the parent selcted in the mux + * @hw: pointer struct clk_hw + * + * Save the parent mux value. + */ +static int imx_clk_mux_save_context(struct clk_hw *hw) +{ + struct clk_imx_mux *mux = to_clk_imx_mux(hw); + + mux->saved_parent = imx_clk_mux_get_parent(hw); + return 0; +} + +/** + * imx_clk_mux_restore_context - Restore the parent in the mux + * @hw: pointer struct clk_hw + * + * Restore the saved parent mux value. + */ +static void imx_clk_mux_restore_context(struct clk_hw *hw) +{ + struct clk_imx_mux *mux = to_clk_imx_mux(hw); + + imx_clk_mux_set_parent(hw, mux->saved_parent); +} + +const struct clk_ops imx_clk_mux_ops = { + .get_parent = imx_clk_mux_get_parent, + .set_parent = imx_clk_mux_set_parent, + .determine_rate = __clk_mux_determine_rate, + .save_context = imx_clk_mux_save_context, + .restore_context = imx_clk_mux_restore_context, +}; + +static void imx_clk_hw_unregister_mux(struct clk_hw *hw) +{ + struct clk_imx_mux *mux = to_clk_imx_mux(hw); + + clk_hw_unregister(hw); + kfree(mux); +} + +static struct clk_hw *imx_clk_hw_register_mux(struct device *dev, + const char *name, + const char * const *parent_names, + u8 num_parents, + unsigned long flags, + struct imx_clk_iomap *iomap, + u8 shift, u32 mask) +{ + struct clk_init_data init = { NULL }; + struct clk_imx_mux *mux; + struct clk_hw *hw; + + int ret; + + mux = kzalloc(sizeof(*mux), GFP_KERNEL); + if (!mux) + return ERR_PTR(-ENOMEM); + + init.name = name; + init.flags = flags; + init.ops = &imx_clk_mux_ops; + init.parent_names = parent_names; + init.num_parents = 1; + + /* struct clk_mux assignments */ + memcpy(&mux->iomap, iomap, sizeof(*iomap)); + mux->hw.init = &init; + + hw = &mux->hw; + ret = clk_hw_register(dev, hw); + if (ret) { + kfree(mux); + return ERR_PTR(ret); + } + + return hw; +} + +static struct clk_hw *_of_imx_mux_clk_setup(struct device_node *node) +{ + struct clk_hw *hw; + struct device_node *parent_node; + unsigned int num_parents; + const char **parent_names; + const char *name; + struct imx_clk_iomap io; + u32 shift = 0; + u32 flags = CLK_SET_RATE_NO_REPARENT; + u32 val; + u32 mask; + int ret; + + parent_node = of_get_parent(node); + if (!parent_node) { + pr_err("%s: %pOFn must have 1 parent\n", __func__, node); + return ERR_PTR(-ENODEV); + } + + num_parents = of_clk_get_parent_count(node); + if (num_parents < 2) { + pr_err("%s: %pOFn must have parents\n", __func__, node); + return ERR_PTR(-ENODEV); + } + + parent_names = kzalloc((sizeof(char *) * num_parents), GFP_KERNEL); + if (!parent_names) + return ERR_PTR(-ENOMEM); + + of_clk_parent_fill(node, parent_names, num_parents); + io.regmap = syscon_node_to_regmap(parent_node); + of_node_put(parent_node); + if (IS_ERR(io.regmap)) { + pr_err("%s: missing regmap for %pOFn\n", __func__, node); + ret = PTR_ERR(io.regmap); + goto free_parent_names; + } + + if (of_property_read_u32(node, "fsl,regmap-offset", &val)) { + pr_err("%s: missing regmap offset for %pOFn\n", __func__, + node); + ret = -EIO; + goto free_parent_names; + } + + io.offset = val; + + of_property_read_u32(node, "fsl,bit-shift", &shift); + + if (of_property_read_bool(node, "fsl,set-rate-parent")) + flags |= CLK_SET_RATE_PARENT; + + if (of_property_read_bool(node, "fsl,ops-parent-enable")) + flags |= CLK_OPS_PARENT_ENABLE; + + /* Generate bit-mask based on parent info */ + mask = num_parents - 1; + mask = (1 << fls(mask)) - 1; + + name = imx_dt_clk_name(node); + hw = imx_clk_hw_register_mux(NULL, name, parent_names, num_parents, + flags, &io, shift, mask); + if (IS_ERR(hw)) { + /* + * Clear OF_POPULATED flag so that clock registration can be + * attempted again from probe function. + */ + of_node_clear_flag(node, OF_POPULATED); + ret = PTR_ERR(hw); + goto free_parent_names; + } + + ret = of_clk_add_hw_provider(node, of_clk_hw_simple_get, hw); + if (ret) { + imx_clk_hw_unregister_mux(hw); + goto free_parent_names; + } + + pr_debug("%s: name: %s, offset: 0x%x, shift: %d, mask: 0x%x, ret: %d\n", + __func__, name, io.offset, shift, mask, ret); + +free_parent_names: + kfree(parent_names); + return ret ? ERR_PTR(ret) : hw; +} + +/** + * of_imx_mux_clk_setup() - Setup function for imx mux clock + * @node: device node for the clock + */ +void __init of_imx_mux_clk_setup(struct device_node *node) +{ + _of_imx_mux_clk_setup(node); +} +CLK_OF_DECLARE(fsl_imx8mn_mux_clk, "fsl,imx8mn-mux-clock", + of_imx_mux_clk_setup);