11802d0beSThomas Gleixner // SPDX-License-Identifier: GPL-2.0-only
21e17de90SSean Wang /*
31e17de90SSean Wang * Copyright (c) 2015 Linaro Ltd.
41e17de90SSean Wang * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>
51e17de90SSean Wang */
61e17de90SSean Wang
71e17de90SSean Wang #include <linux/clk-provider.h>
802f0d762SChen-Yu Tsai #include <linux/container_of.h>
902f0d762SChen-Yu Tsai #include <linux/err.h>
101e17de90SSean Wang #include <linux/mfd/syscon.h>
1132b028fbSMiles Chen #include <linux/module.h>
1202f0d762SChen-Yu Tsai #include <linux/regmap.h>
131e17de90SSean Wang #include <linux/slab.h>
141e17de90SSean Wang
151e17de90SSean Wang #include "clk-mtk.h"
161e17de90SSean Wang #include "clk-cpumux.h"
171e17de90SSean Wang
1875928442SChen-Yu Tsai struct mtk_clk_cpumux {
1975928442SChen-Yu Tsai struct clk_hw hw;
2075928442SChen-Yu Tsai struct regmap *regmap;
2175928442SChen-Yu Tsai u32 reg;
2275928442SChen-Yu Tsai u32 mask;
2375928442SChen-Yu Tsai u8 shift;
2475928442SChen-Yu Tsai };
2575928442SChen-Yu Tsai
to_mtk_clk_cpumux(struct clk_hw * _hw)261e17de90SSean Wang static inline struct mtk_clk_cpumux *to_mtk_clk_cpumux(struct clk_hw *_hw)
271e17de90SSean Wang {
281e17de90SSean Wang return container_of(_hw, struct mtk_clk_cpumux, hw);
291e17de90SSean Wang }
301e17de90SSean Wang
clk_cpumux_get_parent(struct clk_hw * hw)311e17de90SSean Wang static u8 clk_cpumux_get_parent(struct clk_hw *hw)
321e17de90SSean Wang {
331e17de90SSean Wang struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);
341e17de90SSean Wang unsigned int val;
351e17de90SSean Wang
361e17de90SSean Wang regmap_read(mux->regmap, mux->reg, &val);
371e17de90SSean Wang
381e17de90SSean Wang val >>= mux->shift;
391e17de90SSean Wang val &= mux->mask;
401e17de90SSean Wang
411e17de90SSean Wang return val;
421e17de90SSean Wang }
431e17de90SSean Wang
clk_cpumux_set_parent(struct clk_hw * hw,u8 index)441e17de90SSean Wang static int clk_cpumux_set_parent(struct clk_hw *hw, u8 index)
451e17de90SSean Wang {
461e17de90SSean Wang struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);
471e17de90SSean Wang u32 mask, val;
481e17de90SSean Wang
491e17de90SSean Wang val = index << mux->shift;
501e17de90SSean Wang mask = mux->mask << mux->shift;
511e17de90SSean Wang
521e17de90SSean Wang return regmap_update_bits(mux->regmap, mux->reg, mask, val);
531e17de90SSean Wang }
541e17de90SSean Wang
551e17de90SSean Wang static const struct clk_ops clk_cpumux_ops = {
56*90fe6ebfSMaxime Ripard .determine_rate = clk_hw_determine_rate_no_reparent,
571e17de90SSean Wang .get_parent = clk_cpumux_get_parent,
581e17de90SSean Wang .set_parent = clk_cpumux_set_parent,
591e17de90SSean Wang };
601e17de90SSean Wang
616f691a58SChen-Yu Tsai static struct clk_hw *
mtk_clk_register_cpumux(struct device * dev,const struct mtk_composite * mux,struct regmap * regmap)62f0b3140fSAngeloGioacchino Del Regno mtk_clk_register_cpumux(struct device *dev, const struct mtk_composite *mux,
631e17de90SSean Wang struct regmap *regmap)
641e17de90SSean Wang {
651e17de90SSean Wang struct mtk_clk_cpumux *cpumux;
666f691a58SChen-Yu Tsai int ret;
671e17de90SSean Wang struct clk_init_data init;
681e17de90SSean Wang
691e17de90SSean Wang cpumux = kzalloc(sizeof(*cpumux), GFP_KERNEL);
701e17de90SSean Wang if (!cpumux)
711e17de90SSean Wang return ERR_PTR(-ENOMEM);
721e17de90SSean Wang
731e17de90SSean Wang init.name = mux->name;
741e17de90SSean Wang init.ops = &clk_cpumux_ops;
751e17de90SSean Wang init.parent_names = mux->parent_names;
761e17de90SSean Wang init.num_parents = mux->num_parents;
771e17de90SSean Wang init.flags = mux->flags;
781e17de90SSean Wang
791e17de90SSean Wang cpumux->reg = mux->mux_reg;
801e17de90SSean Wang cpumux->shift = mux->mux_shift;
811e17de90SSean Wang cpumux->mask = BIT(mux->mux_width) - 1;
821e17de90SSean Wang cpumux->regmap = regmap;
831e17de90SSean Wang cpumux->hw.init = &init;
841e17de90SSean Wang
85f0b3140fSAngeloGioacchino Del Regno ret = clk_hw_register(dev, &cpumux->hw);
866f691a58SChen-Yu Tsai if (ret) {
871e17de90SSean Wang kfree(cpumux);
886f691a58SChen-Yu Tsai return ERR_PTR(ret);
891e17de90SSean Wang }
901e17de90SSean Wang
916f691a58SChen-Yu Tsai return &cpumux->hw;
926f691a58SChen-Yu Tsai }
936f691a58SChen-Yu Tsai
mtk_clk_unregister_cpumux(struct clk_hw * hw)946f691a58SChen-Yu Tsai static void mtk_clk_unregister_cpumux(struct clk_hw *hw)
9589ceb206SChen-Yu Tsai {
9689ceb206SChen-Yu Tsai struct mtk_clk_cpumux *cpumux;
9789ceb206SChen-Yu Tsai if (!hw)
9889ceb206SChen-Yu Tsai return;
9989ceb206SChen-Yu Tsai
10089ceb206SChen-Yu Tsai cpumux = to_mtk_clk_cpumux(hw);
10189ceb206SChen-Yu Tsai
1026f691a58SChen-Yu Tsai clk_hw_unregister(hw);
10389ceb206SChen-Yu Tsai kfree(cpumux);
10489ceb206SChen-Yu Tsai }
10589ceb206SChen-Yu Tsai
mtk_clk_register_cpumuxes(struct device * dev,struct device_node * node,const struct mtk_composite * clks,int num,struct clk_hw_onecell_data * clk_data)106f0b3140fSAngeloGioacchino Del Regno int mtk_clk_register_cpumuxes(struct device *dev, struct device_node *node,
1071e17de90SSean Wang const struct mtk_composite *clks, int num,
108609cc5e1SChen-Yu Tsai struct clk_hw_onecell_data *clk_data)
1091e17de90SSean Wang {
1101e17de90SSean Wang int i;
1116f691a58SChen-Yu Tsai struct clk_hw *hw;
1121e17de90SSean Wang struct regmap *regmap;
1131e17de90SSean Wang
114197ee543SChun-Jie Chen regmap = device_node_to_regmap(node);
1151e17de90SSean Wang if (IS_ERR(regmap)) {
1162403d6f1SChen-Yu Tsai pr_err("Cannot find regmap for %pOF: %pe\n", node, regmap);
1171e17de90SSean Wang return PTR_ERR(regmap);
1181e17de90SSean Wang }
1191e17de90SSean Wang
1201e17de90SSean Wang for (i = 0; i < num; i++) {
1211e17de90SSean Wang const struct mtk_composite *mux = &clks[i];
1221e17de90SSean Wang
123609cc5e1SChen-Yu Tsai if (!IS_ERR_OR_NULL(clk_data->hws[mux->id])) {
124d54bb86bSChen-Yu Tsai pr_warn("%pOF: Trying to register duplicate clock ID: %d\n",
125d54bb86bSChen-Yu Tsai node, mux->id);
126d54bb86bSChen-Yu Tsai continue;
127d54bb86bSChen-Yu Tsai }
128d54bb86bSChen-Yu Tsai
129f0b3140fSAngeloGioacchino Del Regno hw = mtk_clk_register_cpumux(dev, mux, regmap);
1306f691a58SChen-Yu Tsai if (IS_ERR(hw)) {
1316f691a58SChen-Yu Tsai pr_err("Failed to register clk %s: %pe\n", mux->name,
1326f691a58SChen-Yu Tsai hw);
1334e94ea54SChen-Yu Tsai goto err;
1341e17de90SSean Wang }
1351e17de90SSean Wang
1366f691a58SChen-Yu Tsai clk_data->hws[mux->id] = hw;
1371e17de90SSean Wang }
1381e17de90SSean Wang
1391e17de90SSean Wang return 0;
1404e94ea54SChen-Yu Tsai
1414e94ea54SChen-Yu Tsai err:
1424e94ea54SChen-Yu Tsai while (--i >= 0) {
1434e94ea54SChen-Yu Tsai const struct mtk_composite *mux = &clks[i];
1444e94ea54SChen-Yu Tsai
145609cc5e1SChen-Yu Tsai if (IS_ERR_OR_NULL(clk_data->hws[mux->id]))
1464e94ea54SChen-Yu Tsai continue;
1474e94ea54SChen-Yu Tsai
1486f691a58SChen-Yu Tsai mtk_clk_unregister_cpumux(clk_data->hws[mux->id]);
149609cc5e1SChen-Yu Tsai clk_data->hws[mux->id] = ERR_PTR(-ENOENT);
1504e94ea54SChen-Yu Tsai }
1514e94ea54SChen-Yu Tsai
1526f691a58SChen-Yu Tsai return PTR_ERR(hw);
1531e17de90SSean Wang }
1547cbe5cb2SAngeloGioacchino Del Regno EXPORT_SYMBOL_GPL(mtk_clk_register_cpumuxes);
15532b028fbSMiles Chen
mtk_clk_unregister_cpumuxes(const struct mtk_composite * clks,int num,struct clk_hw_onecell_data * clk_data)15689ceb206SChen-Yu Tsai void mtk_clk_unregister_cpumuxes(const struct mtk_composite *clks, int num,
157609cc5e1SChen-Yu Tsai struct clk_hw_onecell_data *clk_data)
15889ceb206SChen-Yu Tsai {
15989ceb206SChen-Yu Tsai int i;
16089ceb206SChen-Yu Tsai
16189ceb206SChen-Yu Tsai for (i = num; i > 0; i--) {
16289ceb206SChen-Yu Tsai const struct mtk_composite *mux = &clks[i - 1];
16389ceb206SChen-Yu Tsai
164609cc5e1SChen-Yu Tsai if (IS_ERR_OR_NULL(clk_data->hws[mux->id]))
16589ceb206SChen-Yu Tsai continue;
16689ceb206SChen-Yu Tsai
1676f691a58SChen-Yu Tsai mtk_clk_unregister_cpumux(clk_data->hws[mux->id]);
168609cc5e1SChen-Yu Tsai clk_data->hws[mux->id] = ERR_PTR(-ENOENT);
16989ceb206SChen-Yu Tsai }
17089ceb206SChen-Yu Tsai }
1717cbe5cb2SAngeloGioacchino Del Regno EXPORT_SYMBOL_GPL(mtk_clk_unregister_cpumuxes);
17289ceb206SChen-Yu Tsai
17332b028fbSMiles Chen MODULE_LICENSE("GPL");
174