xref: /freebsd/sys/dev/qcom_clk/qcom_clk_rcg2.c (revision 22cf89c938886d14f5796fc49f9f020c23ea8eaf)
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 #include <sys/param.h>
28 #include <sys/systm.h>
29 #include <sys/bus.h>
30 #include <sys/lock.h>
31 #include <sys/mutex.h>
32 #include <sys/rman.h>
33 #include <machine/bus.h>
34 
35 #include <dev/extres/clk/clk.h>
36 #include <dev/extres/clk/clk_div.h>
37 #include <dev/extres/clk/clk_fixed.h>
38 #include <dev/extres/clk/clk_mux.h>
39 
40 #include "qcom_clk_freqtbl.h"
41 #include "qcom_clk_rcg2.h"
42 #include "qcom_clk_rcg2_reg.h"
43 
44 #include "clkdev_if.h"
45 
46 #if 0
47 #define DPRINTF(dev, msg...) device_printf(dev, msg);
48 #else
49 #define DPRINTF(dev, msg...)
50 #endif
51 
52 #define	QCOM_CLK_RCG2_CFG_OFFSET(sc)	\
53 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_CFG_REG)
54 #define	QCOM_CLK_RCG2_CMD_REGISTER(sc)	\
55 	    ((sc)->cmd_rcgr + QCOM_CLK_RCG2_CMD_REG)
56 #define	QCOM_CLK_RCG2_M_OFFSET(sc)	\
57 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_M_REG)
58 #define	QCOM_CLK_RCG2_N_OFFSET(sc)	\
59 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_N_REG)
60 #define	QCOM_CLK_RCG2_D_OFFSET(sc)	\
61 	    ((sc)->cmd_rcgr + (sc)->cfg_offset + QCOM_CLK_RCG2_D_REG)
62 
63 struct qcom_clk_rcg2_sc {
64 	struct clknode *clknode;
65 	uint32_t cmd_rcgr;
66 	uint32_t hid_width;
67 	uint32_t mnd_width;
68 	int32_t safe_src_idx;
69 	uint32_t cfg_offset;
70 	int safe_pre_parent_idx;
71 	uint32_t flags;
72 	const struct qcom_clk_freq_tbl *freq_tbl;
73 };
74 
75 
76 /*
77  * Finish a clock update.
78  *
79  * This instructs the configuration to take effect.
80  */
81 static bool
82 qcom_clk_rcg2_update_config_locked(struct qcom_clk_rcg2_sc *sc)
83 {
84 	uint32_t reg, count;
85 
86 	/*
87 	 * Send "update" to the controller.
88 	 */
89 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
90 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
91 	reg |= QCOM_CLK_RCG2_CMD_UPDATE;
92 	CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
93 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), reg);
94 	wmb();
95 
96 	/*
97 	 * Poll for completion of update.
98 	 */
99 	for (count = 0; count < 1000; count++) {
100 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
101 		    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
102 		if ((reg & QCOM_CLK_RCG2_CMD_UPDATE) == 0) {
103 			return (true);
104 		}
105 		DELAY(10);
106 		rmb();
107 	}
108 
109 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
110 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
111 	DPRINTF(clknode_get_device(sc->clknode), "%s: failed; reg=0x%08x\n",
112 	    __func__, reg);
113 	return (false);
114 }
115 
116 /*
117  * Calculate the output frequency given an input frequency and the m/n:d
118  * configuration.
119  */
120 static uint64_t
121 qcom_clk_rcg2_calc_rate(uint64_t rate, uint32_t mode, uint32_t m, uint32_t n,
122     uint32_t hid_div)
123 {
124 	if (hid_div != 0) {
125 		rate = rate * 2;
126 		rate = rate / (hid_div + 1);
127 	}
128 
129 	/* Note: assume n is not 0 here; bad things happen if it is */
130 
131 	if (mode != 0) {
132 		rate = (rate * m) / n;
133 	}
134 
135 	return (rate);
136 }
137 
138 /*
139  * The inverse of calc_rate() - calculate the required input frequency
140  * given the desired output freqency and m/n:d configuration.
141  */
142 static uint64_t
143 qcom_clk_rcg2_calc_input_freq(uint64_t freq, uint32_t m, uint32_t n,
144     uint32_t hid_div)
145 {
146 	if (hid_div != 0) {
147 		freq = freq / 2;
148 		freq = freq * (hid_div + 1);
149 	}
150 
151 	if (n != 0) {
152 		freq = (freq * n) / m;
153 	}
154 
155 	return (freq);
156 }
157 
158 static int
159 qcom_clk_rcg2_recalc(struct clknode *clk, uint64_t *freq)
160 {
161 	struct qcom_clk_rcg2_sc *sc;
162 	uint32_t cfg, m = 0, n = 0, hid_div = 0;
163 	uint32_t mode = 0, mask;
164 
165 	sc = clknode_get_softc(clk);
166 
167 	/* Read the MODE, CFG, M and N parameters */
168 	CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
169 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
170 	    QCOM_CLK_RCG2_CFG_OFFSET(sc),
171 	    &cfg);
172 	if (sc->mnd_width != 0) {
173 		mask = (1U << sc->mnd_width) - 1;
174 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
175 		    QCOM_CLK_RCG2_M_OFFSET(sc), &m);
176 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
177 		    QCOM_CLK_RCG2_N_OFFSET(sc), &n);
178 		m = m & mask;
179 		n = ~ n;
180 		n = n & mask;
181 		n = n + m;
182 		mode = (cfg & QCOM_CLK_RCG2_CFG_MODE_MASK)
183 		    >> QCOM_CLK_RCG2_CFG_MODE_SHIFT;
184 	}
185 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
186 
187 	/* Fetch the divisor */
188 	mask = (1U << sc->hid_width) - 1;
189 	hid_div = (cfg >> QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT) & mask;
190 
191 	/* Calculate the rate based on the parent rate and config */
192 	*freq = qcom_clk_rcg2_calc_rate(*freq, mode, m, n, hid_div);
193 
194 	return (0);
195 }
196 
197 /*
198  * configure the mn:d divisor, pre-divisor, and parent.
199  */
200 static void
201 qcom_clk_rcg2_set_config_locked(struct qcom_clk_rcg2_sc *sc,
202     const struct qcom_clk_freq_tbl *f, int parent_idx)
203 {
204 	uint32_t mask, reg;
205 
206 	/* If we have MN:D, then update it */
207 	if (sc->mnd_width != 0 && f->n != 0) {
208 		mask = (1U << sc->mnd_width) - 1;
209 
210 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
211 		    QCOM_CLK_RCG2_M_OFFSET(sc), &reg);
212 		reg &= ~mask;
213 		reg |= (f->m & mask);
214 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
215 		    QCOM_CLK_RCG2_M_OFFSET(sc), reg);
216 
217 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
218 		    QCOM_CLK_RCG2_N_OFFSET(sc), &reg);
219 		reg &= ~mask;
220 		reg |= ((~(f->n - f->m)) & mask);
221 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
222 		    QCOM_CLK_RCG2_N_OFFSET(sc), reg);
223 
224 		CLKDEV_READ_4(clknode_get_device(sc->clknode),
225 		    QCOM_CLK_RCG2_D_OFFSET(sc), &reg);
226 		reg &= ~mask;
227 		reg |= ((~f->n) & mask);
228 		CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
229 		    QCOM_CLK_RCG2_D_OFFSET(sc), reg);
230 	}
231 
232 	mask = (1U << sc->hid_width) - 1;
233 	/*
234 	 * Mask out register fields we're going to modify along with
235 	 * the pre-divisor.
236 	 */
237 	mask |= QCOM_CLK_RCG2_CFG_SRC_SEL_MASK
238 	    | QCOM_CLK_RCG2_CFG_MODE_MASK
239 	    | QCOM_CLK_RCG2_CFG_HW_CLK_CTRL_MASK;
240 
241 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
242 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
243 	reg &= ~mask;
244 
245 	/* Configure pre-divisor */
246 	reg = reg | ((f->pre_div) << QCOM_CLK_RCG2_CFG_SRC_DIV_SHIFT);
247 
248 	/* Configure parent clock */
249 	reg = reg | (((parent_idx << QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT)
250 	    & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK));
251 
252 	/* Configure dual-edge if needed */
253 	if (sc->mnd_width != 0 && f->n != 0 && (f->m != f->n))
254 		reg |= QCOM_CLK_RCG2_CFG_MODE_DUAL_EDGE;
255 
256 	CLKDEV_WRITE_4(clknode_get_device(sc->clknode),
257 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), reg);
258 }
259 
260 static int
261 qcom_clk_rcg2_init(struct clknode *clk, device_t dev)
262 {
263 	struct qcom_clk_rcg2_sc *sc;
264 	uint32_t reg;
265 	uint32_t idx;
266 	bool enabled __unused;
267 
268 	sc = clknode_get_softc(clk);
269 
270 	/*
271 	 * Read the mux setting to set the right parent.
272 	 * Whilst here, read the config to get whether we're enabled
273 	 * or not.
274 	 */
275 	CLKDEV_DEVICE_LOCK(clknode_get_device(sc->clknode));
276 	/* check if rcg2 root clock is enabled */
277 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
278 	    QCOM_CLK_RCG2_CMD_REGISTER(sc), &reg);
279 	if (reg & QCOM_CLK_RCG2_CMD_ROOT_OFF)
280 		enabled = false;
281 	else
282 		enabled = true;
283 
284 	/* mux settings */
285 	CLKDEV_READ_4(clknode_get_device(sc->clknode),
286 	    QCOM_CLK_RCG2_CFG_OFFSET(sc), &reg);
287 	CLKDEV_DEVICE_UNLOCK(clknode_get_device(sc->clknode));
288 
289 	idx = (reg & QCOM_CLK_RCG2_CFG_SRC_SEL_MASK)
290 	    >> QCOM_CLK_RCG2_CFG_SRC_SEL_SHIFT;
291 	DPRINTF(clknode_get_device(sc->clknode),
292 	    "%s: mux index %u, enabled=%d\n",
293 	    __func__, idx, enabled);
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: requested 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