174 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * Copyright (c) 2015 Linaro Ltd.4 * Author: Pi-Cheng Chen <pi-cheng.chen@linaro.org>5 */6 7#include <linux/clk-provider.h>8#include <linux/container_of.h>9#include <linux/err.h>10#include <linux/mfd/syscon.h>11#include <linux/module.h>12#include <linux/regmap.h>13#include <linux/slab.h>14 15#include "clk-mtk.h"16#include "clk-cpumux.h"17 18struct mtk_clk_cpumux {19 struct clk_hw hw;20 struct regmap *regmap;21 u32 reg;22 u32 mask;23 u8 shift;24};25 26static inline struct mtk_clk_cpumux *to_mtk_clk_cpumux(struct clk_hw *_hw)27{28 return container_of(_hw, struct mtk_clk_cpumux, hw);29}30 31static u8 clk_cpumux_get_parent(struct clk_hw *hw)32{33 struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);34 unsigned int val;35 36 regmap_read(mux->regmap, mux->reg, &val);37 38 val >>= mux->shift;39 val &= mux->mask;40 41 return val;42}43 44static int clk_cpumux_set_parent(struct clk_hw *hw, u8 index)45{46 struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw);47 u32 mask, val;48 49 val = index << mux->shift;50 mask = mux->mask << mux->shift;51 52 return regmap_update_bits(mux->regmap, mux->reg, mask, val);53}54 55static const struct clk_ops clk_cpumux_ops = {56 .determine_rate = clk_hw_determine_rate_no_reparent,57 .get_parent = clk_cpumux_get_parent,58 .set_parent = clk_cpumux_set_parent,59};60 61static struct clk_hw *62mtk_clk_register_cpumux(struct device *dev, const struct mtk_composite *mux,63 struct regmap *regmap)64{65 struct mtk_clk_cpumux *cpumux;66 int ret;67 struct clk_init_data init;68 69 cpumux = kzalloc(sizeof(*cpumux), GFP_KERNEL);70 if (!cpumux)71 return ERR_PTR(-ENOMEM);72 73 init.name = mux->name;74 init.ops = &clk_cpumux_ops;75 init.parent_names = mux->parent_names;76 init.num_parents = mux->num_parents;77 init.flags = mux->flags;78 79 cpumux->reg = mux->mux_reg;80 cpumux->shift = mux->mux_shift;81 cpumux->mask = BIT(mux->mux_width) - 1;82 cpumux->regmap = regmap;83 cpumux->hw.init = &init;84 85 ret = clk_hw_register(dev, &cpumux->hw);86 if (ret) {87 kfree(cpumux);88 return ERR_PTR(ret);89 }90 91 return &cpumux->hw;92}93 94static void mtk_clk_unregister_cpumux(struct clk_hw *hw)95{96 struct mtk_clk_cpumux *cpumux;97 if (!hw)98 return;99 100 cpumux = to_mtk_clk_cpumux(hw);101 102 clk_hw_unregister(hw);103 kfree(cpumux);104}105 106int mtk_clk_register_cpumuxes(struct device *dev, struct device_node *node,107 const struct mtk_composite *clks, int num,108 struct clk_hw_onecell_data *clk_data)109{110 int i;111 struct clk_hw *hw;112 struct regmap *regmap;113 114 regmap = device_node_to_regmap(node);115 if (IS_ERR(regmap)) {116 pr_err("Cannot find regmap for %pOF: %pe\n", node, regmap);117 return PTR_ERR(regmap);118 }119 120 for (i = 0; i < num; i++) {121 const struct mtk_composite *mux = &clks[i];122 123 if (!IS_ERR_OR_NULL(clk_data->hws[mux->id])) {124 pr_warn("%pOF: Trying to register duplicate clock ID: %d\n",125 node, mux->id);126 continue;127 }128 129 hw = mtk_clk_register_cpumux(dev, mux, regmap);130 if (IS_ERR(hw)) {131 pr_err("Failed to register clk %s: %pe\n", mux->name,132 hw);133 goto err;134 }135 136 clk_data->hws[mux->id] = hw;137 }138 139 return 0;140 141err:142 while (--i >= 0) {143 const struct mtk_composite *mux = &clks[i];144 145 if (IS_ERR_OR_NULL(clk_data->hws[mux->id]))146 continue;147 148 mtk_clk_unregister_cpumux(clk_data->hws[mux->id]);149 clk_data->hws[mux->id] = ERR_PTR(-ENOENT);150 }151 152 return PTR_ERR(hw);153}154EXPORT_SYMBOL_GPL(mtk_clk_register_cpumuxes);155 156void mtk_clk_unregister_cpumuxes(const struct mtk_composite *clks, int num,157 struct clk_hw_onecell_data *clk_data)158{159 int i;160 161 for (i = num; i > 0; i--) {162 const struct mtk_composite *mux = &clks[i - 1];163 164 if (IS_ERR_OR_NULL(clk_data->hws[mux->id]))165 continue;166 167 mtk_clk_unregister_cpumux(clk_data->hws[mux->id]);168 clk_data->hws[mux->id] = ERR_PTR(-ENOENT);169 }170}171EXPORT_SYMBOL_GPL(mtk_clk_unregister_cpumuxes);172 173MODULE_LICENSE("GPL");174