1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
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 | |
18 | struct mtk_clk_cpumux { |
19 | struct clk_hw hw; |
20 | struct regmap *regmap; |
21 | u32 reg; |
22 | u32 mask; |
23 | u8 shift; |
24 | }; |
25 | |
26 | static 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 | |
31 | static u8 clk_cpumux_get_parent(struct clk_hw *hw) |
32 | { |
33 | struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw: hw); |
34 | unsigned int val; |
35 | |
36 | regmap_read(map: mux->regmap, reg: mux->reg, val: &val); |
37 | |
38 | val >>= mux->shift; |
39 | val &= mux->mask; |
40 | |
41 | return val; |
42 | } |
43 | |
44 | static int clk_cpumux_set_parent(struct clk_hw *hw, u8 index) |
45 | { |
46 | struct mtk_clk_cpumux *mux = to_mtk_clk_cpumux(hw: hw); |
47 | u32 mask, val; |
48 | |
49 | val = index << mux->shift; |
50 | mask = mux->mask << mux->shift; |
51 | |
52 | return regmap_update_bits(map: mux->regmap, reg: mux->reg, mask, val); |
53 | } |
54 | |
55 | static 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 | |
61 | static struct clk_hw * |
62 | mtk_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(size: sizeof(*cpumux), GFP_KERNEL); |
70 | if (!cpumux) |
71 | return ERR_PTR(error: -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, hw: &cpumux->hw); |
86 | if (ret) { |
87 | kfree(objp: cpumux); |
88 | return ERR_PTR(error: ret); |
89 | } |
90 | |
91 | return &cpumux->hw; |
92 | } |
93 | |
94 | static 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: hw); |
101 | |
102 | clk_hw_unregister(hw); |
103 | kfree(objp: cpumux); |
104 | } |
105 | |
106 | int 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(np: node); |
115 | if (IS_ERR(ptr: regmap)) { |
116 | pr_err("Cannot find regmap for %pOF: %pe\n" , node, regmap); |
117 | return PTR_ERR(ptr: 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(ptr: 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(ptr: 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 | |
141 | err: |
142 | while (--i >= 0) { |
143 | const struct mtk_composite *mux = &clks[i]; |
144 | |
145 | if (IS_ERR_OR_NULL(ptr: clk_data->hws[mux->id])) |
146 | continue; |
147 | |
148 | mtk_clk_unregister_cpumux(hw: clk_data->hws[mux->id]); |
149 | clk_data->hws[mux->id] = ERR_PTR(error: -ENOENT); |
150 | } |
151 | |
152 | return PTR_ERR(ptr: hw); |
153 | } |
154 | EXPORT_SYMBOL_GPL(mtk_clk_register_cpumuxes); |
155 | |
156 | void 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(ptr: clk_data->hws[mux->id])) |
165 | continue; |
166 | |
167 | mtk_clk_unregister_cpumux(hw: clk_data->hws[mux->id]); |
168 | clk_data->hws[mux->id] = ERR_PTR(error: -ENOENT); |
169 | } |
170 | } |
171 | EXPORT_SYMBOL_GPL(mtk_clk_unregister_cpumuxes); |
172 | |
173 | MODULE_LICENSE("GPL" ); |
174 | |