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 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 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 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 = { 561e17de90SSean Wang .get_parent = clk_cpumux_get_parent, 571e17de90SSean Wang .set_parent = clk_cpumux_set_parent, 581e17de90SSean Wang }; 591e17de90SSean Wang 6028f1186aSStephen Boyd static struct clk * 611e17de90SSean Wang mtk_clk_register_cpumux(const struct mtk_composite *mux, 621e17de90SSean Wang struct regmap *regmap) 631e17de90SSean Wang { 641e17de90SSean Wang struct mtk_clk_cpumux *cpumux; 651e17de90SSean Wang struct clk *clk; 661e17de90SSean Wang struct clk_init_data init; 671e17de90SSean Wang 681e17de90SSean Wang cpumux = kzalloc(sizeof(*cpumux), GFP_KERNEL); 691e17de90SSean Wang if (!cpumux) 701e17de90SSean Wang return ERR_PTR(-ENOMEM); 711e17de90SSean Wang 721e17de90SSean Wang init.name = mux->name; 731e17de90SSean Wang init.ops = &clk_cpumux_ops; 741e17de90SSean Wang init.parent_names = mux->parent_names; 751e17de90SSean Wang init.num_parents = mux->num_parents; 761e17de90SSean Wang init.flags = mux->flags; 771e17de90SSean Wang 781e17de90SSean Wang cpumux->reg = mux->mux_reg; 791e17de90SSean Wang cpumux->shift = mux->mux_shift; 801e17de90SSean Wang cpumux->mask = BIT(mux->mux_width) - 1; 811e17de90SSean Wang cpumux->regmap = regmap; 821e17de90SSean Wang cpumux->hw.init = &init; 831e17de90SSean Wang 841e17de90SSean Wang clk = clk_register(NULL, &cpumux->hw); 851e17de90SSean Wang if (IS_ERR(clk)) 861e17de90SSean Wang kfree(cpumux); 871e17de90SSean Wang 881e17de90SSean Wang return clk; 891e17de90SSean Wang } 901e17de90SSean Wang 9189ceb206SChen-Yu Tsai static void mtk_clk_unregister_cpumux(struct clk *clk) 9289ceb206SChen-Yu Tsai { 9389ceb206SChen-Yu Tsai struct mtk_clk_cpumux *cpumux; 9489ceb206SChen-Yu Tsai struct clk_hw *hw; 9589ceb206SChen-Yu Tsai 9689ceb206SChen-Yu Tsai hw = __clk_get_hw(clk); 9789ceb206SChen-Yu Tsai if (!hw) 9889ceb206SChen-Yu Tsai return; 9989ceb206SChen-Yu Tsai 10089ceb206SChen-Yu Tsai cpumux = to_mtk_clk_cpumux(hw); 10189ceb206SChen-Yu Tsai 10289ceb206SChen-Yu Tsai clk_unregister(clk); 10389ceb206SChen-Yu Tsai kfree(cpumux); 10489ceb206SChen-Yu Tsai } 10589ceb206SChen-Yu Tsai 10628f1186aSStephen Boyd int mtk_clk_register_cpumuxes(struct device_node *node, 1071e17de90SSean Wang const struct mtk_composite *clks, int num, 1081e17de90SSean Wang struct clk_onecell_data *clk_data) 1091e17de90SSean Wang { 1101e17de90SSean Wang int i; 1111e17de90SSean Wang struct clk *clk; 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 1231e17de90SSean Wang clk = mtk_clk_register_cpumux(mux, regmap); 1241e17de90SSean Wang if (IS_ERR(clk)) { 1252403d6f1SChen-Yu Tsai pr_err("Failed to register clk %s: %pe\n", mux->name, clk); 126*4e94ea54SChen-Yu Tsai goto err; 1271e17de90SSean Wang } 1281e17de90SSean Wang 1291e17de90SSean Wang clk_data->clks[mux->id] = clk; 1301e17de90SSean Wang } 1311e17de90SSean Wang 1321e17de90SSean Wang return 0; 133*4e94ea54SChen-Yu Tsai 134*4e94ea54SChen-Yu Tsai err: 135*4e94ea54SChen-Yu Tsai while (--i >= 0) { 136*4e94ea54SChen-Yu Tsai const struct mtk_composite *mux = &clks[i]; 137*4e94ea54SChen-Yu Tsai 138*4e94ea54SChen-Yu Tsai if (IS_ERR_OR_NULL(clk_data->clks[mux->id])) 139*4e94ea54SChen-Yu Tsai continue; 140*4e94ea54SChen-Yu Tsai 141*4e94ea54SChen-Yu Tsai mtk_clk_unregister_cpumux(clk_data->clks[mux->id]); 142*4e94ea54SChen-Yu Tsai clk_data->clks[mux->id] = ERR_PTR(-ENOENT); 143*4e94ea54SChen-Yu Tsai } 144*4e94ea54SChen-Yu Tsai 145*4e94ea54SChen-Yu Tsai return PTR_ERR(clk); 1461e17de90SSean Wang } 14732b028fbSMiles Chen 14889ceb206SChen-Yu Tsai void mtk_clk_unregister_cpumuxes(const struct mtk_composite *clks, int num, 14989ceb206SChen-Yu Tsai struct clk_onecell_data *clk_data) 15089ceb206SChen-Yu Tsai { 15189ceb206SChen-Yu Tsai int i; 15289ceb206SChen-Yu Tsai 15389ceb206SChen-Yu Tsai for (i = num; i > 0; i--) { 15489ceb206SChen-Yu Tsai const struct mtk_composite *mux = &clks[i - 1]; 15589ceb206SChen-Yu Tsai 15689ceb206SChen-Yu Tsai if (IS_ERR_OR_NULL(clk_data->clks[mux->id])) 15789ceb206SChen-Yu Tsai continue; 15889ceb206SChen-Yu Tsai 15989ceb206SChen-Yu Tsai mtk_clk_unregister_cpumux(clk_data->clks[mux->id]); 16089ceb206SChen-Yu Tsai clk_data->clks[mux->id] = ERR_PTR(-ENOENT); 16189ceb206SChen-Yu Tsai } 16289ceb206SChen-Yu Tsai } 16389ceb206SChen-Yu Tsai 16432b028fbSMiles Chen MODULE_LICENSE("GPL"); 165