xref: /freebsd/sys/dev/clk/allwinner/aw_clk_frac.c (revision a90b9d0159070121c221b966469c3e36d912bf82) 
1 /*-
2  * Copyright (c) 2019 Emmanuel Vadot <manu@freebsd.org>
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  * 1. Redistributions of source code must retain the above copyright
8  *    notice, this list of conditions and the following disclaimer.
9  * 2. Redistributions in binary form must reproduce the above copyright
10  *    notice, this list of conditions and the following disclaimer in the
11  *    documentation and/or other materials provided with the distribution.
12  *
13  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
14  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
15  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
16  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
17  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
18  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
19  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
20  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
21  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
22  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
23  * SUCH DAMAGE.
24  */
25 
26 #include <sys/param.h>
27 #include <sys/systm.h>
28 #include <sys/bus.h>
29 
30 #include <dev/clk/clk.h>
31 
32 #include <dev/clk/allwinner/aw_clk.h>
33 #include <dev/clk/allwinner/aw_clk_frac.h>
34 
35 #include "clkdev_if.h"
36 
37 /* #define	dprintf(format, arg...)	printf("%s:(%s)" format, __func__, clknode_get_name(clk), arg) */
38 #define	dprintf(format, arg...)
39 
40 /*
41  * clknode for clocks matching the formula :
42  *
43  * clk = (24Mhz * n) / m in integer mode
44  * clk = frac_out1 or frac_out2 in fractional mode
45  *
46  */
47 
48 struct aw_clk_frac_sc {
49 	uint32_t	offset;
50 
51 	struct aw_clk_factor	m;
52 	struct aw_clk_factor	n;
53 	struct aw_clk_frac	frac;
54 
55 	uint64_t		min_freq;
56 	uint64_t		max_freq;
57 
58 	uint32_t	mux_shift;
59 	uint32_t	mux_mask;
60 	uint32_t	gate_shift;
61 	uint32_t	lock_shift;
62 	uint32_t	lock_retries;
63 
64 	uint32_t	flags;
65 };
66 
67 #define	WRITE4(_clk, off, val)						\
68 	CLKDEV_WRITE_4(clknode_get_device(_clk), off, val)
69 #define	READ4(_clk, off, val)						\
70 	CLKDEV_READ_4(clknode_get_device(_clk), off, val)
71 #define	DEVICE_LOCK(_clk)							\
72 	CLKDEV_DEVICE_LOCK(clknode_get_device(_clk))
73 #define	DEVICE_UNLOCK(_clk)						\
74 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(_clk))
75 
76 static int
77 aw_clk_frac_init(struct clknode *clk, device_t dev)
78 {
79 	struct aw_clk_frac_sc *sc;
80 	uint32_t val, idx;
81 
82 	sc = clknode_get_softc(clk);
83 
84 	idx = 0;
85 	if ((sc->flags & AW_CLK_HAS_MUX) != 0) {
86 		DEVICE_LOCK(clk);
87 		READ4(clk, sc->offset, &val);
88 		DEVICE_UNLOCK(clk);
89 
90 		idx = (val & sc->mux_mask) >> sc->mux_shift;
91 	}
92 
93 	dprintf("init parent idx %d\n", idx);
94 	clknode_init_parent_idx(clk, idx);
95 	return (0);
96 }
97 
98 static int
99 aw_clk_frac_set_gate(struct clknode *clk, bool enable)
100 {
101 	struct aw_clk_frac_sc *sc;
102 	uint32_t val;
103 
104 	sc = clknode_get_softc(clk);
105 
106 	if ((sc->flags & AW_CLK_HAS_GATE) == 0)
107 		return (0);
108 
109 	dprintf("%sabling gate\n", enable ? "En" : "Dis");
110 	DEVICE_LOCK(clk);
111 	READ4(clk, sc->offset, &val);
112 	if (enable)
113 		val |= (1 << sc->gate_shift);
114 	else
115 		val &= ~(1 << sc->gate_shift);
116 	WRITE4(clk, sc->offset, val);
117 	DEVICE_UNLOCK(clk);
118 
119 	return (0);
120 }
121 
122 static int
123 aw_clk_frac_set_mux(struct clknode *clk, int index)
124 {
125 	struct aw_clk_frac_sc *sc;
126 	uint32_t val;
127 
128 	sc = clknode_get_softc(clk);
129 
130 	if ((sc->flags & AW_CLK_HAS_MUX) == 0)
131 		return (0);
132 
133 	dprintf("Set mux to %d\n", index);
134 	DEVICE_LOCK(clk);
135 	READ4(clk, sc->offset, &val);
136 	val &= ~sc->mux_mask;
137 	val |= index << sc->mux_shift;
138 	WRITE4(clk, sc->offset, val);
139 	DEVICE_UNLOCK(clk);
140 
141 	return (0);
142 }
143 
144 static uint64_t
145 aw_clk_frac_find_best(struct aw_clk_frac_sc *sc, uint64_t fparent, uint64_t fout,
146     uint32_t *factor_n, uint32_t *factor_m)
147 {
148 	uint64_t cur, best;
149 	uint32_t m, n, max_m, max_n, min_m, min_n;
150 
151 	*factor_n = *factor_m = 0;
152 	best = cur = 0;
153 
154 	max_m = aw_clk_factor_get_max(&sc->m);
155 	max_n = aw_clk_factor_get_max(&sc->n);
156 	min_m = aw_clk_factor_get_min(&sc->m);
157 	min_n = sc->min_freq / fparent;
158 
159 	for (n = min_n; n <= max_n; n++) {
160 		for (m = min_m; m <= max_m; m++) {
161 			cur = fparent * n / m;
162 			if (cur < sc->min_freq) {
163 				continue;
164 			}
165 			if (cur > sc->max_freq) {
166 				continue;
167 			}
168 			if (cur == fout) {
169 				*factor_n = n;
170 				*factor_m = m;
171 				return (cur);
172 			}
173 			if (abs((fout - cur)) < abs((fout - best))) {
174 				best = cur;
175 				*factor_n = n;
176 				*factor_m = m;
177 			}
178 		}
179 	}
180 
181 	return (best);
182 }
183 
184 static int
185 aw_clk_frac_set_freq(struct clknode *clk, uint64_t fparent, uint64_t *fout,
186     int flags, int *stop)
187 {
188 	struct aw_clk_frac_sc *sc;
189 	uint64_t cur, best, best_frac;
190 	uint32_t val, m, n, best_m, best_n;
191 	int retry, multiple, max_mult, best_mult;
192 
193 	sc = clknode_get_softc(clk);
194 
195 	best = best_frac = cur = 0;
196 	best_mult = 0;
197 	max_mult = 1;
198 
199 	dprintf("Trying to find freq %ju with parent %ju\n", *fout, fparent);
200 	if ((flags & CLK_SET_ROUND_MULTIPLE) != 0)
201 		max_mult = 10;
202 	for (multiple = 1; multiple <= max_mult; multiple++) {
203 		/* First test the fractional frequencies */
204 		dprintf("Testing with multiple %d\n", multiple);
205 		if (*fout * multiple == sc->frac.freq0) {
206 			best = best_frac = sc->frac.freq0;
207 			best_mult = multiple;
208 			dprintf("Found with using frac.freq0 and multiple %d\n", multiple);
209 			break;
210 		}
211 		else if (*fout * multiple == sc->frac.freq1) {
212 			best = best_frac = sc->frac.freq1;
213 			best_mult = multiple;
214 			dprintf("Found with using frac.freq1 and multiple %d\n", multiple);
215 			break;
216 		}
217 		else {
218 			cur = aw_clk_frac_find_best(sc, fparent, *fout * multiple,
219 			  &n, &m);
220 			dprintf("Got %ju with n=%d, m=%d\n", cur, n, m);
221 			if (cur == (*fout * multiple)) {
222 				best = cur;
223 				best_mult = multiple;
224 				best_n = n;
225 				best_m = m;
226 				dprintf("This is the one: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
227 				break;
228 			}
229 			if (abs(((*fout * multiple) - cur)) < abs(((*fout * multiple) - best))) {
230 				best = cur;
231 				best_mult = multiple;
232 				best_n = n;
233 				best_m = m;
234 				dprintf("This is the best for now: n=%d m=%d mult=%d\n", best_n, best_m, best_mult);
235 			}
236 		}
237 	}
238 
239 	if (best < sc->min_freq ||
240 	    best > sc->max_freq) {
241 		printf("%s: Cannot set %ju for %s (min=%ju max=%ju)\n",
242 		    __func__, best, clknode_get_name(clk),
243 		    sc->min_freq, sc->max_freq);
244 		return (ERANGE);
245 	}
246 	if ((flags & CLK_SET_DRYRUN) != 0) {
247 		*fout = best;
248 		*stop = 1;
249 		return (0);
250 	}
251 
252 	if ((best < (*fout * best_mult)) &&
253 	  ((flags & CLK_SET_ROUND_DOWN) == 0)) {
254 		*stop = 1;
255 		return (ERANGE);
256 	}
257 	if ((best > *fout * best_mult) &&
258 	  ((flags & CLK_SET_ROUND_UP) == 0)) {
259 		*stop = 1;
260 		return (ERANGE);
261 	}
262 
263 	DEVICE_LOCK(clk);
264 	READ4(clk, sc->offset, &val);
265 	/* Disable clock during freq changes */
266 	val &= ~(1 << sc->gate_shift);
267 	WRITE4(clk, sc->offset, val);
268 
269 	if (best_frac != 0) {
270 		val &= ~sc->frac.mode_sel;
271 		/* M should be 0 per the manual */
272 		val &= ~sc->m.mask;
273 		if (best_frac == sc->frac.freq0)
274 			val &= ~sc->frac.freq_sel;
275 		else
276 			val |= sc->frac.freq_sel;
277 	} else {
278 		val |= sc->frac.mode_sel; /* Select integer mode */
279 		n = aw_clk_factor_get_value(&sc->n, best_n);
280 		m = aw_clk_factor_get_value(&sc->m, best_m);
281 		val &= ~sc->n.mask;
282 		val &= ~sc->m.mask;
283 		val |= n << sc->n.shift;
284 		val |= m << sc->m.shift;
285 	}
286 
287 	/* Write the clock changes */
288 	WRITE4(clk, sc->offset, val);
289 
290 	/* Enable clock now that we've change it */
291 	val |= 1 << sc->gate_shift;
292 	WRITE4(clk, sc->offset, val);
293 	DEVICE_UNLOCK(clk);
294 
295 	for (retry = 0; retry < sc->lock_retries; retry++) {
296 		READ4(clk, sc->offset, &val);
297 		if ((val & (1 << sc->lock_shift)) != 0)
298 			break;
299 		DELAY(1000);
300 	}
301 
302 	*fout = best;
303 	*stop = 1;
304 
305 	return (0);
306 }
307 
308 static int
309 aw_clk_frac_recalc(struct clknode *clk, uint64_t *freq)
310 {
311 	struct aw_clk_frac_sc *sc;
312 	uint32_t val, m, n;
313 
314 	sc = clknode_get_softc(clk);
315 
316 	DEVICE_LOCK(clk);
317 	READ4(clk, sc->offset, &val);
318 	DEVICE_UNLOCK(clk);
319 
320 	if ((val & sc->frac.mode_sel) == 0) {
321 		if (val & sc->frac.freq_sel)
322 			*freq = sc->frac.freq1;
323 		else
324 			*freq = sc->frac.freq0;
325 	} else {
326 		m = aw_clk_get_factor(val, &sc->m);
327 		n = aw_clk_get_factor(val, &sc->n);
328 		*freq = *freq * n / m;
329 	}
330 
331 	return (0);
332 }
333 
334 static clknode_method_t aw_frac_clknode_methods[] = {
335 	/* Device interface */
336 	CLKNODEMETHOD(clknode_init,		aw_clk_frac_init),
337 	CLKNODEMETHOD(clknode_set_gate,		aw_clk_frac_set_gate),
338 	CLKNODEMETHOD(clknode_set_mux,		aw_clk_frac_set_mux),
339 	CLKNODEMETHOD(clknode_recalc_freq,	aw_clk_frac_recalc),
340 	CLKNODEMETHOD(clknode_set_freq,		aw_clk_frac_set_freq),
341 	CLKNODEMETHOD_END
342 };
343 
344 DEFINE_CLASS_1(aw_frac_clknode, aw_frac_clknode_class, aw_frac_clknode_methods,
345     sizeof(struct aw_clk_frac_sc), clknode_class);
346 
347 int
348 aw_clk_frac_register(struct clkdom *clkdom, struct aw_clk_frac_def *clkdef)
349 {
350 	struct clknode *clk;
351 	struct aw_clk_frac_sc *sc;
352 
353 	clk = clknode_create(clkdom, &aw_frac_clknode_class, &clkdef->clkdef);
354 	if (clk == NULL)
355 		return (1);
356 
357 	sc = clknode_get_softc(clk);
358 
359 	sc->offset = clkdef->offset;
360 
361 	sc->m.shift = clkdef->m.shift;
362 	sc->m.width = clkdef->m.width;
363 	sc->m.mask = ((1 << sc->m.width) - 1) << sc->m.shift;
364 	sc->m.value = clkdef->m.value;
365 	sc->m.flags = clkdef->m.flags;
366 
367 	sc->n.shift = clkdef->n.shift;
368 	sc->n.width = clkdef->n.width;
369 	sc->n.mask = ((1 << sc->n.width) - 1) << sc->n.shift;
370 	sc->n.value = clkdef->n.value;
371 	sc->n.flags = clkdef->n.flags;
372 
373 	sc->frac.freq0 = clkdef->frac.freq0;
374 	sc->frac.freq1 = clkdef->frac.freq1;
375 	sc->frac.mode_sel = 1 << clkdef->frac.mode_sel;
376 	sc->frac.freq_sel = 1 << clkdef->frac.freq_sel;
377 
378 	sc->min_freq = clkdef->min_freq;
379 	sc->max_freq = clkdef->max_freq;
380 
381 	sc->mux_shift = clkdef->mux_shift;
382 	sc->mux_mask = ((1 << clkdef->mux_width) - 1) << sc->mux_shift;
383 
384 	sc->gate_shift = clkdef->gate_shift;
385 
386 	sc->lock_shift = clkdef->lock_shift;
387 	sc->lock_retries = clkdef->lock_retries;
388 
389 	sc->flags = clkdef->flags;
390 
391 	clknode_register(clkdom, clk);
392 
393 	return (0);
394 }
395