sys/dev/cpufreq/cpufreq_dt.c

   1 /*-
   2  * Copyright (c) 2016 Jared McNeill <jmcneill@invisible.ca>
   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 ``AS IS'' AND ANY EXPRESS OR
  15  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  16  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  17  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  18  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  19  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  20  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
  21  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  22  * 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  * $FreeBSD$
  27  */
  28
  29 /*
  30  * Generic DT based cpufreq driver
  31  */
  32
  33 #include <sys/cdefs.h>
  34 __FBSDID("$FreeBSD$");
  35
  36 #include <sys/param.h>
  37 #include <sys/systm.h>
  38 #include <sys/bus.h>
  39 #include <sys/rman.h>
  40 #include <sys/kernel.h>
  41 #include <sys/module.h>
  42 #include <sys/cpu.h>
  43 #include <sys/cpuset.h>
  44 #include <sys/smp.h>
  45
  46 #include <dev/ofw/ofw_bus.h>
  47 #include <dev/ofw/ofw_bus_subr.h>
  48
  49 #include <dev/extres/clk/clk.h>
  50 #include <dev/extres/regulator/regulator.h>
  51
  52 #include "cpufreq_if.h"
  53
  54 struct cpufreq_dt_opp {
  55         uint32_t        freq_khz;
  56         uint32_t        voltage_uv;
  57 };
  58
  59 struct cpufreq_dt_softc {
  60         clk_t clk;
  61         regulator_t reg;
  62
  63         struct cpufreq_dt_opp *opp;
  64         ssize_t nopp;
  65         int clk_latency;
  66
  67         cpuset_t cpus;
  68 };
  69
  70 static void
  71 cpufreq_dt_notify(device_t dev, uint64_t freq)
  72 {
  73 #ifdef __aarch64__
  74         struct cpufreq_dt_softc *sc;
  75         struct pcpu *pc;
  76         int cpu;
  77
  78         sc = device_get_softc(dev);
  79
  80         CPU_FOREACH(cpu) {
  81                 if (CPU_ISSET(cpu, &sc->cpus)) {
  82                         pc = pcpu_find(cpu);
  83                         pc->pc_clock = freq;
  84                 }
  85         }
  86 #endif
  87 }
  88
  89 static const struct cpufreq_dt_opp *
  90 cpufreq_dt_find_opp(device_t dev, uint32_t freq_mhz)
  91 {
  92         struct cpufreq_dt_softc *sc;
  93         ssize_t n;
  94
  95         sc = device_get_softc(dev);
  96
  97         for (n = 0; n < sc->nopp; n++)
  98                 if (CPUFREQ_CMP(sc->opp[n].freq_khz / 1000, freq_mhz))
  99                         return (&sc->opp[n]);
 100
 101         return (NULL);
 102 }
 103
 104 static void
 105 cpufreq_dt_opp_to_setting(device_t dev, const struct cpufreq_dt_opp *opp,
 106     struct cf_setting *set)
 107 {
 108         struct cpufreq_dt_softc *sc;
 109
 110         sc = device_get_softc(dev);
 111
 112         memset(set, 0, sizeof(*set));
 113         set->freq = opp->freq_khz / 1000;
 114         set->volts = opp->voltage_uv / 1000;
 115         set->power = CPUFREQ_VAL_UNKNOWN;
 116         set->lat = sc->clk_latency;
 117         set->dev = dev;
 118 }
 119
 120 static int
 121 cpufreq_dt_get(device_t dev, struct cf_setting *set)
 122 {
 123         struct cpufreq_dt_softc *sc;
 124         const struct cpufreq_dt_opp *opp;
 125         uint64_t freq;
 126
 127         sc = device_get_softc(dev);
 128
 129         if (clk_get_freq(sc->clk, &freq) != 0)
 130                 return (ENXIO);
 131
 132         opp = cpufreq_dt_find_opp(dev, freq / 1000000);
 133         if (opp == NULL)
 134                 return (ENOENT);
 135
 136         cpufreq_dt_opp_to_setting(dev, opp, set);
 137
 138         return (0);
 139 }
 140
 141 static int
 142 cpufreq_dt_set(device_t dev, const struct cf_setting *set)
 143 {
 144         struct cpufreq_dt_softc *sc;
 145         const struct cpufreq_dt_opp *opp, *copp;
 146         uint64_t freq;
 147         int error;
 148
 149         sc = device_get_softc(dev);
 150
 151         if (clk_get_freq(sc->clk, &freq) != 0)
 152                 return (ENXIO);
 153
 154         copp = cpufreq_dt_find_opp(dev, freq / 1000000);
 155         if (copp == NULL)
 156                 return (ENOENT);
 157         opp = cpufreq_dt_find_opp(dev, set->freq);
 158         if (opp == NULL)
 159                 return (EINVAL);
 160
 161         if (copp->voltage_uv < opp->voltage_uv) {
 162                 error = regulator_set_voltage(sc->reg, opp->voltage_uv,
 163                     opp->voltage_uv);
 164                 if (error != 0)
 165                         return (ENXIO);
 166         }
 167
 168         error = clk_set_freq(sc->clk, (uint64_t)opp->freq_khz * 1000, 0);
 169         if (error != 0) {
 170                 /* Restore previous voltage (best effort) */
 171                 (void)regulator_set_voltage(sc->reg, copp->voltage_uv,
 172                     copp->voltage_uv);
 173                 return (ENXIO);
 174         }
 175
 176         if (copp->voltage_uv > opp->voltage_uv) {
 177                 error = regulator_set_voltage(sc->reg, opp->voltage_uv,
 178                     opp->voltage_uv);
 179                 if (error != 0) {
 180                         /* Restore previous CPU frequency (best effort) */
 181                         (void)clk_set_freq(sc->clk,
 182                             (uint64_t)copp->freq_khz * 1000, 0);
 183                         return (ENXIO);
 184                 }
 185         }
 186
 187         if (clk_get_freq(sc->clk, &freq) == 0)
 188                 cpufreq_dt_notify(dev, freq);
 189
 190         return (0);
 191 }
 192
 193
 194 static int
 195 cpufreq_dt_type(device_t dev, int *type)
 196 {
 197         if (type == NULL)
 198                 return (EINVAL);
 199
 200         *type = CPUFREQ_TYPE_ABSOLUTE;
 201         return (0);
 202 }
 203
 204 static int
 205 cpufreq_dt_settings(device_t dev, struct cf_setting *sets, int *count)
 206 {
 207         struct cpufreq_dt_softc *sc;
 208         ssize_t n;
 209
 210         if (sets == NULL || count == NULL)
 211                 return (EINVAL);
 212
 213         sc = device_get_softc(dev);
 214
 215         if (*count < sc->nopp) {
 216                 *count = (int)sc->nopp;
 217                 return (E2BIG);
 218         }
 219
 220         for (n = 0; n < sc->nopp; n++)
 221                 cpufreq_dt_opp_to_setting(dev, &sc->opp[n], &sets[n]);
 222
 223         *count = (int)sc->nopp;
 224
 225         return (0);
 226 }
 227
 228 static void
 229 cpufreq_dt_identify(driver_t *driver, device_t parent)
 230 {
 231         phandle_t node;
 232
 233         /* Properties must be listed under node /cpus/cpu@0 */
 234         node = ofw_bus_get_node(parent);
 235
 236         /* The cpu@0 node must have the following properties */
 237         if (!OF_hasprop(node, "operating-points") ||
 238             !OF_hasprop(node, "clocks") ||
 239             !OF_hasprop(node, "cpu-supply"))
 240                 return;
 241
 242         if (device_find_child(parent, "cpufreq_dt", -1) != NULL)
 243                 return;
 244
 245         if (BUS_ADD_CHILD(parent, 0, "cpufreq_dt", -1) == NULL)
 246                 device_printf(parent, "add cpufreq_dt child failed\n");
 247 }
 248
 249 static int
 250 cpufreq_dt_probe(device_t dev)
 251 {
 252         phandle_t node;
 253
 254         node = ofw_bus_get_node(device_get_parent(dev));
 255
 256         if (!OF_hasprop(node, "operating-points") ||
 257             !OF_hasprop(node, "clocks") ||
 258             !OF_hasprop(node, "cpu-supply"))
 259                 return (ENXIO);
 260
 261         device_set_desc(dev, "Generic cpufreq driver");
 262         return (BUS_PROBE_GENERIC);
 263 }
 264
 265 static int
 266 cpufreq_dt_attach(device_t dev)
 267 {
 268         struct cpufreq_dt_softc *sc;
 269         uint32_t *opp, lat;
 270         phandle_t node, cnode;
 271         uint64_t freq;
 272         ssize_t n;
 273         int cpu;
 274
 275         sc = device_get_softc(dev);
 276         node = ofw_bus_get_node(device_get_parent(dev));
 277
 278         if (regulator_get_by_ofw_property(dev, node,
 279             "cpu-supply", &sc->reg) != 0) {
 280                 device_printf(dev, "no regulator for %s\n",
 281                     ofw_bus_get_name(device_get_parent(dev)));
 282                 return (ENXIO);
 283         }
 284
 285         if (clk_get_by_ofw_index(dev, node, 0, &sc->clk) != 0) {
 286                 device_printf(dev, "no clock for %s\n",
 287                     ofw_bus_get_name(device_get_parent(dev)));
 288                 regulator_release(sc->reg);
 289                 return (ENXIO);
 290         }
 291
 292         sc->nopp = OF_getencprop_alloc(node, "operating-points",
 293             sizeof(*sc->opp), (void **)&opp);
 294         if (sc->nopp == -1)
 295                 return (ENXIO);
 296         sc->opp = malloc(sizeof(*sc->opp) * sc->nopp, M_DEVBUF, M_WAITOK);
 297         for (n = 0; n < sc->nopp; n++) {
 298                 sc->opp[n].freq_khz = opp[n * 2 + 0];
 299                 sc->opp[n].voltage_uv = opp[n * 2 + 1];
 300
 301                 if (bootverbose)
 302                         device_printf(dev, "%u.%03u MHz, %u uV\n",
 303                             sc->opp[n].freq_khz / 1000,
 304                             sc->opp[n].freq_khz % 1000,
 305                             sc->opp[n].voltage_uv);
 306         }
 307         free(opp, M_OFWPROP);
 308
 309         if (OF_getencprop(node, "clock-latency", &lat, sizeof(lat)) == -1)
 310                 sc->clk_latency = CPUFREQ_VAL_UNKNOWN;
 311         else
 312                 sc->clk_latency = (int)lat;
 313
 314         /*
 315          * Find all CPUs that share the same voltage and CPU frequency
 316          * controls. Start with the current node and move forward until
 317          * the end is reached or a peer has an "operating-points" property.
 318          */
 319         CPU_ZERO(&sc->cpus);
 320         cpu = device_get_unit(device_get_parent(dev));
 321         for (cnode = node; cnode > 0; cnode = OF_peer(cnode), cpu++) {
 322                 if (cnode != node && OF_hasprop(cnode, "operating-points"))
 323                         break;
 324                 CPU_SET(cpu, &sc->cpus);
 325         }
 326
 327         if (clk_get_freq(sc->clk, &freq) == 0)
 328                 cpufreq_dt_notify(dev, freq);
 329
 330         cpufreq_register(dev);
 331
 332         return (0);
 333 }
 334
 335
 336 static device_method_t cpufreq_dt_methods[] = {
 337         /* Device interface */
 338         DEVMETHOD(device_identify,      cpufreq_dt_identify),
 339         DEVMETHOD(device_probe,         cpufreq_dt_probe),
 340         DEVMETHOD(device_attach,        cpufreq_dt_attach),
 341
 342         /* cpufreq interface */
 343         DEVMETHOD(cpufreq_drv_get,      cpufreq_dt_get),
 344         DEVMETHOD(cpufreq_drv_set,      cpufreq_dt_set),
 345         DEVMETHOD(cpufreq_drv_type,     cpufreq_dt_type),
 346         DEVMETHOD(cpufreq_drv_settings, cpufreq_dt_settings),
 347
 348         DEVMETHOD_END
 349 };
 350
 351 static driver_t cpufreq_dt_driver = {
 352         "cpufreq_dt",
 353         cpufreq_dt_methods,
 354         sizeof(struct cpufreq_dt_softc),
 355 };
 356
 357 static devclass_t cpufreq_dt_devclass;
 358
 359 DRIVER_MODULE(cpufreq_dt, cpu, cpufreq_dt_driver, cpufreq_dt_devclass, 0, 0);
 360 MODULE_VERSION(cpufreq_dt, 1);