drivers/cpuidle/cpuidle-psci.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * PSCI CPU idle driver.
   4  *
   5  * Copyright (C) 2019 ARM Ltd.
   6  * Author: Lorenzo Pieralisi <[email protected]>
   7  */
   8
   9 #define pr_fmt(fmt) "CPUidle PSCI: " fmt
  10
  11 #include <linux/cpuhotplug.h>
  12 #include <linux/cpu_cooling.h>
  13 #include <linux/cpuidle.h>
  14 #include <linux/cpumask.h>
  15 #include <linux/cpu_pm.h>
  16 #include <linux/kernel.h>
  17 #include <linux/module.h>
  18 #include <linux/of.h>
  19 #include <linux/platform_device.h>
  20 #include <linux/psci.h>
  21 #include <linux/pm_domain.h>
  22 #include <linux/pm_runtime.h>
  23 #include <linux/slab.h>
  24 #include <linux/string.h>
  25 #include <linux/syscore_ops.h>
  26
  27 #include <asm/cpuidle.h>
  28
  29 #include "cpuidle-psci.h"
  30 #include "dt_idle_states.h"
  31 #include "dt_idle_genpd.h"
  32
  33 struct psci_cpuidle_data {
  34         u32 *psci_states;
  35         struct device *dev;
  36 };
  37
  38 static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data);
  39 static DEFINE_PER_CPU(u32, domain_state);
  40 static bool psci_cpuidle_use_syscore;
  41 static bool psci_cpuidle_use_cpuhp;
  42
  43 void psci_set_domain_state(u32 state)
  44 {
  45         __this_cpu_write(domain_state, state);
  46 }
  47
  48 static inline u32 psci_get_domain_state(void)
  49 {
  50         return __this_cpu_read(domain_state);
  51 }
  52
  53 static __cpuidle int __psci_enter_domain_idle_state(struct cpuidle_device *dev,
  54                                                     struct cpuidle_driver *drv, int idx,
  55                                                     bool s2idle)
  56 {
  57         struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data);
  58         u32 *states = data->psci_states;
  59         struct device *pd_dev = data->dev;
  60         u32 state;
  61         int ret;
  62
  63         ret = cpu_pm_enter();
  64         if (ret)
  65                 return -1;
  66
  67         /* Do runtime PM to manage a hierarchical CPU toplogy. */
  68         if (s2idle)
  69                 dev_pm_genpd_suspend(pd_dev);
  70         else
  71                 pm_runtime_put_sync_suspend(pd_dev);
  72
  73         state = psci_get_domain_state();
  74         if (!state)
  75                 state = states[idx];
  76
  77         ret = psci_cpu_suspend_enter(state) ? -1 : idx;
  78
  79         if (s2idle)
  80                 dev_pm_genpd_resume(pd_dev);
  81         else
  82                 pm_runtime_get_sync(pd_dev);
  83
  84         cpu_pm_exit();
  85
  86         /* Clear the domain state to start fresh when back from idle. */
  87         psci_set_domain_state(0);
  88         return ret;
  89 }
  90
  91 static int psci_enter_domain_idle_state(struct cpuidle_device *dev,
  92                                         struct cpuidle_driver *drv, int idx)
  93 {
  94         return __psci_enter_domain_idle_state(dev, drv, idx, false);
  95 }
  96
  97 static int psci_enter_s2idle_domain_idle_state(struct cpuidle_device *dev,
  98                                                struct cpuidle_driver *drv,
  99                                                int idx)
 100 {
 101         return __psci_enter_domain_idle_state(dev, drv, idx, true);
 102 }
 103
 104 static int psci_idle_cpuhp_up(unsigned int cpu)
 105 {
 106         struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);
 107
 108         if (pd_dev)
 109                 pm_runtime_get_sync(pd_dev);
 110
 111         return 0;
 112 }
 113
 114 static int psci_idle_cpuhp_down(unsigned int cpu)
 115 {
 116         struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);
 117
 118         if (pd_dev) {
 119                 pm_runtime_put_sync(pd_dev);
 120                 /* Clear domain state to start fresh at next online. */
 121                 psci_set_domain_state(0);
 122         }
 123
 124         return 0;
 125 }
 126
 127 static void psci_idle_syscore_switch(bool suspend)
 128 {
 129         bool cleared = false;
 130         struct device *dev;
 131         int cpu;
 132
 133         for_each_possible_cpu(cpu) {
 134                 dev = per_cpu_ptr(&psci_cpuidle_data, cpu)->dev;
 135
 136                 if (dev && suspend) {
 137                         dev_pm_genpd_suspend(dev);
 138                 } else if (dev) {
 139                         dev_pm_genpd_resume(dev);
 140
 141                         /* Account for userspace having offlined a CPU. */
 142                         if (pm_runtime_status_suspended(dev))
 143                                 pm_runtime_set_active(dev);
 144
 145                         /* Clear domain state to re-start fresh. */
 146                         if (!cleared) {
 147                                 psci_set_domain_state(0);
 148                                 cleared = true;
 149                         }
 150                 }
 151         }
 152 }
 153
 154 static int psci_idle_syscore_suspend(void)
 155 {
 156         psci_idle_syscore_switch(true);
 157         return 0;
 158 }
 159
 160 static void psci_idle_syscore_resume(void)
 161 {
 162         psci_idle_syscore_switch(false);
 163 }
 164
 165 static struct syscore_ops psci_idle_syscore_ops = {
 166         .suspend = psci_idle_syscore_suspend,
 167         .resume = psci_idle_syscore_resume,
 168 };
 169
 170 static void psci_idle_init_syscore(void)
 171 {
 172         if (psci_cpuidle_use_syscore)
 173                 register_syscore_ops(&psci_idle_syscore_ops);
 174 }
 175
 176 static void psci_idle_init_cpuhp(void)
 177 {
 178         int err;
 179
 180         if (!psci_cpuidle_use_cpuhp)
 181                 return;
 182
 183         err = cpuhp_setup_state_nocalls(CPUHP_AP_CPU_PM_STARTING,
 184                                         "cpuidle/psci:online",
 185                                         psci_idle_cpuhp_up,
 186                                         psci_idle_cpuhp_down);
 187         if (err)
 188                 pr_warn("Failed %d while setup cpuhp state\n", err);
 189 }
 190
 191 static __cpuidle int psci_enter_idle_state(struct cpuidle_device *dev,
 192                                            struct cpuidle_driver *drv, int idx)
 193 {
 194         u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states);
 195
 196         return CPU_PM_CPU_IDLE_ENTER_PARAM_RCU(psci_cpu_suspend_enter, idx, state[idx]);
 197 }
 198
 199 static const struct of_device_id psci_idle_state_match[] = {
 200         { .compatible = "arm,idle-state",
 201           .data = psci_enter_idle_state },
 202         { },
 203 };
 204
 205 int psci_dt_parse_state_node(struct device_node *np, u32 *state)
 206 {
 207         int err = of_property_read_u32(np, "arm,psci-suspend-param", state);
 208
 209         if (err) {
 210                 pr_warn("%pOF missing arm,psci-suspend-param property\n", np);
 211                 return err;
 212         }
 213
 214         if (!psci_power_state_is_valid(*state)) {
 215                 pr_warn("Invalid PSCI power state %#x\n", *state);
 216                 return -EINVAL;
 217         }
 218
 219         return 0;
 220 }
 221
 222 static int psci_dt_cpu_init_topology(struct cpuidle_driver *drv,
 223                                      struct psci_cpuidle_data *data,
 224                                      unsigned int state_count, int cpu)
 225 {
 226         /* Currently limit the hierarchical topology to be used in OSI mode. */
 227         if (!psci_has_osi_support())
 228                 return 0;
 229
 230         data->dev = dt_idle_attach_cpu(cpu, "psci");
 231         if (IS_ERR_OR_NULL(data->dev))
 232                 return PTR_ERR_OR_ZERO(data->dev);
 233
 234         psci_cpuidle_use_syscore = true;
 235
 236         /*
 237          * Using the deepest state for the CPU to trigger a potential selection
 238          * of a shared state for the domain, assumes the domain states are all
 239          * deeper states. On PREEMPT_RT the hierarchical topology is limited to
 240          * s2ram and s2idle.
 241          */
 242         drv->states[state_count - 1].enter_s2idle = psci_enter_s2idle_domain_idle_state;
 243         if (!IS_ENABLED(CONFIG_PREEMPT_RT)) {
 244                 drv->states[state_count - 1].enter = psci_enter_domain_idle_state;
 245                 psci_cpuidle_use_cpuhp = true;
 246         }
 247
 248         return 0;
 249 }
 250
 251 static int psci_dt_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
 252                                  struct device_node *cpu_node,
 253                                  unsigned int state_count, int cpu)
 254 {
 255         int i, ret = 0;
 256         u32 *psci_states;
 257         struct device_node *state_node;
 258         struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
 259
 260         state_count++; /* Add WFI state too */
 261         psci_states = devm_kcalloc(dev, state_count, sizeof(*psci_states),
 262                                    GFP_KERNEL);
 263         if (!psci_states)
 264                 return -ENOMEM;
 265
 266         for (i = 1; i < state_count; i++) {
 267                 state_node = of_get_cpu_state_node(cpu_node, i - 1);
 268                 if (!state_node)
 269                         break;
 270
 271                 ret = psci_dt_parse_state_node(state_node, &psci_states[i]);
 272                 of_node_put(state_node);
 273
 274                 if (ret)
 275                         return ret;
 276
 277                 pr_debug("psci-power-state %#x index %d\n", psci_states[i], i);
 278         }
 279
 280         if (i != state_count)
 281                 return -ENODEV;
 282
 283         /* Initialize optional data, used for the hierarchical topology. */
 284         ret = psci_dt_cpu_init_topology(drv, data, state_count, cpu);
 285         if (ret < 0)
 286                 return ret;
 287
 288         /* Idle states parsed correctly, store them in the per-cpu struct. */
 289         data->psci_states = psci_states;
 290         return 0;
 291 }
 292
 293 static int psci_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
 294                               unsigned int cpu, unsigned int state_count)
 295 {
 296         struct device_node *cpu_node;
 297         int ret;
 298
 299         /*
 300          * If the PSCI cpu_suspend function hook has not been initialized
 301          * idle states must not be enabled, so bail out
 302          */
 303         if (!psci_ops.cpu_suspend)
 304                 return -EOPNOTSUPP;
 305
 306         cpu_node = of_cpu_device_node_get(cpu);
 307         if (!cpu_node)
 308                 return -ENODEV;
 309
 310         ret = psci_dt_cpu_init_idle(dev, drv, cpu_node, state_count, cpu);
 311
 312         of_node_put(cpu_node);
 313
 314         return ret;
 315 }
 316
 317 static void psci_cpu_deinit_idle(int cpu)
 318 {
 319         struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
 320
 321         dt_idle_detach_cpu(data->dev);
 322         psci_cpuidle_use_syscore = false;
 323         psci_cpuidle_use_cpuhp = false;
 324 }
 325
 326 static int psci_idle_init_cpu(struct device *dev, int cpu)
 327 {
 328         struct cpuidle_driver *drv;
 329         struct device_node *cpu_node;
 330         const char *enable_method;
 331         int ret = 0;
 332
 333         cpu_node = of_cpu_device_node_get(cpu);
 334         if (!cpu_node)
 335                 return -ENODEV;
 336
 337         /*
 338          * Check whether the enable-method for the cpu is PSCI, fail
 339          * if it is not.
 340          */
 341         enable_method = of_get_property(cpu_node, "enable-method", NULL);
 342         if (!enable_method || (strcmp(enable_method, "psci")))
 343                 ret = -ENODEV;
 344
 345         of_node_put(cpu_node);
 346         if (ret)
 347                 return ret;
 348
 349         drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL);
 350         if (!drv)
 351                 return -ENOMEM;
 352
 353         drv->name = "psci_idle";
 354         drv->owner = THIS_MODULE;
 355         drv->cpumask = (struct cpumask *)cpumask_of(cpu);
 356
 357         /*
 358          * PSCI idle states relies on architectural WFI to be represented as
 359          * state index 0.
 360          */
 361         drv->states[0].enter = psci_enter_idle_state;
 362         drv->states[0].exit_latency = 1;
 363         drv->states[0].target_residency = 1;
 364         drv->states[0].power_usage = UINT_MAX;
 365         strcpy(drv->states[0].name, "WFI");
 366         strcpy(drv->states[0].desc, "ARM WFI");
 367
 368         /*
 369          * If no DT idle states are detected (ret == 0) let the driver
 370          * initialization fail accordingly since there is no reason to
 371          * initialize the idle driver if only wfi is supported, the
 372          * default archictectural back-end already executes wfi
 373          * on idle entry.
 374          */
 375         ret = dt_init_idle_driver(drv, psci_idle_state_match, 1);
 376         if (ret <= 0)
 377                 return ret ? : -ENODEV;
 378
 379         /*
 380          * Initialize PSCI idle states.
 381          */
 382         ret = psci_cpu_init_idle(dev, drv, cpu, ret);
 383         if (ret) {
 384                 pr_err("CPU %d failed to PSCI idle\n", cpu);
 385                 return ret;
 386         }
 387
 388         ret = cpuidle_register(drv, NULL);
 389         if (ret)
 390                 goto deinit;
 391
 392         cpuidle_cooling_register(drv);
 393
 394         return 0;
 395 deinit:
 396         psci_cpu_deinit_idle(cpu);
 397         return ret;
 398 }
 399
 400 /*
 401  * psci_idle_probe - Initializes PSCI cpuidle driver
 402  *
 403  * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails
 404  * to register cpuidle driver then rollback to cancel all CPUs
 405  * registration.
 406  */
 407 static int psci_cpuidle_probe(struct platform_device *pdev)
 408 {
 409         int cpu, ret;
 410         struct cpuidle_driver *drv;
 411         struct cpuidle_device *dev;
 412
 413         for_each_possible_cpu(cpu) {
 414                 ret = psci_idle_init_cpu(&pdev->dev, cpu);
 415                 if (ret)
 416                         goto out_fail;
 417         }
 418
 419         psci_idle_init_syscore();
 420         psci_idle_init_cpuhp();
 421         return 0;
 422
 423 out_fail:
 424         while (--cpu >= 0) {
 425                 dev = per_cpu(cpuidle_devices, cpu);
 426                 drv = cpuidle_get_cpu_driver(dev);
 427                 cpuidle_unregister(drv);
 428                 psci_cpu_deinit_idle(cpu);
 429         }
 430
 431         return ret;
 432 }
 433
 434 static struct platform_driver psci_cpuidle_driver = {
 435         .probe = psci_cpuidle_probe,
 436         .driver = {
 437                 .name = "psci-cpuidle",
 438         },
 439 };
 440
 441 static int __init psci_idle_init(void)
 442 {
 443         struct platform_device *pdev;
 444         int ret;
 445
 446         ret = platform_driver_register(&psci_cpuidle_driver);
 447         if (ret)
 448                 return ret;
 449
 450         pdev = platform_device_register_simple("psci-cpuidle", -1, NULL, 0);
 451         if (IS_ERR(pdev)) {
 452                 platform_driver_unregister(&psci_cpuidle_driver);
 453                 return PTR_ERR(pdev);
 454         }
 455
 456         return 0;
 457 }
 458 device_initcall(psci_idle_init);