xref: /freebsd/sys/dev/qcom_clk/qcom_clk_rcg2.c (revision 833a452e9f082a7982a31c21f0da437dbbe0a39d)
1 /*-
2  * Copyright (c) 2021 Adrian Chadd <adrian@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 AND CONTRIBUTORS ``AS IS'' AND
14  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
15  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
16  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
17  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
18  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
19  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
20  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
21  * LIABILITY, 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/cdefs.h>
27 __FBSDID("$FreeBSD$");
28 
29 #include <sys/param.h>
30 #include <sys/systm.h>
31 #include <sys/bus.h>
32 #include <sys/lock.h>
33 #include <sys/mutex.h>
34 #include <sys/rman.h>
35 #include <machine/bus.h>
36 
37 #include <dev/extres/clk/clk.h>
38 #include <dev/extres/clk/clk_div.h>
39 #include <dev/extres/clk/clk_fixed.h>
40 #include <dev/extres/clk/clk_mux.h>
41 
42 #include "qcom_clk_freqtbl.h"
43 #include "qcom_clk_rcg2.h"
44 #include "qcom_clk_rcg2_reg.h"
45 
46 #include "clkdev_if.h"
47 
48 #if 0
49 #define DPRINTF(dev, msg...) device_printf(dev, msg);
50 #else
51 #define DPRINTF(dev, msg...)
52 #endif
53 
54 #define	QCOM_CLK_RCG2_CFG_OFFSET(sc)	\
55 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_CFG_REG)
56 #define	QCOM_CLK_RCG2_CMD_REGISTER(sc)	\
57 	    ((sc)->cmd_rcgr + QCOM_CLK_RCG2_CMD_REG)
58 #define	QCOM_CLK_RCG2_M_OFFSET(sc)	\
59 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_M_REG)
60 #define	QCOM_CLK_RCG2_N_OFFSET(sc)	\
61 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_N_REG)
62 #define	QCOM_CLK_RCG2_D_OFFSET(sc)	\
63 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_D_REG)
64 
65 struct qcom_clk_rcg2_sc {
66 	struct clknode *clknode;
67 	uint32_t cmd_rcgr;
68 	uint32_t hid_width;
69 	uint32_t mnd_width;
70 	int32_t safe_src_idx;
71 	uint32_t cfg_offset;
72 	int safe_pre_parent_idx;
73 	uint32_t flags;
74 	const struct qcom_clk_freq_tbl *freq_tbl;
75 };
76 
77 
78 /*
79  * Finish a clock update.
80  *
81  * This instructs the configuration to take effect.
82  */
83 static bool
84 qcom_clk_rcg2_update_config_locked(struct qcom_clk_rcg2_sc *sc)
85 {
86 	uint32_t reg, count;
87 
88 	/*
89 	 * Send "update" to the controller.
90 	 */
91 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
92 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
93 	reg |= QCOM_CLK_RCG2_CMD_UPDATE;
94 	CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
95 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), reg);
96 	wmb();
97 
98 	/*
99 	 * Poll for completion of update.
100 	 */
101 	for (count = 0; count < 1000; count++) {
102 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
103 		    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
104 		if ((reg & QCOM_CLK_RCG2_CMD_UPDATE) == 0) {
105 			return (true);
106 		}
107 		DELAY(10);
108 		rmb();
109 	}
110 
111 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
112 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
113 	DPRINTF(clknode_get_device(sc->clknode), "%s: failed; reg=0x%08x\n",
114 	    __func__, reg);
115 	return (false);
116 }
117 
118 /*
119  * Calculate the output frequency given an input frequency and the m/n:d
120  * configuration.
121  */
122 static uint64_t
123 qcom_clk_rcg2_calc_rate(uint64_t rate, uint32_t mode, uint32_t m, uint32_t n,
124     uint32_t hid_div)
125 {
126 	if (hid_div != 0) {
127 		rate = rate * 2;
128 		rate = rate / (hid_div + 1);
129 	}
130 
131 	/* Note: assume n is not 0 here; bad things happen if it is */
132 
133 	if (mode != 0) {
134 		rate = (rate * m) / n;
135 	}
136 
137 	return (rate);
138 }
139 
140 /*
141  * The inverse of calc_rate() - calculate the required input frequency
142  * given the desired output freqency and m/n:d configuration.
143  */
144 static uint64_t
145 qcom_clk_rcg2_calc_input_freq(uint64_t freq, uint32_t m, uint32_t n,
146     uint32_t hid_div)
147 {
148 	if (hid_div != 0) {
149 		freq = freq / 2;
150 		freq = freq * (hid_div + 1);
151 	}
152 
153 	if (n != 0) {
154 		freq = (freq * n) / m;
155 	}
156 
157 	return (freq);
158 }
159 
160 static int
161 qcom_clk_rcg2_recalc(struct clknode *clk, uint64_t *freq)
162 {
163 	struct qcom_clk_rcg2_sc *sc;
164 	uint32_t cfg, m = 0, n = 0, hid_div = 0;
165 	uint32_t mode = 0, mask;
166 
167 	sc = clknode_get_softc(clk);
168 
169 	/* Read the MODE, CFG, M and N parameters */
170 	CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
171 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
172 	    QCOM_CLK_RCG2_CFG_OFFSET(sc),
173 	    &cfg);
174 	if (sc->mnd_width != 0) {
175 		mask = (1U << sc->mnd_width) - 1;
176 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
177 		    QCOM_CLK_RCG2_M_OFFSET(sc), &m);
178 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
179 		    QCOM_CLK_RCG2_N_OFFSET(sc), &n);
180 		m = m & mask;
181 		n = ~ n;
182 		n = n & mask;
183 		n = n + m;
184 		mode = (cfg & QCOM_CLK_RCG2_CFG_MODE_MASK)
185 		    >> QCOM_CLK_RCG2_CFG_MODE_SHIFT;
186 	}
187 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
188 
189 	/* Fetch the divisor */
190 	mask = (1U << sc->hid_width) - 1;
191 	hid_div = (cfg >> QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT) & mask;
192 
193 	/* Calculate the rate based on the parent rate and config */
194 	*freq = qcom_clk_rcg2_calc_rate(*freq, mode, m, n, hid_div);
195 
196 	return (0);
197 }
198 
199 /*
200  * configure the mn:d divisor, pre-divisor, and parent.
201  */
202 static void
203 qcom_clk_rcg2_set_config_locked(struct qcom_clk_rcg2_sc *sc,
204     const struct qcom_clk_freq_tbl *f, int parent_idx)
205 {
206 	uint32_t mask, reg;
207 
208 	/* If we have MN:D, then update it */
209 	if (sc->mnd_width != 0 && f->n != 0) {
210 		mask = (1U << sc->mnd_width) - 1;
211 
212 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
213 		    QCOM_CLK_RCG2_M_OFFSET(sc), &reg);
214 		reg &= ~mask;
215 		reg |= (f->m & mask);
216 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
217 		    QCOM_CLK_RCG2_M_OFFSET(sc), reg);
218 
219 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
220 		    QCOM_CLK_RCG2_N_OFFSET(sc), &reg);
221 		reg &= ~mask;
222 		reg |= ((~(f->n - f->m)) & mask);
223 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
224 		    QCOM_CLK_RCG2_N_OFFSET(sc), reg);
225 
226 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
227 		    QCOM_CLK_RCG2_D_OFFSET(sc), &reg);
228 		reg &= ~mask;
229 		reg |= ((~f->n) & mask);
230 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
231 		    QCOM_CLK_RCG2_D_OFFSET(sc), reg);
232 	}
233 
234 	mask = (1U << sc->hid_width) - 1;
235 	/*
236 	 * Mask out register fields we're going to modify along with
237 	 * the pre-divisor.
238 	 */
239 	mask |= QCOM_CLK_RCG2_CFG_SRC_SEL_MASK
240 	    | QCOM_CLK_RCG2_CFG_MODE_MASK
241 	    | QCOM_CLK_RCG2_CFG_HW_CLK_CTRL_MASK;
242 
243 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
244 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
245 	reg &= ~mask;
246 
247 	/* Configure pre-divisor */
248 	reg = reg | ((f->pre_div) << QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT);
249 
250 	/* Configure parent clock */
251 	reg = reg | (((parent_idx << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
252 	    & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
253 
254 	/* Configure dual-edge if needed */
255 	if (sc->mnd_width != 0 && f->n != 0 && (f->m != f->n))
256 		reg |= QCOM_CLK_RCG2_CFG_MODE_DUAL_EDGE;
257 
258 	CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
259 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), reg);
260 }
261 
262 static int
263 qcom_clk_rcg2_init(struct clknode *clk, device_t dev)
264 {
265 	struct qcom_clk_rcg2_sc *sc;
266 	uint32_t reg;
267 	uint32_t idx;
268 	bool enabled;
269 
270 	sc = clknode_get_softc(clk);
271 
272 	/*
273 	 * Read the mux setting to set the right parent.
274 	 * Whilst here, read the config to get whether we're enabled
275 	 * or not.
276 	 */
277 	CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
278 	/* check if rcg2 root clock is enabled */
279 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
280 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
281 	if (reg & QCOM_CLK_RCG2_CMD_ROOT_OFF)
282 		enabled = false;
283 	else
284 		enabled = true;
285 	/* mux settings */
286 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
287 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
288 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
289 
290 	idx = (reg & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK)
291 	    >> QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT;
292 	DPRINTF(clknode_get_device(sc->clknode), "%s: mux index %u\n",
293 	    __func__, idx);
294 	clknode_init_parent_idx(clk, idx);
295 
296 	/*
297 	 * If we could be sure our parent clocks existed here in the tree,
298 	 * we could calculate our current frequency by fetching the parent
299 	 * frequency and then do our divider math.  Unfortunately that
300 	 * currently isn't the case.
301 	 */
302 
303 	return(0);
304 }
305 
306 static int
307 qcom_clk_rcg2_set_gate(struct clknode *clk, bool enable)
308 {
309 
310 	/*
311 	 * For now this isn't supported; there's some support for
312 	 * "shared" rcg2 nodes in the Qualcomm/upstream Linux trees but
313 	 * it's not currently needed for the supported platforms.
314 	 */
315 	return (0);
316 }
317 
318 /*
319  * Program the parent index.
320  *
321  * This doesn't do the update.  It also must be called with the device
322  * lock held.
323  */
324 static void
325 qcom_clk_rcg2_set_parent_index_locked(struct qcom_clk_rcg2_sc *sc,
326     uint32_t index)
327 {
328 	uint32_t reg;
329 
330 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
331 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
332 	reg = reg & ~QCOM_CLK_RCG2_CFG_SRC_SEL_MASK;
333 	reg = reg | (((index << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
334 	    & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
335 	CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
336 	    QCOM_CLK_RCG2_CFG_OFFSET(sc),
337 	    reg);
338 }
339 
340 /*
341  * Set frequency
342  *
343  * fin - the parent frequency, if exists
344  * fout - starts as the requested frequency, ends with the configured
345  *        or dry-run frequency
346  * Flags - CLK_SET_DRYRUN, CLK_SET_ROUND_UP, CLK_SET_ROUND_DOWN
347  * retval - 0, ERANGE
348  */
349 static int
350 qcom_clk_rcg2_set_freq(struct clknode *clk, uint64_t fin, uint64_t *fout,
351     int flags, int *stop)
352 {
353 	struct qcom_clk_rcg2_sc *sc;
354 	const struct qcom_clk_freq_tbl *f;
355 	const char **parent_names;
356 	uint64_t p_freq, p_clk_freq;
357 	int parent_cnt;
358 	struct clknode *p_clk;
359 	int i;
360 
361 	sc = clknode_get_softc(clk);
362 
363 	/*
364 	 * Find a suitable frequency in the frequency table.
365 	 *
366 	 * TODO: should pay attention to ROUND_UP / ROUND_DOWN and add
367 	 * a freqtbl method to handle both accordingly.
368 	 */
369 	f = qcom_clk_freq_tbl_lookup(sc->freq_tbl, *fout);
370 	if (f == NULL) {
371 		device_printf(clknode_get_device(sc->clknode),
372 		    "%s: no suitable freqtbl entry found for freq %llu\n",
373 		    __func__,
374 		    *fout);
375 		return (ERANGE);
376 	}
377 
378 	/*
379 	 * Find the parent index for the given parent clock.
380 	 * Abort if we can't actually find it.
381 	 *
382 	 * XXX TODO: this should be a clk API call!
383 	 */
384 	parent_cnt = clknode_get_parents_num(clk);
385 	parent_names = clknode_get_parent_names(clk);
386 	for (i = 0; i < parent_cnt; i++) {
387 		if (parent_names[i] == NULL)
388 			continue;
389 		if (strcmp(parent_names[i], f->parent) == 0)
390 			break;
391 	}
392 	if (i >= parent_cnt) {
393 		device_printf(clknode_get_device(sc->clknode),
394 		    "%s: couldn't find suitable parent?\n",
395 		    __func__);
396 		return (ENXIO);
397 	}
398 
399 	/*
400 	 * If we aren't setting the parent clock, then we need
401 	 * to just program the new parent clock in and update.
402 	 * (or for DRYRUN just skip that and return the new
403 	 * frequency.)
404 	 */
405 	if ((sc->flags & QCOM_CLK_RCG2_FLAGS_SET_RATE_PARENT) == 0) {
406 		if (flags & CLK_SET_DRYRUN) {
407 			*fout = f->freq;
408 			return (0);
409 		}
410 
411 		if (sc->safe_pre_parent_idx > -1) {
412 			DPRINTF(clknode_get_device(sc->clknode),
413 			    "%s: setting to safe parent idx %d\n",
414 			    __func__,
415 			    sc->safe_pre_parent_idx);
416 			CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
417 			qcom_clk_rcg2_set_parent_index_locked(sc,
418 			    sc->safe_pre_parent_idx);
419 			DPRINTF(clknode_get_device(sc->clknode),
420 			    "%s: safe parent: updating config\n", __func__);
421 			if (! qcom_clk_rcg2_update_config_locked(sc)) {
422 				CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
423 				DPRINTF(clknode_get_device(sc->clknode),
424 				    "%s: error updating config\n",
425 				    __func__);
426 				return (ENXIO);
427 			}
428 			CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
429 			DPRINTF(clknode_get_device(sc->clknode),
430 			    "%s: safe parent: done\n", __func__);
431 			clknode_set_parent_by_idx(sc->clknode,
432 			    sc->safe_pre_parent_idx);
433 		}
434 		/* Program parent index, then schedule update */
435 		CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
436 		qcom_clk_rcg2_set_parent_index_locked(sc, i);
437 		if (! qcom_clk_rcg2_update_config_locked(sc)) {
438 			CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
439 			device_printf(clknode_get_device(sc->clknode),
440 			    "%s: couldn't program in parent idx %u!\n",
441 			    __func__, i);
442 			return (ENXIO);
443 		}
444 		CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
445 		clknode_set_parent_by_idx(sc->clknode, i);
446 		*fout = f->freq;
447 		return (0);
448 	}
449 
450 	/*
451 	 * If we /are/ setting the parent clock, then we need
452 	 * to determine what frequency we need the parent to
453 	 * be, and then reconfigure the parent to the new
454 	 * frequency, and then change our parent.
455 	 *
456 	 * (Again, if we're doing DRYRUN, just skip that
457 	 * and return the new frequency.)
458 	 */
459 	p_clk = clknode_find_by_name(f->parent);
460 	if (p_clk == NULL) {
461 		device_printf(clknode_get_device(sc->clknode),
462 		    "%s: couldn't find parent clk (%s)\n",
463 		    __func__, f->parent);
464 		return (ENXIO);
465 	}
466 
467 	/*
468 	 * Calculate required frequency from said parent clock to
469 	 * meet the needs of our target clock.
470 	 */
471 	p_freq = qcom_clk_rcg2_calc_input_freq(f->freq, f->m, f->n,
472 	    f->pre_div);
473 	DPRINTF(clknode_get_device(sc->clknode),
474 	    "%s: request %llu, parent %s freq %llu, parent freq %llu\n",
475 	    __func__,
476 	    *fout,
477 	    f->parent,
478 	    f->freq,
479 	    p_freq);
480 
481 	/*
482 	 * To ensure glitch-free operation on some clocks, set it to
483 	 * a safe parent before programming our divisor and the parent
484 	 * clock configuration.  Then once it's done, flip the parent
485 	 * to the new parent.
486 	 *
487 	 * If we're doing a dry-run then we don't need to re-parent the
488 	 * clock just yet!
489 	 */
490 	if (((flags & CLK_SET_DRYRUN) == 0) &&
491 	    (sc->safe_pre_parent_idx > -1)) {
492 		DPRINTF(clknode_get_device(sc->clknode),
493 		    "%s: setting to safe parent idx %d\n",
494 		    __func__,
495 		    sc->safe_pre_parent_idx);
496 		CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
497 		qcom_clk_rcg2_set_parent_index_locked(sc,
498 		    sc->safe_pre_parent_idx);
499 		DPRINTF(clknode_get_device(sc->clknode),
500 		    "%s: safe parent: updating config\n", __func__);
501 		if (! qcom_clk_rcg2_update_config_locked(sc)) {
502 			CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
503 			DPRINTF(clknode_get_device(sc->clknode),
504 			    "%s: error updating config\n",
505 			    __func__);
506 			return (ENXIO);
507 		}
508 		CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
509 		DPRINTF(clknode_get_device(sc->clknode),
510 		    "%s: safe parent: done\n", __func__);
511 		clknode_set_parent_by_idx(sc->clknode,
512 		    sc->safe_pre_parent_idx);
513 	}
514 
515 	/*
516 	 * Set the parent frequency before we change our mux and divisor
517 	 * configuration.
518 	 */
519 	if (clknode_get_freq(p_clk, &p_clk_freq) != 0) {
520 		device_printf(clknode_get_device(sc->clknode),
521 		    "%s: couldn't get freq for parent clock %s\n",
522 		    __func__,
523 		    f->parent);
524 		return (ENXIO);
525 	}
526 	if (p_clk_freq != p_freq) {
527 		uint64_t n_freq;
528 		int rv;
529 
530 		/*
531 		 * If we're doing a dryrun then call test_freq() not set_freq().
532 		 * That way we get the frequency back that we would be set to.
533 		 *
534 		 * If we're not doing a dry run then set the frequency, then
535 		 * call get_freq to get what it was set to.
536 		 */
537 		if (flags & CLK_SET_DRYRUN) {
538 			n_freq = p_freq;
539 			rv = clknode_test_freq(p_clk, n_freq, flags, 0,
540 			    &p_freq);
541 		} else {
542 			rv = clknode_set_freq(p_clk, p_freq, flags, 0);
543 		}
544 
545 		if (rv != 0) {
546 			device_printf(clknode_get_device(sc->clknode),
547 			    "%s: couldn't set parent clock %s frequency to "
548 			    "%llu\n",
549 			    __func__,
550 			    f->parent,
551 			    p_freq);
552 			return (ENXIO);
553 		}
554 
555 		/* Frequency was set, fetch what it was set to */
556 		if ((flags & CLK_SET_DRYRUN) == 0) {
557 			rv = clknode_get_freq(p_clk, &p_freq);
558 			if (rv != 0) {
559 				device_printf(clknode_get_device(sc->clknode),
560 				    "%s: couldn't get parent frequency",
561 				    __func__);
562 				return (ENXIO);
563 			}
564 		}
565 	}
566 
567 	DPRINTF(clknode_get_device(sc->clknode),
568 	    "%s: requsted freq=%llu, target freq=%llu,"
569 	    " parent choice=%s, parent_freq=%llu\n",
570 	    __func__,
571 	    *fout,
572 	    f->freq,
573 	    f->parent,
574 	    p_freq);
575 
576 	/*
577 	 * Set the parent node, the parent programming and the divisor
578 	 * config.  Because they're done together, we don't go via
579 	 * a mux method on this node.
580 	 */
581 
582 	/*
583 	 * Program the divisor and parent.
584 	 */
585 	if ((flags & CLK_SET_DRYRUN) == 0) {
586 		CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
587 		qcom_clk_rcg2_set_config_locked(sc, f, i);
588 		if (! qcom_clk_rcg2_update_config_locked(sc)) {
589 			CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
590 			device_printf(clknode_get_device(sc->clknode),
591 			    "%s: couldn't program in divisor, help!\n",
592 			    __func__);
593 			return (ENXIO);
594 		}
595 		CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
596 		clknode_set_parent_by_idx(sc->clknode, i);
597 	}
598 
599 	/*
600 	 * p_freq is now the frequency that the parent /is/ set to.
601 	 * (Or would be set to for a dry run.)
602 	 *
603 	 * Calculate what the eventual frequency would be, we'll want
604 	 * this to return when we're done - and again, if it's a dryrun,
605 	 * don't set anything up.  This doesn't rely on the register
606 	 * contents.
607 	 */
608 	*fout = qcom_clk_rcg2_calc_rate(p_freq, (f->n == 0 ? 0 : 1),
609 	    f->m, f->n, f->pre_div);
610 
611 	return (0);
612 }
613 
614 static clknode_method_t qcom_clk_rcg2_methods[] = {
615 	/* Device interface */
616 	CLKNODEMETHOD(clknode_init,		qcom_clk_rcg2_init),
617 	CLKNODEMETHOD(clknode_recalc_freq,	qcom_clk_rcg2_recalc),
618 	CLKNODEMETHOD(clknode_set_gate,		qcom_clk_rcg2_set_gate),
619 	CLKNODEMETHOD(clknode_set_freq,		qcom_clk_rcg2_set_freq),
620 	CLKNODEMETHOD_END
621 };
622 
623 DEFINE_CLASS_1(qcom_clk_fepll, qcom_clk_rcg2_class, qcom_clk_rcg2_methods,
624    sizeof(struct qcom_clk_rcg2_sc), clknode_class);
625 
626 int
627 qcom_clk_rcg2_register(struct clkdom *clkdom,
628     struct qcom_clk_rcg2_def *clkdef)
629 {
630 	struct clknode *clk;
631 	struct qcom_clk_rcg2_sc *sc;
632 
633 	/*
634 	 * Right now the rcg2 code isn't supporting turning off the clock
635 	 * or limiting it to the lowest parent clock.  But, do set the
636 	 * flags appropriately.
637 	 */
638 	if (clkdef->flags & QCOM_CLK_RCG2_FLAGS_CRITICAL)
639 		clkdef->clkdef.flags |= CLK_NODE_CANNOT_STOP;
640 
641 	clk = clknode_create(clkdom, &qcom_clk_rcg2_class, &clkdef->clkdef);
642 	if (clk == NULL)
643 		return (1);
644 
645 	sc = clknode_get_softc(clk);
646 	sc->clknode = clk;
647 
648 	sc->cmd_rcgr = clkdef->cmd_rcgr;
649 	sc->hid_width = clkdef->hid_width;
650 	sc->mnd_width = clkdef->mnd_width;
651 	sc->safe_src_idx = clkdef->safe_src_idx;
652 	sc->safe_pre_parent_idx = clkdef->safe_pre_parent_idx;
653 	sc->cfg_offset = clkdef->cfg_offset;
654 	sc->flags = clkdef->flags;
655 	sc->freq_tbl = clkdef->freq_tbl;
656 
657 	clknode_register(clkdom, clk);
658 
659 	return (0);
660 }
661