1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | // Copyright (C) 2014 Broadcom Corporation |
3 | |
4 | #include <linux/kernel.h> |
5 | #include <linux/err.h> |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/io.h> |
8 | #include <linux/of.h> |
9 | #include <linux/clkdev.h> |
10 | #include <linux/of_address.h> |
11 | #include <linux/delay.h> |
12 | |
13 | #include "clk-iproc.h" |
14 | |
15 | struct iproc_asiu; |
16 | |
17 | struct iproc_asiu_clk { |
18 | struct clk_hw hw; |
19 | const char *name; |
20 | struct iproc_asiu *asiu; |
21 | unsigned long rate; |
22 | struct iproc_asiu_div div; |
23 | struct iproc_asiu_gate gate; |
24 | }; |
25 | |
26 | struct iproc_asiu { |
27 | void __iomem *div_base; |
28 | void __iomem *gate_base; |
29 | |
30 | struct clk_hw_onecell_data *clk_data; |
31 | struct iproc_asiu_clk *clks; |
32 | }; |
33 | |
34 | #define to_asiu_clk(hw) container_of(hw, struct iproc_asiu_clk, hw) |
35 | |
36 | static int iproc_asiu_clk_enable(struct clk_hw *hw) |
37 | { |
38 | struct iproc_asiu_clk *clk = to_asiu_clk(hw); |
39 | struct iproc_asiu *asiu = clk->asiu; |
40 | u32 val; |
41 | |
42 | /* some clocks at the ASIU level are always enabled */ |
43 | if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET) |
44 | return 0; |
45 | |
46 | val = readl(addr: asiu->gate_base + clk->gate.offset); |
47 | val |= (1 << clk->gate.en_shift); |
48 | writel(val, addr: asiu->gate_base + clk->gate.offset); |
49 | |
50 | return 0; |
51 | } |
52 | |
53 | static void iproc_asiu_clk_disable(struct clk_hw *hw) |
54 | { |
55 | struct iproc_asiu_clk *clk = to_asiu_clk(hw); |
56 | struct iproc_asiu *asiu = clk->asiu; |
57 | u32 val; |
58 | |
59 | /* some clocks at the ASIU level are always enabled */ |
60 | if (clk->gate.offset == IPROC_CLK_INVALID_OFFSET) |
61 | return; |
62 | |
63 | val = readl(addr: asiu->gate_base + clk->gate.offset); |
64 | val &= ~(1 << clk->gate.en_shift); |
65 | writel(val, addr: asiu->gate_base + clk->gate.offset); |
66 | } |
67 | |
68 | static unsigned long iproc_asiu_clk_recalc_rate(struct clk_hw *hw, |
69 | unsigned long parent_rate) |
70 | { |
71 | struct iproc_asiu_clk *clk = to_asiu_clk(hw); |
72 | struct iproc_asiu *asiu = clk->asiu; |
73 | u32 val; |
74 | unsigned int div_h, div_l; |
75 | |
76 | if (parent_rate == 0) { |
77 | clk->rate = 0; |
78 | return 0; |
79 | } |
80 | |
81 | /* if clock divisor is not enabled, simply return parent rate */ |
82 | val = readl(addr: asiu->div_base + clk->div.offset); |
83 | if ((val & (1 << clk->div.en_shift)) == 0) { |
84 | clk->rate = parent_rate; |
85 | return parent_rate; |
86 | } |
87 | |
88 | /* clock rate = parent rate / (high_div + 1) + (low_div + 1) */ |
89 | div_h = (val >> clk->div.high_shift) & bit_mask(clk->div.high_width); |
90 | div_h++; |
91 | div_l = (val >> clk->div.low_shift) & bit_mask(clk->div.low_width); |
92 | div_l++; |
93 | |
94 | clk->rate = parent_rate / (div_h + div_l); |
95 | pr_debug("%s: rate: %lu. parent rate: %lu div_h: %u div_l: %u\n" , |
96 | __func__, clk->rate, parent_rate, div_h, div_l); |
97 | |
98 | return clk->rate; |
99 | } |
100 | |
101 | static long iproc_asiu_clk_round_rate(struct clk_hw *hw, unsigned long rate, |
102 | unsigned long *parent_rate) |
103 | { |
104 | unsigned int div; |
105 | |
106 | if (rate == 0 || *parent_rate == 0) |
107 | return -EINVAL; |
108 | |
109 | if (rate == *parent_rate) |
110 | return *parent_rate; |
111 | |
112 | div = DIV_ROUND_CLOSEST(*parent_rate, rate); |
113 | if (div < 2) |
114 | return *parent_rate; |
115 | |
116 | return *parent_rate / div; |
117 | } |
118 | |
119 | static int iproc_asiu_clk_set_rate(struct clk_hw *hw, unsigned long rate, |
120 | unsigned long parent_rate) |
121 | { |
122 | struct iproc_asiu_clk *clk = to_asiu_clk(hw); |
123 | struct iproc_asiu *asiu = clk->asiu; |
124 | unsigned int div, div_h, div_l; |
125 | u32 val; |
126 | |
127 | if (rate == 0 || parent_rate == 0) |
128 | return -EINVAL; |
129 | |
130 | /* simply disable the divisor if one wants the same rate as parent */ |
131 | if (rate == parent_rate) { |
132 | val = readl(addr: asiu->div_base + clk->div.offset); |
133 | val &= ~(1 << clk->div.en_shift); |
134 | writel(val, addr: asiu->div_base + clk->div.offset); |
135 | return 0; |
136 | } |
137 | |
138 | div = DIV_ROUND_CLOSEST(parent_rate, rate); |
139 | if (div < 2) |
140 | return -EINVAL; |
141 | |
142 | div_h = div_l = div >> 1; |
143 | div_h--; |
144 | div_l--; |
145 | |
146 | val = readl(addr: asiu->div_base + clk->div.offset); |
147 | val |= 1 << clk->div.en_shift; |
148 | if (div_h) { |
149 | val &= ~(bit_mask(clk->div.high_width) |
150 | << clk->div.high_shift); |
151 | val |= div_h << clk->div.high_shift; |
152 | } else { |
153 | val &= ~(bit_mask(clk->div.high_width) |
154 | << clk->div.high_shift); |
155 | } |
156 | if (div_l) { |
157 | val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift); |
158 | val |= div_l << clk->div.low_shift; |
159 | } else { |
160 | val &= ~(bit_mask(clk->div.low_width) << clk->div.low_shift); |
161 | } |
162 | writel(val, addr: asiu->div_base + clk->div.offset); |
163 | |
164 | return 0; |
165 | } |
166 | |
167 | static const struct clk_ops iproc_asiu_ops = { |
168 | .enable = iproc_asiu_clk_enable, |
169 | .disable = iproc_asiu_clk_disable, |
170 | .recalc_rate = iproc_asiu_clk_recalc_rate, |
171 | .round_rate = iproc_asiu_clk_round_rate, |
172 | .set_rate = iproc_asiu_clk_set_rate, |
173 | }; |
174 | |
175 | void __init iproc_asiu_setup(struct device_node *node, |
176 | const struct iproc_asiu_div *div, |
177 | const struct iproc_asiu_gate *gate, |
178 | unsigned int num_clks) |
179 | { |
180 | int i, ret; |
181 | struct iproc_asiu *asiu; |
182 | |
183 | if (WARN_ON(!gate || !div)) |
184 | return; |
185 | |
186 | asiu = kzalloc(size: sizeof(*asiu), GFP_KERNEL); |
187 | if (WARN_ON(!asiu)) |
188 | return; |
189 | |
190 | asiu->clk_data = kzalloc(struct_size(asiu->clk_data, hws, num_clks), |
191 | GFP_KERNEL); |
192 | if (WARN_ON(!asiu->clk_data)) |
193 | goto err_clks; |
194 | asiu->clk_data->num = num_clks; |
195 | |
196 | asiu->clks = kcalloc(n: num_clks, size: sizeof(*asiu->clks), GFP_KERNEL); |
197 | if (WARN_ON(!asiu->clks)) |
198 | goto err_asiu_clks; |
199 | |
200 | asiu->div_base = of_iomap(node, index: 0); |
201 | if (WARN_ON(!asiu->div_base)) |
202 | goto err_iomap_div; |
203 | |
204 | asiu->gate_base = of_iomap(node, index: 1); |
205 | if (WARN_ON(!asiu->gate_base)) |
206 | goto err_iomap_gate; |
207 | |
208 | for (i = 0; i < num_clks; i++) { |
209 | struct clk_init_data init; |
210 | const char *parent_name; |
211 | struct iproc_asiu_clk *asiu_clk; |
212 | const char *clk_name; |
213 | |
214 | ret = of_property_read_string_index(np: node, propname: "clock-output-names" , |
215 | index: i, output: &clk_name); |
216 | if (WARN_ON(ret)) |
217 | goto err_clk_register; |
218 | |
219 | asiu_clk = &asiu->clks[i]; |
220 | asiu_clk->name = clk_name; |
221 | asiu_clk->asiu = asiu; |
222 | asiu_clk->div = div[i]; |
223 | asiu_clk->gate = gate[i]; |
224 | init.name = clk_name; |
225 | init.ops = &iproc_asiu_ops; |
226 | init.flags = 0; |
227 | parent_name = of_clk_get_parent_name(np: node, index: 0); |
228 | init.parent_names = (parent_name ? &parent_name : NULL); |
229 | init.num_parents = (parent_name ? 1 : 0); |
230 | asiu_clk->hw.init = &init; |
231 | |
232 | ret = clk_hw_register(NULL, hw: &asiu_clk->hw); |
233 | if (WARN_ON(ret)) |
234 | goto err_clk_register; |
235 | asiu->clk_data->hws[i] = &asiu_clk->hw; |
236 | } |
237 | |
238 | ret = of_clk_add_hw_provider(np: node, get: of_clk_hw_onecell_get, |
239 | data: asiu->clk_data); |
240 | if (WARN_ON(ret)) |
241 | goto err_clk_register; |
242 | |
243 | return; |
244 | |
245 | err_clk_register: |
246 | while (--i >= 0) |
247 | clk_hw_unregister(hw: asiu->clk_data->hws[i]); |
248 | iounmap(addr: asiu->gate_base); |
249 | |
250 | err_iomap_gate: |
251 | iounmap(addr: asiu->div_base); |
252 | |
253 | err_iomap_div: |
254 | kfree(objp: asiu->clks); |
255 | |
256 | err_asiu_clks: |
257 | kfree(objp: asiu->clk_data); |
258 | |
259 | err_clks: |
260 | kfree(objp: asiu); |
261 | } |
262 | |