1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2018 Emmanuel Vadot <manu@freebsd.org>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
16 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
17 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
18 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
19 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
20 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
22 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
23 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/cdefs.h>
29 #include <sys/param.h>
30 #include <sys/systm.h>
31 #include <sys/bus.h>
32
33 #include <dev/extres/clk/clk.h>
34 #include <dev/extres/syscon/syscon.h>
35
36 #include <arm64/rockchip/clk/rk_clk_composite.h>
37
38 #include "clkdev_if.h"
39 #include "syscon_if.h"
40
41 struct rk_clk_composite_sc {
42 uint32_t muxdiv_offset;
43 uint32_t mux_shift;
44 uint32_t mux_width;
45 uint32_t mux_mask;
46
47 uint32_t div_shift;
48 uint32_t div_width;
49 uint32_t div_mask;
50
51 uint32_t gate_offset;
52 uint32_t gate_shift;
53
54 uint32_t flags;
55
56 struct syscon *grf;
57 };
58
59 #define WRITE4(_clk, off, val) \
60 rk_clk_composite_write_4(_clk, off, val)
61 #define READ4(_clk, off, val) \
62 rk_clk_composite_read_4(_clk, off, val)
63 #define DEVICE_LOCK(_clk) \
64 CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
65 #define DEVICE_UNLOCK(_clk) \
66 CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
67
68 #define RK_CLK_COMPOSITE_MASK_SHIFT 16
69
70 #if 0
71 #define dprintf(format, arg...) \
72 printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg)
73 #else
74 #define dprintf(format, arg...)
75 #endif
76
77 static void
rk_clk_composite_read_4(struct clknode * clk,bus_addr_t addr,uint32_t * val)78 rk_clk_composite_read_4(struct clknode *clk, bus_addr_t addr, uint32_t *val)
79 {
80 struct rk_clk_composite_sc *sc;
81
82 sc = clknode_get_softc(clk);
83 if (sc->grf)
84 *val = SYSCON_READ_4(sc->grf, addr);
85 else
86 CLKDEV_READ_4(clknode_get_device(clk), addr, val);
87 }
88
89 static void
rk_clk_composite_write_4(struct clknode * clk,bus_addr_t addr,uint32_t val)90 rk_clk_composite_write_4(struct clknode *clk, bus_addr_t addr, uint32_t val)
91 {
92 struct rk_clk_composite_sc *sc;
93
94 sc = clknode_get_softc(clk);
95 if (sc->grf)
96 SYSCON_WRITE_4(sc->grf, addr, val | (0xffff << 16));
97 else
98 CLKDEV_WRITE_4(clknode_get_device(clk), addr, val);
99 }
100
101 static struct syscon *
rk_clk_composite_get_grf(struct clknode * clk)102 rk_clk_composite_get_grf(struct clknode *clk)
103 {
104 device_t dev;
105 phandle_t node;
106 struct syscon *grf;
107
108 grf = NULL;
109 dev = clknode_get_device(clk);
110 node = ofw_bus_get_node(dev);
111 if (OF_hasprop(node, "rockchip,grf") &&
112 syscon_get_by_ofw_property(dev, node,
113 "rockchip,grf", &grf) != 0) {
114 return (NULL);
115 }
116
117 return (grf);
118 }
119
120 static int
rk_clk_composite_init(struct clknode * clk,device_t dev)121 rk_clk_composite_init(struct clknode *clk, device_t dev)
122 {
123 struct rk_clk_composite_sc *sc;
124 uint32_t val, idx;
125
126 sc = clknode_get_softc(clk);
127 if ((sc->flags & RK_CLK_COMPOSITE_GRF) != 0) {
128 sc->grf = rk_clk_composite_get_grf(clk);
129 if (sc->grf == NULL)
130 panic("clock %s has GRF flag set but no syscon is available",
131 clknode_get_name(clk));
132 }
133
134 idx = 0;
135 if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) != 0) {
136 DEVICE_LOCK(clk);
137 READ4(clk, sc->muxdiv_offset, &val);
138 DEVICE_UNLOCK(clk);
139
140 idx = (val & sc->mux_mask) >> sc->mux_shift;
141 }
142
143 clknode_init_parent_idx(clk, idx);
144
145 return (0);
146 }
147
148 static int
rk_clk_composite_set_gate(struct clknode * clk,bool enable)149 rk_clk_composite_set_gate(struct clknode *clk, bool enable)
150 {
151 struct rk_clk_composite_sc *sc;
152 uint32_t val = 0;
153
154 sc = clknode_get_softc(clk);
155
156 if ((sc->flags & RK_CLK_COMPOSITE_HAVE_GATE) == 0)
157 return (0);
158
159 dprintf("%sabling gate\n", enable ? "En" : "Dis");
160 if (!enable)
161 val |= 1 << sc->gate_shift;
162 dprintf("sc->gate_shift: %x\n", sc->gate_shift);
163 val |= (1 << sc->gate_shift) << RK_CLK_COMPOSITE_MASK_SHIFT;
164 dprintf("Write: gate_offset=%x, val=%x\n", sc->gate_offset, val);
165 DEVICE_LOCK(clk);
166 WRITE4(clk, sc->gate_offset, val);
167 DEVICE_UNLOCK(clk);
168
169 return (0);
170 }
171
172 static int
rk_clk_composite_set_mux(struct clknode * clk,int index)173 rk_clk_composite_set_mux(struct clknode *clk, int index)
174 {
175 struct rk_clk_composite_sc *sc;
176 uint32_t val = 0;
177
178 sc = clknode_get_softc(clk);
179
180 if ((sc->flags & RK_CLK_COMPOSITE_HAVE_MUX) == 0)
181 return (0);
182
183 dprintf("Set mux to %d\n", index);
184 DEVICE_LOCK(clk);
185 val |= (index << sc->mux_shift);
186 val |= sc->mux_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
187 dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
188 WRITE4(clk, sc->muxdiv_offset, val);
189 DEVICE_UNLOCK(clk);
190
191 return (0);
192 }
193
194 static int
rk_clk_composite_recalc(struct clknode * clk,uint64_t * freq)195 rk_clk_composite_recalc(struct clknode *clk, uint64_t *freq)
196 {
197 struct rk_clk_composite_sc *sc;
198 uint32_t reg, div;
199
200 sc = clknode_get_softc(clk);
201
202 DEVICE_LOCK(clk);
203
204 READ4(clk, sc->muxdiv_offset, ®);
205 dprintf("Read: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, reg);
206
207 DEVICE_UNLOCK(clk);
208
209 div = ((reg & sc->div_mask) >> sc->div_shift);
210 if (sc->flags & RK_CLK_COMPOSITE_DIV_EXP)
211 div = 1 << div;
212 else
213 div += 1;
214 dprintf("parent_freq=%ju, div=%u\n", *freq, div);
215 *freq = *freq / div;
216 dprintf("Final freq=%ju\n", *freq);
217 return (0);
218 }
219
220 static uint32_t
rk_clk_composite_find_best(struct rk_clk_composite_sc * sc,uint64_t fparent,uint64_t freq,uint32_t * reg)221 rk_clk_composite_find_best(struct rk_clk_composite_sc *sc, uint64_t fparent,
222 uint64_t freq, uint32_t *reg)
223 {
224 uint64_t best, cur;
225 uint32_t best_div, best_div_reg;
226 uint32_t div, div_reg;
227
228 best = 0;
229 best_div = 0;
230 best_div_reg = 0;
231
232 for (div_reg = 0; div_reg <= ((sc->div_mask >> sc->div_shift) + 1);
233 div_reg++) {
234 if (sc->flags == RK_CLK_COMPOSITE_DIV_EXP)
235 div = 1 << div_reg;
236 else
237 div = div_reg + 1;
238 cur = fparent / div;
239 if ((freq - cur) < (freq - best)) {
240 best = cur;
241 best_div = div;
242 best_div_reg = div_reg;
243 break;
244 }
245 }
246 *reg = div_reg;
247 return (best_div);
248 }
249
250 static int
rk_clk_composite_set_freq(struct clknode * clk,uint64_t fparent,uint64_t * fout,int flags,int * stop)251 rk_clk_composite_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
252 int flags, int *stop)
253 {
254 struct rk_clk_composite_sc *sc;
255 struct clknode *p_clk;
256 const char **p_names;
257 uint64_t best, cur;
258 uint32_t div, div_reg, best_div, best_div_reg, val;
259 int p_idx, best_parent;
260
261 sc = clknode_get_softc(clk);
262 dprintf("Finding best parent/div for target freq of %ju\n", *fout);
263 p_names = clknode_get_parent_names(clk);
264 for (best_div = 0, best = 0, p_idx = 0;
265 p_idx != clknode_get_parents_num(clk); p_idx++) {
266 p_clk = clknode_find_by_name(p_names[p_idx]);
267 clknode_get_freq(p_clk, &fparent);
268 dprintf("Testing with parent %s (%d) at freq %ju\n",
269 clknode_get_name(p_clk), p_idx, fparent);
270 div = rk_clk_composite_find_best(sc, fparent, *fout, &div_reg);
271 cur = fparent / div;
272 if ((*fout - cur) < (*fout - best)) {
273 best = cur;
274 best_div = div;
275 best_div_reg = div_reg;
276 best_parent = p_idx;
277 dprintf("Best parent so far %s (%d) with best freq at "
278 "%ju\n", clknode_get_name(p_clk), p_idx, best);
279 }
280 }
281
282 *stop = 1;
283 if (best_div == 0)
284 return (ERANGE);
285
286 if ((best < *fout) && ((flags & CLK_SET_ROUND_DOWN) == 0))
287 return (ERANGE);
288
289 if ((best > *fout) && ((flags & CLK_SET_ROUND_UP) == 0)) {
290 return (ERANGE);
291 }
292
293 if ((flags & CLK_SET_DRYRUN) != 0) {
294 *fout = best;
295 return (0);
296 }
297
298 p_idx = clknode_get_parent_idx(clk);
299 if (p_idx != best_parent) {
300 dprintf("Switching parent index from %d to %d\n", p_idx,
301 best_parent);
302 clknode_set_parent_by_idx(clk, best_parent);
303 }
304
305 dprintf("Setting divider to %d (reg: %d)\n", best_div, best_div_reg);
306 dprintf(" div_mask: 0x%X, div_shift: %d\n", sc->div_mask,
307 sc->div_shift);
308
309 DEVICE_LOCK(clk);
310 val = best_div_reg << sc->div_shift;
311 val |= sc->div_mask << RK_CLK_COMPOSITE_MASK_SHIFT;
312 dprintf("Write: muxdiv_offset=%x, val=%x\n", sc->muxdiv_offset, val);
313 WRITE4(clk, sc->muxdiv_offset, val);
314 DEVICE_UNLOCK(clk);
315
316 *fout = best;
317 return (0);
318 }
319
320 static clknode_method_t rk_clk_composite_clknode_methods[] = {
321 /* Device interface */
322 CLKNODEMETHOD(clknode_init, rk_clk_composite_init),
323 CLKNODEMETHOD(clknode_set_gate, rk_clk_composite_set_gate),
324 CLKNODEMETHOD(clknode_set_mux, rk_clk_composite_set_mux),
325 CLKNODEMETHOD(clknode_recalc_freq, rk_clk_composite_recalc),
326 CLKNODEMETHOD(clknode_set_freq, rk_clk_composite_set_freq),
327 CLKNODEMETHOD_END
328 };
329
330 DEFINE_CLASS_1(rk_clk_composite_clknode, rk_clk_composite_clknode_class,
331 rk_clk_composite_clknode_methods, sizeof(struct rk_clk_composite_sc),
332 clknode_class);
333
334 int
rk_clk_composite_register(struct clkdom * clkdom,struct rk_clk_composite_def * clkdef)335 rk_clk_composite_register(struct clkdom *clkdom,
336 struct rk_clk_composite_def *clkdef)
337 {
338 struct clknode *clk;
339 struct rk_clk_composite_sc *sc;
340
341 clk = clknode_create(clkdom, &rk_clk_composite_clknode_class,
342 &clkdef->clkdef);
343 if (clk == NULL)
344 return (1);
345
346 sc = clknode_get_softc(clk);
347
348 sc->muxdiv_offset = clkdef->muxdiv_offset;
349
350 sc->mux_shift = clkdef->mux_shift;
351 sc->mux_width = clkdef->mux_width;
352 sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
353
354 sc->div_shift = clkdef->div_shift;
355 sc->div_width = clkdef->div_width;
356 sc->div_mask = ((1 << clkdef->div_width) - 1) << sc->div_shift;
357
358 sc->gate_offset = clkdef->gate_offset;
359 sc->gate_shift = clkdef->gate_shift;
360
361 sc->flags = clkdef->flags;
362
363 clknode_register(clkdom, clk);
364
365 return (0);
366 }
367