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