sys/arm/broadcom/bcm2835/bcm2835_cpufreq.c

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