drivers/cpuidle/cpuidle-psci.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * PSCI CPU idle driver.
0004  *
0005  * Copyright (C) 2019 ARM Ltd.
0006  * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
0007  */
0008
0009 #define pr_fmt(fmt) "CPUidle PSCI: " fmt
0010
0011 #include <linux/cpuhotplug.h>
0012 #include <linux/cpu_cooling.h>
0013 #include <linux/cpuidle.h>
0014 #include <linux/cpumask.h>
0015 #include <linux/cpu_pm.h>
0016 #include <linux/kernel.h>
0017 #include <linux/module.h>
0018 #include <linux/of.h>
0019 #include <linux/of_device.h>
0020 #include <linux/platform_device.h>
0021 #include <linux/psci.h>
0022 #include <linux/pm_domain.h>
0023 #include <linux/pm_runtime.h>
0024 #include <linux/slab.h>
0025 #include <linux/string.h>
0026 #include <linux/syscore_ops.h>
0027
0028 #include <asm/cpuidle.h>
0029
0030 #include "cpuidle-psci.h"
0031 #include "dt_idle_states.h"
0032
0033 struct psci_cpuidle_data {
0034     u32 *psci_states;
0035     struct device *dev;
0036 };
0037
0038 static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data);
0039 static DEFINE_PER_CPU(u32, domain_state);
0040 static bool psci_cpuidle_use_cpuhp;
0041
0042 void psci_set_domain_state(u32 state)
0043 {
0044     __this_cpu_write(domain_state, state);
0045 }
0046
0047 static inline u32 psci_get_domain_state(void)
0048 {
0049     return __this_cpu_read(domain_state);
0050 }
0051
0052 static inline int psci_enter_state(int idx, u32 state)
0053 {
0054     return CPU_PM_CPU_IDLE_ENTER_PARAM(psci_cpu_suspend_enter, idx, state);
0055 }
0056
0057 static int __psci_enter_domain_idle_state(struct cpuidle_device *dev,
0058                       struct cpuidle_driver *drv, int idx,
0059                       bool s2idle)
0060 {
0061     struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data);
0062     u32 *states = data->psci_states;
0063     struct device *pd_dev = data->dev;
0064     u32 state;
0065     int ret;
0066
0067     ret = cpu_pm_enter();
0068     if (ret)
0069         return -1;
0070
0071     /* Do runtime PM to manage a hierarchical CPU toplogy. */
0072     ct_irq_enter_irqson();
0073     if (s2idle)
0074         dev_pm_genpd_suspend(pd_dev);
0075     else
0076         pm_runtime_put_sync_suspend(pd_dev);
0077     ct_irq_exit_irqson();
0078
0079     state = psci_get_domain_state();
0080     if (!state)
0081         state = states[idx];
0082
0083     ret = psci_cpu_suspend_enter(state) ? -1 : idx;
0084
0085     ct_irq_enter_irqson();
0086     if (s2idle)
0087         dev_pm_genpd_resume(pd_dev);
0088     else
0089         pm_runtime_get_sync(pd_dev);
0090     ct_irq_exit_irqson();
0091
0092     cpu_pm_exit();
0093
0094     /* Clear the domain state to start fresh when back from idle. */
0095     psci_set_domain_state(0);
0096     return ret;
0097 }
0098
0099 static int psci_enter_domain_idle_state(struct cpuidle_device *dev,
0100                     struct cpuidle_driver *drv, int idx)
0101 {
0102     return __psci_enter_domain_idle_state(dev, drv, idx, false);
0103 }
0104
0105 static int psci_enter_s2idle_domain_idle_state(struct cpuidle_device *dev,
0106                            struct cpuidle_driver *drv,
0107                            int idx)
0108 {
0109     return __psci_enter_domain_idle_state(dev, drv, idx, true);
0110 }
0111
0112 static int psci_idle_cpuhp_up(unsigned int cpu)
0113 {
0114     struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);
0115
0116     if (pd_dev)
0117         pm_runtime_get_sync(pd_dev);
0118
0119     return 0;
0120 }
0121
0122 static int psci_idle_cpuhp_down(unsigned int cpu)
0123 {
0124     struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);
0125
0126     if (pd_dev) {
0127         pm_runtime_put_sync(pd_dev);
0128         /* Clear domain state to start fresh at next online. */
0129         psci_set_domain_state(0);
0130     }
0131
0132     return 0;
0133 }
0134
0135 static void psci_idle_syscore_switch(bool suspend)
0136 {
0137     bool cleared = false;
0138     struct device *dev;
0139     int cpu;
0140
0141     for_each_possible_cpu(cpu) {
0142         dev = per_cpu_ptr(&psci_cpuidle_data, cpu)->dev;
0143
0144         if (dev && suspend) {
0145             dev_pm_genpd_suspend(dev);
0146         } else if (dev) {
0147             dev_pm_genpd_resume(dev);
0148
0149             /* Account for userspace having offlined a CPU. */
0150             if (pm_runtime_status_suspended(dev))
0151                 pm_runtime_set_active(dev);
0152
0153             /* Clear domain state to re-start fresh. */
0154             if (!cleared) {
0155                 psci_set_domain_state(0);
0156                 cleared = true;
0157             }
0158         }
0159     }
0160 }
0161
0162 static int psci_idle_syscore_suspend(void)
0163 {
0164     psci_idle_syscore_switch(true);
0165     return 0;
0166 }
0167
0168 static void psci_idle_syscore_resume(void)
0169 {
0170     psci_idle_syscore_switch(false);
0171 }
0172
0173 static struct syscore_ops psci_idle_syscore_ops = {
0174     .suspend = psci_idle_syscore_suspend,
0175     .resume = psci_idle_syscore_resume,
0176 };
0177
0178 static void psci_idle_init_cpuhp(void)
0179 {
0180     int err;
0181
0182     if (!psci_cpuidle_use_cpuhp)
0183         return;
0184
0185     register_syscore_ops(&psci_idle_syscore_ops);
0186
0187     err = cpuhp_setup_state_nocalls(CPUHP_AP_CPU_PM_STARTING,
0188                     "cpuidle/psci:online",
0189                     psci_idle_cpuhp_up,
0190                     psci_idle_cpuhp_down);
0191     if (err)
0192         pr_warn("Failed %d while setup cpuhp state\n", err);
0193 }
0194
0195 static int psci_enter_idle_state(struct cpuidle_device *dev,
0196                 struct cpuidle_driver *drv, int idx)
0197 {
0198     u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states);
0199
0200     return psci_enter_state(idx, state[idx]);
0201 }
0202
0203 static const struct of_device_id psci_idle_state_match[] = {
0204     { .compatible = "arm,idle-state",
0205       .data = psci_enter_idle_state },
0206     { },
0207 };
0208
0209 int psci_dt_parse_state_node(struct device_node *np, u32 *state)
0210 {
0211     int err = of_property_read_u32(np, "arm,psci-suspend-param", state);
0212
0213     if (err) {
0214         pr_warn("%pOF missing arm,psci-suspend-param property\n", np);
0215         return err;
0216     }
0217
0218     if (!psci_power_state_is_valid(*state)) {
0219         pr_warn("Invalid PSCI power state %#x\n", *state);
0220         return -EINVAL;
0221     }
0222
0223     return 0;
0224 }
0225
0226 static int psci_dt_cpu_init_topology(struct cpuidle_driver *drv,
0227                      struct psci_cpuidle_data *data,
0228                      unsigned int state_count, int cpu)
0229 {
0230     /* Currently limit the hierarchical topology to be used in OSI mode. */
0231     if (!psci_has_osi_support())
0232         return 0;
0233
0234     data->dev = psci_dt_attach_cpu(cpu);
0235     if (IS_ERR_OR_NULL(data->dev))
0236         return PTR_ERR_OR_ZERO(data->dev);
0237
0238     /*
0239      * Using the deepest state for the CPU to trigger a potential selection
0240      * of a shared state for the domain, assumes the domain states are all
0241      * deeper states.
0242      */
0243     drv->states[state_count - 1].enter = psci_enter_domain_idle_state;
0244     drv->states[state_count - 1].enter_s2idle = psci_enter_s2idle_domain_idle_state;
0245     psci_cpuidle_use_cpuhp = true;
0246
0247     return 0;
0248 }
0249
0250 static int psci_dt_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
0251                  struct device_node *cpu_node,
0252                  unsigned int state_count, int cpu)
0253 {
0254     int i, ret = 0;
0255     u32 *psci_states;
0256     struct device_node *state_node;
0257     struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
0258
0259     state_count++; /* Add WFI state too */
0260     psci_states = devm_kcalloc(dev, state_count, sizeof(*psci_states),
0261                    GFP_KERNEL);
0262     if (!psci_states)
0263         return -ENOMEM;
0264
0265     for (i = 1; i < state_count; i++) {
0266         state_node = of_get_cpu_state_node(cpu_node, i - 1);
0267         if (!state_node)
0268             break;
0269
0270         ret = psci_dt_parse_state_node(state_node, &psci_states[i]);
0271         of_node_put(state_node);
0272
0273         if (ret)
0274             return ret;
0275
0276         pr_debug("psci-power-state %#x index %d\n", psci_states[i], i);
0277     }
0278
0279     if (i != state_count)
0280         return -ENODEV;
0281
0282     /* Initialize optional data, used for the hierarchical topology. */
0283     ret = psci_dt_cpu_init_topology(drv, data, state_count, cpu);
0284     if (ret < 0)
0285         return ret;
0286
0287     /* Idle states parsed correctly, store them in the per-cpu struct. */
0288     data->psci_states = psci_states;
0289     return 0;
0290 }
0291
0292 static int psci_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
0293                   unsigned int cpu, unsigned int state_count)
0294 {
0295     struct device_node *cpu_node;
0296     int ret;
0297
0298     /*
0299      * If the PSCI cpu_suspend function hook has not been initialized
0300      * idle states must not be enabled, so bail out
0301      */
0302     if (!psci_ops.cpu_suspend)
0303         return -EOPNOTSUPP;
0304
0305     cpu_node = of_cpu_device_node_get(cpu);
0306     if (!cpu_node)
0307         return -ENODEV;
0308
0309     ret = psci_dt_cpu_init_idle(dev, drv, cpu_node, state_count, cpu);
0310
0311     of_node_put(cpu_node);
0312
0313     return ret;
0314 }
0315
0316 static void psci_cpu_deinit_idle(int cpu)
0317 {
0318     struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
0319
0320     psci_dt_detach_cpu(data->dev);
0321     psci_cpuidle_use_cpuhp = false;
0322 }
0323
0324 static int psci_idle_init_cpu(struct device *dev, int cpu)
0325 {
0326     struct cpuidle_driver *drv;
0327     struct device_node *cpu_node;
0328     const char *enable_method;
0329     int ret = 0;
0330
0331     cpu_node = of_cpu_device_node_get(cpu);
0332     if (!cpu_node)
0333         return -ENODEV;
0334
0335     /*
0336      * Check whether the enable-method for the cpu is PSCI, fail
0337      * if it is not.
0338      */
0339     enable_method = of_get_property(cpu_node, "enable-method", NULL);
0340     if (!enable_method || (strcmp(enable_method, "psci")))
0341         ret = -ENODEV;
0342
0343     of_node_put(cpu_node);
0344     if (ret)
0345         return ret;
0346
0347     drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL);
0348     if (!drv)
0349         return -ENOMEM;
0350
0351     drv->name = "psci_idle";
0352     drv->owner = THIS_MODULE;
0353     drv->cpumask = (struct cpumask *)cpumask_of(cpu);
0354
0355     /*
0356      * PSCI idle states relies on architectural WFI to be represented as
0357      * state index 0.
0358      */
0359     drv->states[0].enter = psci_enter_idle_state;
0360     drv->states[0].exit_latency = 1;
0361     drv->states[0].target_residency = 1;
0362     drv->states[0].power_usage = UINT_MAX;
0363     strcpy(drv->states[0].name, "WFI");
0364     strcpy(drv->states[0].desc, "ARM WFI");
0365
0366     /*
0367      * If no DT idle states are detected (ret == 0) let the driver
0368      * initialization fail accordingly since there is no reason to
0369      * initialize the idle driver if only wfi is supported, the
0370      * default archictectural back-end already executes wfi
0371      * on idle entry.
0372      */
0373     ret = dt_init_idle_driver(drv, psci_idle_state_match, 1);
0374     if (ret <= 0)
0375         return ret ? : -ENODEV;
0376
0377     /*
0378      * Initialize PSCI idle states.
0379      */
0380     ret = psci_cpu_init_idle(dev, drv, cpu, ret);
0381     if (ret) {
0382         pr_err("CPU %d failed to PSCI idle\n", cpu);
0383         return ret;
0384     }
0385
0386     ret = cpuidle_register(drv, NULL);
0387     if (ret)
0388         goto deinit;
0389
0390     cpuidle_cooling_register(drv);
0391
0392     return 0;
0393 deinit:
0394     psci_cpu_deinit_idle(cpu);
0395     return ret;
0396 }
0397
0398 /*
0399  * psci_idle_probe - Initializes PSCI cpuidle driver
0400  *
0401  * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails
0402  * to register cpuidle driver then rollback to cancel all CPUs
0403  * registration.
0404  */
0405 static int psci_cpuidle_probe(struct platform_device *pdev)
0406 {
0407     int cpu, ret;
0408     struct cpuidle_driver *drv;
0409     struct cpuidle_device *dev;
0410
0411     for_each_possible_cpu(cpu) {
0412         ret = psci_idle_init_cpu(&pdev->dev, cpu);
0413         if (ret)
0414             goto out_fail;
0415     }
0416
0417     psci_idle_init_cpuhp();
0418     return 0;
0419
0420 out_fail:
0421     while (--cpu >= 0) {
0422         dev = per_cpu(cpuidle_devices, cpu);
0423         drv = cpuidle_get_cpu_driver(dev);
0424         cpuidle_unregister(drv);
0425         psci_cpu_deinit_idle(cpu);
0426     }
0427
0428     return ret;
0429 }
0430
0431 static struct platform_driver psci_cpuidle_driver = {
0432     .probe = psci_cpuidle_probe,
0433     .driver = {
0434         .name = "psci-cpuidle",
0435     },
0436 };
0437
0438 static int __init psci_idle_init(void)
0439 {
0440     struct platform_device *pdev;
0441     int ret;
0442
0443     ret = platform_driver_register(&psci_cpuidle_driver);
0444     if (ret)
0445         return ret;
0446
0447     pdev = platform_device_register_simple("psci-cpuidle", -1, NULL, 0);
0448     if (IS_ERR(pdev)) {
0449         platform_driver_unregister(&psci_cpuidle_driver);
0450         return PTR_ERR(pdev);
0451     }
0452
0453     return 0;
0454 }
0455 device_initcall(psci_idle_init);