xref: /freebsd/sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c (revision c66ec88fed842fbaad62c30d510644ceb7bd2d71)
1 /*-
2  * Copyright (C) 2013-2015 Daisuke Aoyama <aoyama@peach.ne.jp>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  *
14  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
15  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
16  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
17  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
18  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
19  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
20  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
21  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
22  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
23  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
24  * SUCH DAMAGE.
25  *
26  */
27 
28 #include <sys/cdefs.h>
29 __FBSDID("$FreeBSD$");
30 
31 #include <sys/param.h>
32 #include <sys/systm.h>
33 #include <sys/bus.h>
34 #include <sys/cpu.h>
35 #include <sys/kernel.h>
36 #include <sys/lock.h>
37 #include <sys/malloc.h>
38 #include <sys/module.h>
39 #include <sys/mutex.h>
40 #include <sys/sema.h>
41 #include <sys/sysctl.h>
42 
43 #include <machine/bus.h>
44 #include <machine/cpu.h>
45 #include <machine/intr.h>
46 
47 #include <dev/ofw/ofw_bus.h>
48 #include <dev/ofw/ofw_bus_subr.h>
49 
50 #include <arm/broadcom/bcm2835/bcm2835_firmware.h>
51 #include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
52 
53 #include "cpufreq_if.h"
54 
55 #ifdef DEBUG
56 #define DPRINTF(fmt, ...) do {			\
57 	printf("%s:%u: ", __func__, __LINE__);	\
58 	printf(fmt, ##__VA_ARGS__);		\
59 } while (0)
60 #else
61 #define DPRINTF(fmt, ...)
62 #endif
63 
64 #define	HZ2MHZ(freq) ((freq) / (1000 * 1000))
65 #define	MHZ2HZ(freq) ((freq) * (1000 * 1000))
66 
67 #ifdef SOC_BCM2835
68 #define	OFFSET2MVOLT(val) (1200 + ((val) * 25))
69 #define	MVOLT2OFFSET(val) (((val) - 1200) / 25)
70 #define	DEFAULT_ARM_FREQUENCY	 700
71 #define	DEFAULT_LOWEST_FREQ	 300
72 #else
73 #define	OFFSET2MVOLT(val) (((val) / 1000))
74 #define	MVOLT2OFFSET(val) (((val) * 1000))
75 #define	DEFAULT_ARM_FREQUENCY	 600
76 #define	DEFAULT_LOWEST_FREQ	 600
77 #endif
78 #define	DEFAULT_CORE_FREQUENCY	 250
79 #define	DEFAULT_SDRAM_FREQUENCY	 400
80 #define	TRANSITION_LATENCY	1000
81 #define	MIN_OVER_VOLTAGE	 -16
82 #define	MAX_OVER_VOLTAGE	   6
83 #define	MSG_ERROR	  -999999999
84 #define	MHZSTEP			 100
85 #define	HZSTEP	   (MHZ2HZ(MHZSTEP))
86 #define	TZ_ZEROC		2731
87 
88 #define VC_LOCK(sc) do {			\
89 		sema_wait(&vc_sema);		\
90 	} while (0)
91 #define VC_UNLOCK(sc) do {			\
92 		sema_post(&vc_sema);		\
93 	} while (0)
94 
95 /* ARM->VC mailbox property semaphore */
96 static struct sema vc_sema;
97 
98 static struct sysctl_ctx_list bcm2835_sysctl_ctx;
99 
100 struct bcm2835_cpufreq_softc {
101 	device_t	dev;
102 	device_t	firmware;
103 	int		arm_max_freq;
104 	int		arm_min_freq;
105 	int		core_max_freq;
106 	int		core_min_freq;
107 	int		sdram_max_freq;
108 	int		sdram_min_freq;
109 	int		max_voltage_core;
110 	int		min_voltage_core;
111 
112 	/* the values written in mbox */
113 	int		voltage_core;
114 	int		voltage_sdram;
115 	int		voltage_sdram_c;
116 	int		voltage_sdram_i;
117 	int		voltage_sdram_p;
118 	int		turbo_mode;
119 
120 	/* initial hook for waiting mbox intr */
121 	struct intr_config_hook	init_hook;
122 };
123 
124 static struct ofw_compat_data compat_data[] = {
125 	{ "broadcom,bcm2835-vc",	1 },
126 	{ "broadcom,bcm2708-vc",	1 },
127 	{ "brcm,bcm2709",	1 },
128 	{ "brcm,bcm2835",	1 },
129 	{ "brcm,bcm2836",	1 },
130 	{ "brcm,bcm2837",	1 },
131 	{ "brcm,bcm2711",	1 },
132 	{ NULL, 0 }
133 };
134 
135 static int cpufreq_verbose = 0;
136 TUNABLE_INT("hw.bcm2835.cpufreq.verbose", &cpufreq_verbose);
137 static int cpufreq_lowest_freq = DEFAULT_LOWEST_FREQ;
138 TUNABLE_INT("hw.bcm2835.cpufreq.lowest_freq", &cpufreq_lowest_freq);
139 
140 #ifdef PROP_DEBUG
141 static void
142 bcm2835_dump(const void *data, int len)
143 {
144 	const uint8_t *p = (const uint8_t*)data;
145 	int i;
146 
147 	printf("dump @ %p:\n", data);
148 	for (i = 0; i < len; i++) {
149 		printf("%2.2x ", p[i]);
150 		if ((i % 4) == 3)
151 			printf(" ");
152 		if ((i % 16) == 15)
153 			printf("\n");
154 	}
155 	printf("\n");
156 }
157 #endif
158 
159 static int
160 bcm2835_cpufreq_get_clock_rate(struct bcm2835_cpufreq_softc *sc,
161     uint32_t clock_id)
162 {
163 	union msg_get_clock_rate_body msg;
164 	int rate;
165 	int err;
166 
167 	/*
168 	 * Get clock rate
169 	 *   Tag: 0x00030002
170 	 *   Request:
171 	 *     Length: 4
172 	 *     Value:
173 	 *       u32: clock id
174 	 *   Response:
175 	 *     Length: 8
176 	 *     Value:
177 	 *       u32: clock id
178 	 *       u32: rate (in Hz)
179 	 */
180 
181 	/* setup single tag buffer */
182 	memset(&msg, 0, sizeof(msg));
183 	msg.req.clock_id = clock_id;
184 
185 	/* call mailbox property */
186 	err = bcm2835_firmware_property(sc->firmware,
187 	    BCM2835_FIRMWARE_TAG_GET_CLOCK_RATE, &msg, sizeof(msg));
188 	if (err) {
189 		device_printf(sc->dev, "can't get clock rate (id=%u)\n",
190 		    clock_id);
191 		return (MSG_ERROR);
192 	}
193 
194 	/* result (Hz) */
195 	rate = (int)msg.resp.rate_hz;
196 	DPRINTF("clock = %d(Hz)\n", rate);
197 	return (rate);
198 }
199 
200 static int
201 bcm2835_cpufreq_get_max_clock_rate(struct bcm2835_cpufreq_softc *sc,
202     uint32_t clock_id)
203 {
204 	union msg_get_clock_rate_body msg;
205 	int rate;
206 	int err;
207 
208 	/*
209 	 * Get max clock rate
210 	 *   Tag: 0x00030004
211 	 *   Request:
212 	 *     Length: 4
213 	 *     Value:
214 	 *       u32: clock id
215 	 *   Response:
216 	 *     Length: 8
217 	 *     Value:
218 	 *       u32: clock id
219 	 *       u32: rate (in Hz)
220 	 */
221 
222 	/* setup single tag buffer */
223 	memset(&msg, 0, sizeof(msg));
224 	msg.req.clock_id = clock_id;
225 
226 	/* call mailbox property */
227 	err = bcm2835_firmware_property(sc->firmware,
228 	    BCM2835_FIRMWARE_TAG_GET_MAX_CLOCK_RATE, &msg, sizeof(msg));
229 	if (err) {
230 		device_printf(sc->dev, "can't get max clock rate (id=%u)\n",
231 		    clock_id);
232 		return (MSG_ERROR);
233 	}
234 
235 	/* result (Hz) */
236 	rate = (int)msg.resp.rate_hz;
237 	DPRINTF("clock = %d(Hz)\n", rate);
238 	return (rate);
239 }
240 
241 static int
242 bcm2835_cpufreq_get_min_clock_rate(struct bcm2835_cpufreq_softc *sc,
243     uint32_t clock_id)
244 {
245 	union msg_get_clock_rate_body msg;
246 	int rate;
247 	int err;
248 
249 	/*
250 	 * Get min clock rate
251 	 *   Tag: 0x00030007
252 	 *   Request:
253 	 *     Length: 4
254 	 *     Value:
255 	 *       u32: clock id
256 	 *   Response:
257 	 *     Length: 8
258 	 *     Value:
259 	 *       u32: clock id
260 	 *       u32: rate (in Hz)
261 	 */
262 
263 	/* setup single tag buffer */
264 	memset(&msg, 0, sizeof(msg));
265 	msg.req.clock_id = clock_id;
266 
267 	/* call mailbox property */
268 	err = bcm2835_firmware_property(sc->firmware,
269 	    BCM2835_FIRMWARE_TAG_GET_MIN_CLOCK_RATE, &msg, sizeof(msg));
270 	if (err) {
271 		device_printf(sc->dev, "can't get min clock rate (id=%u)\n",
272 		    clock_id);
273 		return (MSG_ERROR);
274 	}
275 
276 	/* result (Hz) */
277 	rate = (int)msg.resp.rate_hz;
278 	DPRINTF("clock = %d(Hz)\n", rate);
279 	return (rate);
280 }
281 
282 static int
283 bcm2835_cpufreq_set_clock_rate(struct bcm2835_cpufreq_softc *sc,
284     uint32_t clock_id, uint32_t rate_hz)
285 {
286 	union msg_set_clock_rate_body msg;
287 	int rate;
288 	int err;
289 
290 	/*
291 	 * Set clock rate
292 	 *   Tag: 0x00038002
293 	 *   Request:
294 	 *     Length: 8
295 	 *     Value:
296 	 *       u32: clock id
297 	 *       u32: rate (in Hz)
298 	 *   Response:
299 	 *     Length: 8
300 	 *     Value:
301 	 *       u32: clock id
302 	 *       u32: rate (in Hz)
303 	 */
304 
305 	/* setup single tag buffer */
306 	memset(&msg, 0, sizeof(msg));
307 	msg.req.clock_id = clock_id;
308 	msg.req.rate_hz = rate_hz;
309 
310 	/* call mailbox property */
311 	err = bcm2835_firmware_property(sc->firmware,
312 	    BCM2835_FIRMWARE_TAG_SET_CLOCK_RATE, &msg, sizeof(msg));
313 	if (err) {
314 		device_printf(sc->dev, "can't set clock rate (id=%u)\n",
315 		    clock_id);
316 		return (MSG_ERROR);
317 	}
318 
319 	/* workaround for core clock */
320 	if (clock_id == BCM2835_FIRMWARE_CLOCK_ID_CORE) {
321 		/* for safety (may change voltage without changing clock) */
322 		DELAY(TRANSITION_LATENCY);
323 
324 		/*
325 		 * XXX: the core clock is unable to change at once,
326 		 * to change certainly, write it twice now.
327 		 */
328 
329 		/* setup single tag buffer */
330 		memset(&msg, 0, sizeof(msg));
331 		msg.req.clock_id = clock_id;
332 		msg.req.rate_hz = rate_hz;
333 
334 		/* call mailbox property */
335 		err = bcm2835_firmware_property(sc->firmware,
336 		    BCM2835_FIRMWARE_TAG_SET_CLOCK_RATE, &msg, sizeof(msg));
337 		if (err) {
338 			device_printf(sc->dev,
339 			    "can't set clock rate (id=%u)\n", clock_id);
340 			return (MSG_ERROR);
341 		}
342 	}
343 
344 	/* result (Hz) */
345 	rate = (int)msg.resp.rate_hz;
346 	DPRINTF("clock = %d(Hz)\n", rate);
347 	return (rate);
348 }
349 
350 static int
351 bcm2835_cpufreq_get_turbo(struct bcm2835_cpufreq_softc *sc)
352 {
353 	union msg_get_turbo_body msg;
354 	int level;
355 	int err;
356 
357 	/*
358 	 * Get turbo
359 	 *   Tag: 0x00030009
360 	 *   Request:
361 	 *     Length: 4
362 	 *     Value:
363 	 *       u32: id
364 	 *   Response:
365 	 *     Length: 8
366 	 *     Value:
367 	 *       u32: id
368 	 *       u32: level
369 	 */
370 
371 	/* setup single tag buffer */
372 	memset(&msg, 0, sizeof(msg));
373 	msg.req.id = 0;
374 
375 	/* call mailbox property */
376 	err = bcm2835_firmware_property(sc->firmware,
377 	    BCM2835_FIRMWARE_TAG_GET_TURBO, &msg, sizeof(msg));
378 	if (err) {
379 		device_printf(sc->dev, "can't get turbo\n");
380 		return (MSG_ERROR);
381 	}
382 
383 	/* result 0=non-turbo, 1=turbo */
384 	level = (int)msg.resp.level;
385 	DPRINTF("level = %d\n", level);
386 	return (level);
387 }
388 
389 static int
390 bcm2835_cpufreq_set_turbo(struct bcm2835_cpufreq_softc *sc, uint32_t level)
391 {
392 	union msg_set_turbo_body msg;
393 	int value;
394 	int err;
395 
396 	/*
397 	 * Set turbo
398 	 *   Tag: 0x00038009
399 	 *   Request:
400 	 *     Length: 8
401 	 *     Value:
402 	 *       u32: id
403 	 *       u32: level
404 	 *   Response:
405 	 *     Length: 8
406 	 *     Value:
407 	 *       u32: id
408 	 *       u32: level
409 	 */
410 
411 	/* replace unknown value to OFF */
412 	if (level != BCM2835_FIRMWARE_TURBO_ON &&
413 	    level != BCM2835_FIRMWARE_TURBO_OFF)
414 		level = BCM2835_FIRMWARE_TURBO_OFF;
415 
416 	/* setup single tag buffer */
417 	memset(&msg, 0, sizeof(msg));
418 	msg.req.id = 0;
419 	msg.req.level = level;
420 
421 	/* call mailbox property */
422 	err = bcm2835_firmware_property(sc->firmware,
423 	    BCM2835_FIRMWARE_TAG_SET_TURBO, &msg, sizeof(msg));
424 	if (err) {
425 		device_printf(sc->dev, "can't set turbo\n");
426 		return (MSG_ERROR);
427 	}
428 
429 	/* result 0=non-turbo, 1=turbo */
430 	value = (int)msg.resp.level;
431 	DPRINTF("level = %d\n", value);
432 	return (value);
433 }
434 
435 static int
436 bcm2835_cpufreq_get_voltage(struct bcm2835_cpufreq_softc *sc,
437     uint32_t voltage_id)
438 {
439 	union msg_get_voltage_body msg;
440 	int value;
441 	int err;
442 
443 	/*
444 	 * Get voltage
445 	 *   Tag: 0x00030003
446 	 *   Request:
447 	 *     Length: 4
448 	 *     Value:
449 	 *       u32: voltage id
450 	 *   Response:
451 	 *     Length: 8
452 	 *     Value:
453 	 *       u32: voltage id
454 	 *       u32: value (offset from 1.2V in units of 0.025V)
455 	 */
456 
457 	/* setup single tag buffer */
458 	memset(&msg, 0, sizeof(msg));
459 	msg.req.voltage_id = voltage_id;
460 
461 	/* call mailbox property */
462 	err = bcm2835_firmware_property(sc->firmware,
463 	    BCM2835_FIRMWARE_TAG_GET_VOLTAGE, &msg, sizeof(msg));
464 	if (err) {
465 		device_printf(sc->dev, "can't get voltage\n");
466 		return (MSG_ERROR);
467 	}
468 
469 	/* result (offset from 1.2V) */
470 	value = (int)msg.resp.value;
471 	DPRINTF("value = %d\n", value);
472 	return (value);
473 }
474 
475 static int
476 bcm2835_cpufreq_get_max_voltage(struct bcm2835_cpufreq_softc *sc,
477     uint32_t voltage_id)
478 {
479 	union msg_get_voltage_body msg;
480 	int value;
481 	int err;
482 
483 	/*
484 	 * Get voltage
485 	 *   Tag: 0x00030005
486 	 *   Request:
487 	 *     Length: 4
488 	 *     Value:
489 	 *       u32: voltage id
490 	 *   Response:
491 	 *     Length: 8
492 	 *     Value:
493 	 *       u32: voltage id
494 	 *       u32: value (offset from 1.2V in units of 0.025V)
495 	 */
496 
497 	/* setup single tag buffer */
498 	memset(&msg, 0, sizeof(msg));
499 	msg.req.voltage_id = voltage_id;
500 
501 	/* call mailbox property */
502 	err = bcm2835_firmware_property(sc->firmware,
503 	    BCM2835_FIRMWARE_TAG_GET_MAX_VOLTAGE, &msg, sizeof(msg));
504 	if (err) {
505 		device_printf(sc->dev, "can't get max voltage\n");
506 		return (MSG_ERROR);
507 	}
508 
509 	/* result (offset from 1.2V) */
510 	value = (int)msg.resp.value;
511 	DPRINTF("value = %d\n", value);
512 	return (value);
513 }
514 static int
515 bcm2835_cpufreq_get_min_voltage(struct bcm2835_cpufreq_softc *sc,
516     uint32_t voltage_id)
517 {
518 	union msg_get_voltage_body msg;
519 	int value;
520 	int err;
521 
522 	/*
523 	 * Get voltage
524 	 *   Tag: 0x00030008
525 	 *   Request:
526 	 *     Length: 4
527 	 *     Value:
528 	 *       u32: voltage id
529 	 *   Response:
530 	 *     Length: 8
531 	 *     Value:
532 	 *       u32: voltage id
533 	 *       u32: value (offset from 1.2V in units of 0.025V)
534 	 */
535 
536 	/* setup single tag buffer */
537 	memset(&msg, 0, sizeof(msg));
538 	msg.req.voltage_id = voltage_id;
539 
540 	/* call mailbox property */
541 	err = bcm2835_firmware_property(sc->firmware,
542 	    BCM2835_FIRMWARE_TAG_GET_MIN_VOLTAGE, &msg, sizeof(msg));
543 	if (err) {
544 		device_printf(sc->dev, "can't get min voltage\n");
545 		return (MSG_ERROR);
546 	}
547 
548 	/* result (offset from 1.2V) */
549 	value = (int)msg.resp.value;
550 	DPRINTF("value = %d\n", value);
551 	return (value);
552 }
553 
554 static int
555 bcm2835_cpufreq_set_voltage(struct bcm2835_cpufreq_softc *sc,
556     uint32_t voltage_id, int32_t value)
557 {
558 	union msg_set_voltage_body msg;
559 	int err;
560 
561 	/*
562 	 * Set voltage
563 	 *   Tag: 0x00038003
564 	 *   Request:
565 	 *     Length: 4
566 	 *     Value:
567 	 *       u32: voltage id
568 	 *       u32: value (offset from 1.2V in units of 0.025V)
569 	 *   Response:
570 	 *     Length: 8
571 	 *     Value:
572 	 *       u32: voltage id
573 	 *       u32: value (offset from 1.2V in units of 0.025V)
574 	 */
575 
576 	/*
577 	 * over_voltage:
578 	 * 0 (1.2 V). Values above 6 are only allowed when force_turbo or
579 	 * current_limit_override are specified (which set the warranty bit).
580 	 */
581 	if (value > MAX_OVER_VOLTAGE || value < MIN_OVER_VOLTAGE) {
582 		/* currently not supported */
583 		device_printf(sc->dev, "not supported voltage: %d\n", value);
584 		return (MSG_ERROR);
585 	}
586 
587 	/* setup single tag buffer */
588 	memset(&msg, 0, sizeof(msg));
589 	msg.req.voltage_id = voltage_id;
590 	msg.req.value = (uint32_t)value;
591 
592 	/* call mailbox property */
593 	err = bcm2835_firmware_property(sc->firmware,
594 	    BCM2835_FIRMWARE_TAG_SET_VOLTAGE, &msg, sizeof(msg));
595 	if (err) {
596 		device_printf(sc->dev, "can't set voltage\n");
597 		return (MSG_ERROR);
598 	}
599 
600 	/* result (offset from 1.2V) */
601 	value = (int)msg.resp.value;
602 	DPRINTF("value = %d\n", value);
603 	return (value);
604 }
605 
606 static int
607 bcm2835_cpufreq_get_temperature(struct bcm2835_cpufreq_softc *sc)
608 {
609 	union msg_get_temperature_body msg;
610 	int value;
611 	int err;
612 
613 	/*
614 	 * Get temperature
615 	 *   Tag: 0x00030006
616 	 *   Request:
617 	 *     Length: 4
618 	 *     Value:
619 	 *       u32: temperature id
620 	 *   Response:
621 	 *     Length: 8
622 	 *     Value:
623 	 *       u32: temperature id
624 	 *       u32: value
625 	 */
626 
627 	/* setup single tag buffer */
628 	memset(&msg, 0, sizeof(msg));
629 	msg.req.temperature_id = 0;
630 
631 	/* call mailbox property */
632 	err = bcm2835_firmware_property(sc->firmware,
633 	    BCM2835_FIRMWARE_TAG_GET_TEMPERATURE, &msg, sizeof(msg));
634 	if (err) {
635 		device_printf(sc->dev, "can't get temperature\n");
636 		return (MSG_ERROR);
637 	}
638 
639 	/* result (temperature of degree C) */
640 	value = (int)msg.resp.value;
641 	DPRINTF("value = %d\n", value);
642 	return (value);
643 }
644 
645 static int
646 sysctl_bcm2835_cpufreq_arm_freq(SYSCTL_HANDLER_ARGS)
647 {
648 	struct bcm2835_cpufreq_softc *sc = arg1;
649 	int val;
650 	int err;
651 
652 	/* get realtime value */
653 	VC_LOCK(sc);
654 	val = bcm2835_cpufreq_get_clock_rate(sc, BCM2835_FIRMWARE_CLOCK_ID_ARM);
655 	VC_UNLOCK(sc);
656 	if (val == MSG_ERROR)
657 		return (EIO);
658 
659 	err = sysctl_handle_int(oidp, &val, 0, req);
660 	if (err || !req->newptr) /* error || read request */
661 		return (err);
662 
663 	/* write request */
664 	VC_LOCK(sc);
665 	err = bcm2835_cpufreq_set_clock_rate(sc, BCM2835_FIRMWARE_CLOCK_ID_ARM,
666 	    val);
667 	VC_UNLOCK(sc);
668 	if (err == MSG_ERROR) {
669 		device_printf(sc->dev, "set clock arm_freq error\n");
670 		return (EIO);
671 	}
672 	DELAY(TRANSITION_LATENCY);
673 
674 	return (0);
675 }
676 
677 static int
678 sysctl_bcm2835_cpufreq_core_freq(SYSCTL_HANDLER_ARGS)
679 {
680 	struct bcm2835_cpufreq_softc *sc = arg1;
681 	int val;
682 	int err;
683 
684 	/* get realtime value */
685 	VC_LOCK(sc);
686 	val = bcm2835_cpufreq_get_clock_rate(sc,
687 	    BCM2835_FIRMWARE_CLOCK_ID_CORE);
688 	VC_UNLOCK(sc);
689 	if (val == MSG_ERROR)
690 		return (EIO);
691 
692 	err = sysctl_handle_int(oidp, &val, 0, req);
693 	if (err || !req->newptr) /* error || read request */
694 		return (err);
695 
696 	/* write request */
697 	VC_LOCK(sc);
698 	err = bcm2835_cpufreq_set_clock_rate(sc, BCM2835_FIRMWARE_CLOCK_ID_CORE,
699 	    val);
700 	if (err == MSG_ERROR) {
701 		VC_UNLOCK(sc);
702 		device_printf(sc->dev, "set clock core_freq error\n");
703 		return (EIO);
704 	}
705 	VC_UNLOCK(sc);
706 	DELAY(TRANSITION_LATENCY);
707 
708 	return (0);
709 }
710 
711 static int
712 sysctl_bcm2835_cpufreq_sdram_freq(SYSCTL_HANDLER_ARGS)
713 {
714 	struct bcm2835_cpufreq_softc *sc = arg1;
715 	int val;
716 	int err;
717 
718 	/* get realtime value */
719 	VC_LOCK(sc);
720 	val = bcm2835_cpufreq_get_clock_rate(sc,
721 	    BCM2835_FIRMWARE_CLOCK_ID_SDRAM);
722 	VC_UNLOCK(sc);
723 	if (val == MSG_ERROR)
724 		return (EIO);
725 
726 	err = sysctl_handle_int(oidp, &val, 0, req);
727 	if (err || !req->newptr) /* error || read request */
728 		return (err);
729 
730 	/* write request */
731 	VC_LOCK(sc);
732 	err = bcm2835_cpufreq_set_clock_rate(sc,
733 	    BCM2835_FIRMWARE_CLOCK_ID_SDRAM, val);
734 	VC_UNLOCK(sc);
735 	if (err == MSG_ERROR) {
736 		device_printf(sc->dev, "set clock sdram_freq error\n");
737 		return (EIO);
738 	}
739 	DELAY(TRANSITION_LATENCY);
740 
741 	return (0);
742 }
743 
744 static int
745 sysctl_bcm2835_cpufreq_turbo(SYSCTL_HANDLER_ARGS)
746 {
747 	struct bcm2835_cpufreq_softc *sc = arg1;
748 	int val;
749 	int err;
750 
751 	/* get realtime value */
752 	VC_LOCK(sc);
753 	val = bcm2835_cpufreq_get_turbo(sc);
754 	VC_UNLOCK(sc);
755 	if (val == MSG_ERROR)
756 		return (EIO);
757 
758 	err = sysctl_handle_int(oidp, &val, 0, req);
759 	if (err || !req->newptr) /* error || read request */
760 		return (err);
761 
762 	/* write request */
763 	if (val > 0)
764 		sc->turbo_mode = BCM2835_FIRMWARE_TURBO_ON;
765 	else
766 		sc->turbo_mode = BCM2835_FIRMWARE_TURBO_OFF;
767 
768 	VC_LOCK(sc);
769 	err = bcm2835_cpufreq_set_turbo(sc, sc->turbo_mode);
770 	VC_UNLOCK(sc);
771 	if (err == MSG_ERROR) {
772 		device_printf(sc->dev, "set turbo error\n");
773 		return (EIO);
774 	}
775 	DELAY(TRANSITION_LATENCY);
776 
777 	return (0);
778 }
779 
780 static int
781 sysctl_bcm2835_cpufreq_voltage_core(SYSCTL_HANDLER_ARGS)
782 {
783 	struct bcm2835_cpufreq_softc *sc = arg1;
784 	int val;
785 	int err;
786 
787 	/* get realtime value */
788 	VC_LOCK(sc);
789 	val = bcm2835_cpufreq_get_voltage(sc, BCM2835_FIRMWARE_VOLTAGE_ID_CORE);
790 	VC_UNLOCK(sc);
791 	if (val == MSG_ERROR)
792 		return (EIO);
793 
794 	err = sysctl_handle_int(oidp, &val, 0, req);
795 	if (err || !req->newptr) /* error || read request */
796 		return (err);
797 
798 	/* write request */
799 	if (val > MAX_OVER_VOLTAGE || val < MIN_OVER_VOLTAGE)
800 		return (EINVAL);
801 	sc->voltage_core = val;
802 
803 	VC_LOCK(sc);
804 	err = bcm2835_cpufreq_set_voltage(sc, BCM2835_FIRMWARE_VOLTAGE_ID_CORE,
805 	    sc->voltage_core);
806 	VC_UNLOCK(sc);
807 	if (err == MSG_ERROR) {
808 		device_printf(sc->dev, "set voltage core error\n");
809 		return (EIO);
810 	}
811 	DELAY(TRANSITION_LATENCY);
812 
813 	return (0);
814 }
815 
816 static int
817 sysctl_bcm2835_cpufreq_voltage_sdram_c(SYSCTL_HANDLER_ARGS)
818 {
819 	struct bcm2835_cpufreq_softc *sc = arg1;
820 	int val;
821 	int err;
822 
823 	/* get realtime value */
824 	VC_LOCK(sc);
825 	val = bcm2835_cpufreq_get_voltage(sc,
826 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C);
827 	VC_UNLOCK(sc);
828 	if (val == MSG_ERROR)
829 		return (EIO);
830 
831 	err = sysctl_handle_int(oidp, &val, 0, req);
832 	if (err || !req->newptr) /* error || read request */
833 		return (err);
834 
835 	/* write request */
836 	if (val > MAX_OVER_VOLTAGE || val < MIN_OVER_VOLTAGE)
837 		return (EINVAL);
838 	sc->voltage_sdram_c = val;
839 
840 	VC_LOCK(sc);
841 	err = bcm2835_cpufreq_set_voltage(sc,
842 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C,
843 	   sc->voltage_sdram_c);
844 	VC_UNLOCK(sc);
845 	if (err == MSG_ERROR) {
846 		device_printf(sc->dev, "set voltage sdram_c error\n");
847 		return (EIO);
848 	}
849 	DELAY(TRANSITION_LATENCY);
850 
851 	return (0);
852 }
853 
854 static int
855 sysctl_bcm2835_cpufreq_voltage_sdram_i(SYSCTL_HANDLER_ARGS)
856 {
857 	struct bcm2835_cpufreq_softc *sc = arg1;
858 	int val;
859 	int err;
860 
861 	/* get realtime value */
862 	VC_LOCK(sc);
863 	val = bcm2835_cpufreq_get_voltage(sc,
864 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I);
865 	VC_UNLOCK(sc);
866 	if (val == MSG_ERROR)
867 		return (EIO);
868 
869 	err = sysctl_handle_int(oidp, &val, 0, req);
870 	if (err || !req->newptr) /* error || read request */
871 		return (err);
872 
873 	/* write request */
874 	if (val > MAX_OVER_VOLTAGE || val < MIN_OVER_VOLTAGE)
875 		return (EINVAL);
876 	sc->voltage_sdram_i = val;
877 
878 	VC_LOCK(sc);
879 	err = bcm2835_cpufreq_set_voltage(sc,
880 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I, sc->voltage_sdram_i);
881 	VC_UNLOCK(sc);
882 	if (err == MSG_ERROR) {
883 		device_printf(sc->dev, "set voltage sdram_i error\n");
884 		return (EIO);
885 	}
886 	DELAY(TRANSITION_LATENCY);
887 
888 	return (0);
889 }
890 
891 static int
892 sysctl_bcm2835_cpufreq_voltage_sdram_p(SYSCTL_HANDLER_ARGS)
893 {
894 	struct bcm2835_cpufreq_softc *sc = arg1;
895 	int val;
896 	int err;
897 
898 	/* get realtime value */
899 	VC_LOCK(sc);
900 	val = bcm2835_cpufreq_get_voltage(sc,
901 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P);
902 	VC_UNLOCK(sc);
903 	if (val == MSG_ERROR)
904 		return (EIO);
905 
906 	err = sysctl_handle_int(oidp, &val, 0, req);
907 	if (err || !req->newptr) /* error || read request */
908 		return (err);
909 
910 	/* write request */
911 	if (val > MAX_OVER_VOLTAGE || val < MIN_OVER_VOLTAGE)
912 		return (EINVAL);
913 	sc->voltage_sdram_p = val;
914 
915 	VC_LOCK(sc);
916 	err = bcm2835_cpufreq_set_voltage(sc,
917 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P, sc->voltage_sdram_p);
918 	VC_UNLOCK(sc);
919 	if (err == MSG_ERROR) {
920 		device_printf(sc->dev, "set voltage sdram_p error\n");
921 		return (EIO);
922 	}
923 	DELAY(TRANSITION_LATENCY);
924 
925 	return (0);
926 }
927 
928 static int
929 sysctl_bcm2835_cpufreq_voltage_sdram(SYSCTL_HANDLER_ARGS)
930 {
931 	struct bcm2835_cpufreq_softc *sc = arg1;
932 	int val;
933 	int err;
934 
935 	/* multiple write only */
936 	if (!req->newptr)
937 		return (EINVAL);
938 	val = 0;
939 	err = sysctl_handle_int(oidp, &val, 0, req);
940 	if (err)
941 		return (err);
942 
943 	/* write request */
944 	if (val > MAX_OVER_VOLTAGE || val < MIN_OVER_VOLTAGE)
945 		return (EINVAL);
946 	sc->voltage_sdram = val;
947 
948 	VC_LOCK(sc);
949 	err = bcm2835_cpufreq_set_voltage(sc,
950 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C, val);
951 	if (err == MSG_ERROR) {
952 		VC_UNLOCK(sc);
953 		device_printf(sc->dev, "set voltage sdram_c error\n");
954 		return (EIO);
955 	}
956 	err = bcm2835_cpufreq_set_voltage(sc,
957 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I, val);
958 	if (err == MSG_ERROR) {
959 		VC_UNLOCK(sc);
960 		device_printf(sc->dev, "set voltage sdram_i error\n");
961 		return (EIO);
962 	}
963 	err = bcm2835_cpufreq_set_voltage(sc,
964 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P, val);
965 	if (err == MSG_ERROR) {
966 		VC_UNLOCK(sc);
967 		device_printf(sc->dev, "set voltage sdram_p error\n");
968 		return (EIO);
969 	}
970 	VC_UNLOCK(sc);
971 	DELAY(TRANSITION_LATENCY);
972 
973 	return (0);
974 }
975 
976 static int
977 sysctl_bcm2835_cpufreq_temperature(SYSCTL_HANDLER_ARGS)
978 {
979 	struct bcm2835_cpufreq_softc *sc = arg1;
980 	int val;
981 	int err;
982 
983 	/* get realtime value */
984 	VC_LOCK(sc);
985 	val = bcm2835_cpufreq_get_temperature(sc);
986 	VC_UNLOCK(sc);
987 	if (val == MSG_ERROR)
988 		return (EIO);
989 
990 	err = sysctl_handle_int(oidp, &val, 0, req);
991 	if (err || !req->newptr) /* error || read request */
992 		return (err);
993 
994 	/* write request */
995 	return (EINVAL);
996 }
997 
998 static int
999 sysctl_bcm2835_devcpu_temperature(SYSCTL_HANDLER_ARGS)
1000 {
1001 	struct bcm2835_cpufreq_softc *sc = arg1;
1002 	int val;
1003 	int err;
1004 
1005 	/* get realtime value */
1006 	VC_LOCK(sc);
1007 	val = bcm2835_cpufreq_get_temperature(sc);
1008 	VC_UNLOCK(sc);
1009 	if (val == MSG_ERROR)
1010 		return (EIO);
1011 
1012 	/* 1/1000 celsius (raw) to 1/10 kelvin */
1013 	val = val / 100 + TZ_ZEROC;
1014 
1015 	err = sysctl_handle_int(oidp, &val, 0, req);
1016 	if (err || !req->newptr) /* error || read request */
1017 		return (err);
1018 
1019 	/* write request */
1020 	return (EINVAL);
1021 }
1022 
1023 static void
1024 bcm2835_cpufreq_init(void *arg)
1025 {
1026 	struct bcm2835_cpufreq_softc *sc = arg;
1027 	struct sysctl_ctx_list *ctx;
1028 	device_t cpu;
1029 	int arm_freq, core_freq, sdram_freq;
1030 	int arm_max_freq, arm_min_freq, core_max_freq, core_min_freq;
1031 	int sdram_max_freq, sdram_min_freq;
1032 	int voltage_core, voltage_sdram_c, voltage_sdram_i, voltage_sdram_p;
1033 	int max_voltage_core, min_voltage_core;
1034 	int max_voltage_sdram_c, min_voltage_sdram_c;
1035 	int max_voltage_sdram_i, min_voltage_sdram_i;
1036 	int max_voltage_sdram_p, min_voltage_sdram_p;
1037 	int turbo, temperature;
1038 
1039 	VC_LOCK(sc);
1040 
1041 	/* current clock */
1042 	arm_freq = bcm2835_cpufreq_get_clock_rate(sc,
1043 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1044 	core_freq = bcm2835_cpufreq_get_clock_rate(sc,
1045 	    BCM2835_FIRMWARE_CLOCK_ID_CORE);
1046 	sdram_freq = bcm2835_cpufreq_get_clock_rate(sc,
1047 	    BCM2835_FIRMWARE_CLOCK_ID_SDRAM);
1048 
1049 	/* max/min clock */
1050 	arm_max_freq = bcm2835_cpufreq_get_max_clock_rate(sc,
1051 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1052 	arm_min_freq = bcm2835_cpufreq_get_min_clock_rate(sc,
1053 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1054 	core_max_freq = bcm2835_cpufreq_get_max_clock_rate(sc,
1055 	    BCM2835_FIRMWARE_CLOCK_ID_CORE);
1056 	core_min_freq = bcm2835_cpufreq_get_min_clock_rate(sc,
1057 	    BCM2835_FIRMWARE_CLOCK_ID_CORE);
1058 	sdram_max_freq = bcm2835_cpufreq_get_max_clock_rate(sc,
1059 	    BCM2835_FIRMWARE_CLOCK_ID_SDRAM);
1060 	sdram_min_freq = bcm2835_cpufreq_get_min_clock_rate(sc,
1061 	    BCM2835_FIRMWARE_CLOCK_ID_SDRAM);
1062 
1063 	/* turbo mode */
1064 	turbo = bcm2835_cpufreq_get_turbo(sc);
1065 	if (turbo > 0)
1066 		sc->turbo_mode = BCM2835_FIRMWARE_TURBO_ON;
1067 	else
1068 		sc->turbo_mode = BCM2835_FIRMWARE_TURBO_OFF;
1069 
1070 	/* voltage */
1071 	voltage_core = bcm2835_cpufreq_get_voltage(sc,
1072 	    BCM2835_FIRMWARE_VOLTAGE_ID_CORE);
1073 	voltage_sdram_c = bcm2835_cpufreq_get_voltage(sc,
1074 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C);
1075 	voltage_sdram_i = bcm2835_cpufreq_get_voltage(sc,
1076 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I);
1077 	voltage_sdram_p = bcm2835_cpufreq_get_voltage(sc,
1078 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P);
1079 
1080 	/* current values (offset from 1.2V) */
1081 	sc->voltage_core = voltage_core;
1082 	sc->voltage_sdram = voltage_sdram_c;
1083 	sc->voltage_sdram_c = voltage_sdram_c;
1084 	sc->voltage_sdram_i = voltage_sdram_i;
1085 	sc->voltage_sdram_p = voltage_sdram_p;
1086 
1087 	/* max/min voltage */
1088 	max_voltage_core = bcm2835_cpufreq_get_max_voltage(sc,
1089 	    BCM2835_FIRMWARE_VOLTAGE_ID_CORE);
1090 	min_voltage_core = bcm2835_cpufreq_get_min_voltage(sc,
1091 	    BCM2835_FIRMWARE_VOLTAGE_ID_CORE);
1092 	max_voltage_sdram_c = bcm2835_cpufreq_get_max_voltage(sc,
1093 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C);
1094 	max_voltage_sdram_i = bcm2835_cpufreq_get_max_voltage(sc,
1095 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I);
1096 	max_voltage_sdram_p = bcm2835_cpufreq_get_max_voltage(sc,
1097 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P);
1098 	min_voltage_sdram_c = bcm2835_cpufreq_get_min_voltage(sc,
1099 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_C);
1100 	min_voltage_sdram_i = bcm2835_cpufreq_get_min_voltage(sc,
1101 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_I);
1102 	min_voltage_sdram_p = bcm2835_cpufreq_get_min_voltage(sc,
1103 	    BCM2835_FIRMWARE_VOLTAGE_ID_SDRAM_P);
1104 
1105 	/* temperature */
1106 	temperature = bcm2835_cpufreq_get_temperature(sc);
1107 
1108 	/* show result */
1109 	if (cpufreq_verbose || bootverbose) {
1110 		device_printf(sc->dev, "Boot settings:\n");
1111 		device_printf(sc->dev,
1112 		    "current ARM %dMHz, Core %dMHz, SDRAM %dMHz, Turbo %s\n",
1113 		    HZ2MHZ(arm_freq), HZ2MHZ(core_freq), HZ2MHZ(sdram_freq),
1114 		    (sc->turbo_mode == BCM2835_FIRMWARE_TURBO_ON) ? "ON":"OFF");
1115 
1116 		device_printf(sc->dev,
1117 		    "max/min ARM %d/%dMHz, Core %d/%dMHz, SDRAM %d/%dMHz\n",
1118 		    HZ2MHZ(arm_max_freq), HZ2MHZ(arm_min_freq),
1119 		    HZ2MHZ(core_max_freq), HZ2MHZ(core_min_freq),
1120 		    HZ2MHZ(sdram_max_freq), HZ2MHZ(sdram_min_freq));
1121 
1122 		device_printf(sc->dev,
1123 		    "current Core %dmV, SDRAM_C %dmV, SDRAM_I %dmV, "
1124 		    "SDRAM_P %dmV\n",
1125 		    OFFSET2MVOLT(voltage_core), OFFSET2MVOLT(voltage_sdram_c),
1126 		    OFFSET2MVOLT(voltage_sdram_i),
1127 		    OFFSET2MVOLT(voltage_sdram_p));
1128 
1129 		device_printf(sc->dev,
1130 		    "max/min Core %d/%dmV, SDRAM_C %d/%dmV, SDRAM_I %d/%dmV, "
1131 		    "SDRAM_P %d/%dmV\n",
1132 		    OFFSET2MVOLT(max_voltage_core),
1133 		    OFFSET2MVOLT(min_voltage_core),
1134 		    OFFSET2MVOLT(max_voltage_sdram_c),
1135 		    OFFSET2MVOLT(min_voltage_sdram_c),
1136 		    OFFSET2MVOLT(max_voltage_sdram_i),
1137 		    OFFSET2MVOLT(min_voltage_sdram_i),
1138 		    OFFSET2MVOLT(max_voltage_sdram_p),
1139 		    OFFSET2MVOLT(min_voltage_sdram_p));
1140 
1141 		device_printf(sc->dev,
1142 		    "Temperature %d.%dC\n", (temperature / 1000),
1143 		    (temperature % 1000) / 100);
1144 	} else { /* !cpufreq_verbose && !bootverbose */
1145 		device_printf(sc->dev,
1146 		    "ARM %dMHz, Core %dMHz, SDRAM %dMHz, Turbo %s\n",
1147 		    HZ2MHZ(arm_freq), HZ2MHZ(core_freq), HZ2MHZ(sdram_freq),
1148 		    (sc->turbo_mode == BCM2835_FIRMWARE_TURBO_ON) ? "ON":"OFF");
1149 	}
1150 
1151 	/* keep in softc (MHz/mV) */
1152 	sc->arm_max_freq = HZ2MHZ(arm_max_freq);
1153 	sc->arm_min_freq = HZ2MHZ(arm_min_freq);
1154 	sc->core_max_freq = HZ2MHZ(core_max_freq);
1155 	sc->core_min_freq = HZ2MHZ(core_min_freq);
1156 	sc->sdram_max_freq = HZ2MHZ(sdram_max_freq);
1157 	sc->sdram_min_freq = HZ2MHZ(sdram_min_freq);
1158 	sc->max_voltage_core = OFFSET2MVOLT(max_voltage_core);
1159 	sc->min_voltage_core = OFFSET2MVOLT(min_voltage_core);
1160 
1161 	/* if turbo is on, set to max values */
1162 	if (sc->turbo_mode == BCM2835_FIRMWARE_TURBO_ON) {
1163 		bcm2835_cpufreq_set_clock_rate(sc,
1164 		    BCM2835_FIRMWARE_CLOCK_ID_ARM, arm_max_freq);
1165 		DELAY(TRANSITION_LATENCY);
1166 		bcm2835_cpufreq_set_clock_rate(sc,
1167 		    BCM2835_FIRMWARE_CLOCK_ID_CORE, core_max_freq);
1168 		DELAY(TRANSITION_LATENCY);
1169 		bcm2835_cpufreq_set_clock_rate(sc,
1170 		    BCM2835_FIRMWARE_CLOCK_ID_SDRAM, sdram_max_freq);
1171 		DELAY(TRANSITION_LATENCY);
1172 	} else {
1173 		bcm2835_cpufreq_set_clock_rate(sc,
1174 		    BCM2835_FIRMWARE_CLOCK_ID_ARM, arm_min_freq);
1175 		DELAY(TRANSITION_LATENCY);
1176 		bcm2835_cpufreq_set_clock_rate(sc,
1177 		    BCM2835_FIRMWARE_CLOCK_ID_CORE, core_min_freq);
1178 		DELAY(TRANSITION_LATENCY);
1179 		bcm2835_cpufreq_set_clock_rate(sc,
1180 		    BCM2835_FIRMWARE_CLOCK_ID_SDRAM, sdram_min_freq);
1181 		DELAY(TRANSITION_LATENCY);
1182 	}
1183 
1184 	VC_UNLOCK(sc);
1185 
1186 	/* add human readable temperature to dev.cpu node */
1187 	cpu = device_get_parent(sc->dev);
1188 	if (cpu != NULL) {
1189 		ctx = device_get_sysctl_ctx(cpu);
1190 		SYSCTL_ADD_PROC(ctx,
1191 		    SYSCTL_CHILDREN(device_get_sysctl_tree(cpu)), OID_AUTO,
1192 		    "temperature",
1193 		    CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_NEEDGIANT, sc, 0,
1194 		    sysctl_bcm2835_devcpu_temperature, "IK",
1195 		    "Current SoC temperature");
1196 	}
1197 
1198 	/* release this hook (continue boot) */
1199 	config_intrhook_disestablish(&sc->init_hook);
1200 }
1201 
1202 static void
1203 bcm2835_cpufreq_identify(driver_t *driver, device_t parent)
1204 {
1205 	const struct ofw_compat_data *compat;
1206 	phandle_t root;
1207 
1208 	root = OF_finddevice("/");
1209 	for (compat = compat_data; compat->ocd_str != NULL; compat++)
1210 		if (ofw_bus_node_is_compatible(root, compat->ocd_str))
1211 			break;
1212 
1213 	if (compat->ocd_data == 0)
1214 		return;
1215 
1216 	DPRINTF("driver=%p, parent=%p\n", driver, parent);
1217 	if (device_find_child(parent, "bcm2835_cpufreq", -1) != NULL)
1218 		return;
1219 	if (BUS_ADD_CHILD(parent, 0, "bcm2835_cpufreq", -1) == NULL)
1220 		device_printf(parent, "add child failed\n");
1221 }
1222 
1223 static int
1224 bcm2835_cpufreq_probe(device_t dev)
1225 {
1226 
1227 	if (device_get_unit(dev) != 0)
1228 		return (ENXIO);
1229 	device_set_desc(dev, "CPU Frequency Control");
1230 
1231 	return (0);
1232 }
1233 
1234 static int
1235 bcm2835_cpufreq_attach(device_t dev)
1236 {
1237 	struct bcm2835_cpufreq_softc *sc;
1238 	struct sysctl_oid *oid;
1239 
1240 	/* set self dev */
1241 	sc = device_get_softc(dev);
1242 	sc->dev = dev;
1243 	sc->firmware = devclass_get_device(
1244 	    devclass_find("bcm2835_firmware"), 0);
1245 	if (sc->firmware == NULL) {
1246 		device_printf(dev, "Unable to find firmware device\n");
1247 		return (ENXIO);
1248 	}
1249 
1250 	/* initial values */
1251 	sc->arm_max_freq = -1;
1252 	sc->arm_min_freq = -1;
1253 	sc->core_max_freq = -1;
1254 	sc->core_min_freq = -1;
1255 	sc->sdram_max_freq = -1;
1256 	sc->sdram_min_freq = -1;
1257 	sc->max_voltage_core = 0;
1258 	sc->min_voltage_core = 0;
1259 
1260 	/* setup sysctl at first device */
1261 	if (device_get_unit(dev) == 0) {
1262 		sysctl_ctx_init(&bcm2835_sysctl_ctx);
1263 		/* create node for hw.cpufreq */
1264 		oid = SYSCTL_ADD_NODE(&bcm2835_sysctl_ctx,
1265 		    SYSCTL_STATIC_CHILDREN(_hw), OID_AUTO, "cpufreq",
1266 		    CTLFLAG_RD | CTLFLAG_MPSAFE, NULL, "");
1267 
1268 		/* Frequency (Hz) */
1269 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1270 		    OID_AUTO, "arm_freq",
1271 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc, 0,
1272 		    sysctl_bcm2835_cpufreq_arm_freq, "IU",
1273 		    "ARM frequency (Hz)");
1274 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1275 		    OID_AUTO, "core_freq",
1276 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc, 0,
1277 		    sysctl_bcm2835_cpufreq_core_freq, "IU",
1278 		    "Core frequency (Hz)");
1279 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1280 		    OID_AUTO, "sdram_freq",
1281 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc, 0,
1282 		    sysctl_bcm2835_cpufreq_sdram_freq, "IU",
1283 		    "SDRAM frequency (Hz)");
1284 
1285 		/* Turbo state */
1286 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1287 		    OID_AUTO, "turbo",
1288 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc, 0,
1289 		    sysctl_bcm2835_cpufreq_turbo, "IU",
1290 		    "Disables dynamic clocking");
1291 
1292 		/* Voltage (offset from 1.2V in units of 0.025V) */
1293 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1294 		    OID_AUTO, "voltage_core",
1295 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc, 0,
1296 		    sysctl_bcm2835_cpufreq_voltage_core, "I",
1297 		    "ARM/GPU core voltage"
1298 		    "(offset from 1.2V in units of 0.025V)");
1299 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1300 		    OID_AUTO, "voltage_sdram",
1301 		    CTLTYPE_INT | CTLFLAG_WR | CTLFLAG_NEEDGIANT, sc,
1302 		    0, sysctl_bcm2835_cpufreq_voltage_sdram, "I",
1303 		    "SDRAM voltage (offset from 1.2V in units of 0.025V)");
1304 
1305 		/* Voltage individual SDRAM */
1306 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1307 		    OID_AUTO, "voltage_sdram_c",
1308 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc,
1309 		    0, sysctl_bcm2835_cpufreq_voltage_sdram_c, "I",
1310 		    "SDRAM controller voltage"
1311 		    "(offset from 1.2V in units of 0.025V)");
1312 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1313 		    OID_AUTO, "voltage_sdram_i",
1314 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc,
1315 		    0, sysctl_bcm2835_cpufreq_voltage_sdram_i, "I",
1316 		    "SDRAM I/O voltage (offset from 1.2V in units of 0.025V)");
1317 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1318 		    OID_AUTO, "voltage_sdram_p",
1319 		    CTLTYPE_INT | CTLFLAG_RW | CTLFLAG_NEEDGIANT, sc,
1320 		    0, sysctl_bcm2835_cpufreq_voltage_sdram_p, "I",
1321 		    "SDRAM phy voltage (offset from 1.2V in units of 0.025V)");
1322 
1323 		/* Temperature */
1324 		SYSCTL_ADD_PROC(&bcm2835_sysctl_ctx, SYSCTL_CHILDREN(oid),
1325 		    OID_AUTO, "temperature",
1326 		    CTLTYPE_INT | CTLFLAG_RD | CTLFLAG_NEEDGIANT, sc, 0,
1327 		    sysctl_bcm2835_cpufreq_temperature, "I",
1328 		    "SoC temperature (thousandths of a degree C)");
1329 	}
1330 
1331 	/* ARM->VC lock */
1332 	sema_init(&vc_sema, 1, "vcsema");
1333 
1334 	/* register callback for using mbox when interrupts are enabled */
1335 	sc->init_hook.ich_func = bcm2835_cpufreq_init;
1336 	sc->init_hook.ich_arg = sc;
1337 
1338 	if (config_intrhook_establish(&sc->init_hook) != 0) {
1339 		device_printf(dev, "config_intrhook_establish failed\n");
1340 		return (ENOMEM);
1341 	}
1342 
1343 	/* this device is controlled by cpufreq(4) */
1344 	cpufreq_register(dev);
1345 
1346 	return (0);
1347 }
1348 
1349 static int
1350 bcm2835_cpufreq_detach(device_t dev)
1351 {
1352 
1353 	sema_destroy(&vc_sema);
1354 
1355 	return (cpufreq_unregister(dev));
1356 }
1357 
1358 static int
1359 bcm2835_cpufreq_set(device_t dev, const struct cf_setting *cf)
1360 {
1361 	struct bcm2835_cpufreq_softc *sc;
1362 	uint32_t rate_hz, rem;
1363 	int resp_freq, arm_freq, min_freq, core_freq;
1364 #ifdef DEBUG
1365 	int cur_freq;
1366 #endif
1367 
1368 	if (cf == NULL || cf->freq < 0)
1369 		return (EINVAL);
1370 
1371 	sc = device_get_softc(dev);
1372 
1373 	/* setting clock (Hz) */
1374 	rate_hz = (uint32_t)MHZ2HZ(cf->freq);
1375 	rem = rate_hz % HZSTEP;
1376 	rate_hz -= rem;
1377 	if (rate_hz == 0)
1378 		return (EINVAL);
1379 
1380 	/* adjust min freq */
1381 	min_freq = sc->arm_min_freq;
1382 	if (sc->turbo_mode != BCM2835_FIRMWARE_TURBO_ON)
1383 		if (min_freq > cpufreq_lowest_freq)
1384 			min_freq = cpufreq_lowest_freq;
1385 
1386 	if (rate_hz < MHZ2HZ(min_freq) || rate_hz > MHZ2HZ(sc->arm_max_freq))
1387 		return (EINVAL);
1388 
1389 	/* set new value and verify it */
1390 	VC_LOCK(sc);
1391 #ifdef DEBUG
1392 	cur_freq = bcm2835_cpufreq_get_clock_rate(sc,
1393 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1394 #endif
1395 	resp_freq = bcm2835_cpufreq_set_clock_rate(sc,
1396 	    BCM2835_FIRMWARE_CLOCK_ID_ARM, rate_hz);
1397 	DELAY(TRANSITION_LATENCY);
1398 	arm_freq = bcm2835_cpufreq_get_clock_rate(sc,
1399 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1400 
1401 	/*
1402 	 * if non-turbo and lower than or equal min_freq,
1403 	 * clock down core and sdram to default first.
1404 	 */
1405 	if (sc->turbo_mode != BCM2835_FIRMWARE_TURBO_ON) {
1406 		core_freq = bcm2835_cpufreq_get_clock_rate(sc,
1407 		    BCM2835_FIRMWARE_CLOCK_ID_CORE);
1408 		if (rate_hz > MHZ2HZ(sc->arm_min_freq)) {
1409 			bcm2835_cpufreq_set_clock_rate(sc,
1410 			    BCM2835_FIRMWARE_CLOCK_ID_CORE,
1411 			    MHZ2HZ(sc->core_max_freq));
1412 			DELAY(TRANSITION_LATENCY);
1413 			bcm2835_cpufreq_set_clock_rate(sc,
1414 			    BCM2835_FIRMWARE_CLOCK_ID_SDRAM,
1415 			    MHZ2HZ(sc->sdram_max_freq));
1416 			DELAY(TRANSITION_LATENCY);
1417 		} else {
1418 			if (sc->core_min_freq < DEFAULT_CORE_FREQUENCY &&
1419 			    core_freq > DEFAULT_CORE_FREQUENCY) {
1420 				/* first, down to 250, then down to min */
1421 				DELAY(TRANSITION_LATENCY);
1422 				bcm2835_cpufreq_set_clock_rate(sc,
1423 				    BCM2835_FIRMWARE_CLOCK_ID_CORE,
1424 				    MHZ2HZ(DEFAULT_CORE_FREQUENCY));
1425 				DELAY(TRANSITION_LATENCY);
1426 				/* reset core voltage */
1427 				bcm2835_cpufreq_set_voltage(sc,
1428 				    BCM2835_FIRMWARE_VOLTAGE_ID_CORE, 0);
1429 				DELAY(TRANSITION_LATENCY);
1430 			}
1431 			bcm2835_cpufreq_set_clock_rate(sc,
1432 			    BCM2835_FIRMWARE_CLOCK_ID_CORE,
1433 			    MHZ2HZ(sc->core_min_freq));
1434 			DELAY(TRANSITION_LATENCY);
1435 			bcm2835_cpufreq_set_clock_rate(sc,
1436 			    BCM2835_FIRMWARE_CLOCK_ID_SDRAM,
1437 			    MHZ2HZ(sc->sdram_min_freq));
1438 			DELAY(TRANSITION_LATENCY);
1439 		}
1440 	}
1441 
1442 	VC_UNLOCK(sc);
1443 
1444 	if (resp_freq < 0 || arm_freq < 0 || resp_freq != arm_freq) {
1445 		device_printf(dev, "wrong freq\n");
1446 		return (EIO);
1447 	}
1448 	DPRINTF("cpufreq: %d -> %d\n", cur_freq, arm_freq);
1449 
1450 	return (0);
1451 }
1452 
1453 static int
1454 bcm2835_cpufreq_get(device_t dev, struct cf_setting *cf)
1455 {
1456 	struct bcm2835_cpufreq_softc *sc;
1457 	int arm_freq;
1458 
1459 	if (cf == NULL)
1460 		return (EINVAL);
1461 
1462 	sc = device_get_softc(dev);
1463 	memset(cf, CPUFREQ_VAL_UNKNOWN, sizeof(*cf));
1464 	cf->dev = NULL;
1465 
1466 	/* get cuurent value */
1467 	VC_LOCK(sc);
1468 	arm_freq = bcm2835_cpufreq_get_clock_rate(sc,
1469 	    BCM2835_FIRMWARE_CLOCK_ID_ARM);
1470 	VC_UNLOCK(sc);
1471 	if (arm_freq < 0) {
1472 		device_printf(dev, "can't get clock\n");
1473 		return (EINVAL);
1474 	}
1475 
1476 	/* CPU clock in MHz or 100ths of a percent. */
1477 	cf->freq = HZ2MHZ(arm_freq);
1478 	/* Voltage in mV. */
1479 	cf->volts = CPUFREQ_VAL_UNKNOWN;
1480 	/* Power consumed in mW. */
1481 	cf->power = CPUFREQ_VAL_UNKNOWN;
1482 	/* Transition latency in us. */
1483 	cf->lat = TRANSITION_LATENCY;
1484 	/* Driver providing this setting. */
1485 	cf->dev = dev;
1486 
1487 	return (0);
1488 }
1489 
1490 static int
1491 bcm2835_cpufreq_make_freq_list(device_t dev, struct cf_setting *sets,
1492     int *count)
1493 {
1494 	struct bcm2835_cpufreq_softc *sc;
1495 	int freq, min_freq, volts, rem;
1496 	int idx;
1497 
1498 	sc = device_get_softc(dev);
1499 	freq = sc->arm_max_freq;
1500 	min_freq = sc->arm_min_freq;
1501 
1502 	/* adjust head freq to STEP */
1503 	rem = freq % MHZSTEP;
1504 	freq -= rem;
1505 	if (freq < min_freq)
1506 		freq = min_freq;
1507 
1508 	/* if non-turbo, add extra low freq */
1509 	if (sc->turbo_mode != BCM2835_FIRMWARE_TURBO_ON)
1510 		if (min_freq > cpufreq_lowest_freq)
1511 			min_freq = cpufreq_lowest_freq;
1512 
1513 #ifdef SOC_BCM2835
1514 	/* from freq to min_freq */
1515 	for (idx = 0; idx < *count && freq >= min_freq; idx++) {
1516 		if (freq > sc->arm_min_freq)
1517 			volts = sc->max_voltage_core;
1518 		else
1519 			volts = sc->min_voltage_core;
1520 		sets[idx].freq = freq;
1521 		sets[idx].volts = volts;
1522 		sets[idx].lat = TRANSITION_LATENCY;
1523 		sets[idx].dev = dev;
1524 		freq -= MHZSTEP;
1525 	}
1526 #else
1527 	/* XXX RPi2 have only 900/600MHz */
1528 	idx = 0;
1529 	volts = sc->min_voltage_core;
1530 	sets[idx].freq = freq;
1531 	sets[idx].volts = volts;
1532 	sets[idx].lat = TRANSITION_LATENCY;
1533 	sets[idx].dev = dev;
1534 	idx++;
1535 	if (freq != min_freq) {
1536 		sets[idx].freq = min_freq;
1537 		sets[idx].volts = volts;
1538 		sets[idx].lat = TRANSITION_LATENCY;
1539 		sets[idx].dev = dev;
1540 		idx++;
1541 	}
1542 #endif
1543 	*count = idx;
1544 
1545 	return (0);
1546 }
1547 
1548 static int
1549 bcm2835_cpufreq_settings(device_t dev, struct cf_setting *sets, int *count)
1550 {
1551 	struct bcm2835_cpufreq_softc *sc;
1552 
1553 	if (sets == NULL || count == NULL)
1554 		return (EINVAL);
1555 
1556 	sc = device_get_softc(dev);
1557 	if (sc->arm_min_freq < 0 || sc->arm_max_freq < 0) {
1558 		printf("device is not configured\n");
1559 		return (EINVAL);
1560 	}
1561 
1562 	/* fill data with unknown value */
1563 	memset(sets, CPUFREQ_VAL_UNKNOWN, sizeof(*sets) * (*count));
1564 	/* create new array up to count */
1565 	bcm2835_cpufreq_make_freq_list(dev, sets, count);
1566 
1567 	return (0);
1568 }
1569 
1570 static int
1571 bcm2835_cpufreq_type(device_t dev, int *type)
1572 {
1573 
1574 	if (type == NULL)
1575 		return (EINVAL);
1576 	*type = CPUFREQ_TYPE_ABSOLUTE;
1577 
1578 	return (0);
1579 }
1580 
1581 static device_method_t bcm2835_cpufreq_methods[] = {
1582 	/* Device interface */
1583 	DEVMETHOD(device_identify,	bcm2835_cpufreq_identify),
1584 	DEVMETHOD(device_probe,		bcm2835_cpufreq_probe),
1585 	DEVMETHOD(device_attach,	bcm2835_cpufreq_attach),
1586 	DEVMETHOD(device_detach,	bcm2835_cpufreq_detach),
1587 
1588 	/* cpufreq interface */
1589 	DEVMETHOD(cpufreq_drv_set,	bcm2835_cpufreq_set),
1590 	DEVMETHOD(cpufreq_drv_get,	bcm2835_cpufreq_get),
1591 	DEVMETHOD(cpufreq_drv_settings,	bcm2835_cpufreq_settings),
1592 	DEVMETHOD(cpufreq_drv_type,	bcm2835_cpufreq_type),
1593 
1594 	DEVMETHOD_END
1595 };
1596 
1597 static devclass_t bcm2835_cpufreq_devclass;
1598 static driver_t bcm2835_cpufreq_driver = {
1599 	"bcm2835_cpufreq",
1600 	bcm2835_cpufreq_methods,
1601 	sizeof(struct bcm2835_cpufreq_softc),
1602 };
1603 
1604 DRIVER_MODULE(bcm2835_cpufreq, cpu, bcm2835_cpufreq_driver,
1605     bcm2835_cpufreq_devclass, 0, 0);
1606 MODULE_DEPEND(bcm2835_cpufreq, bcm2835_firmware, 1, 1, 1);
1607