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