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/cpuidle.h>
  12 #include <linux/cpumask.h>
  13 #include <linux/cpu_pm.h>
  14 #include <linux/kernel.h>
  15 #include <linux/module.h>
  16 #include <linux/of.h>
  17 #include <linux/of_device.h>
  18 #include <linux/psci.h>
  19 #include <linux/slab.h>
  20
  21 #include <asm/cpuidle.h>
  22
  23 #include "cpuidle-psci.h"
  24 #include "dt_idle_states.h"
  25
  26 struct psci_cpuidle_data {
  27         u32 *psci_states;
  28         struct device *dev;
  29 };
  30
  31 static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data);
  32 static DEFINE_PER_CPU(u32, domain_state);
  33
  34 static inline void psci_set_domain_state(u32 state)
  35 {
  36         __this_cpu_write(domain_state, state);
  37 }
  38
  39 static inline u32 psci_get_domain_state(void)
  40 {
  41         return __this_cpu_read(domain_state);
  42 }
  43
  44 static inline int psci_enter_state(int idx, u32 state)
  45 {
  46         return CPU_PM_CPU_IDLE_ENTER_PARAM(psci_cpu_suspend_enter, idx, state);
  47 }
  48
  49 static int psci_enter_domain_idle_state(struct cpuidle_device *dev,
  50                                         struct cpuidle_driver *drv, int idx)
  51 {
  52         struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data);
  53         u32 *states = data->psci_states;
  54         u32 state = psci_get_domain_state();
  55         int ret;
  56
  57         if (!state)
  58                 state = states[idx];
  59
  60         ret = psci_enter_state(idx, state);
  61
  62         /* Clear the domain state to start fresh when back from idle. */
  63         psci_set_domain_state(0);
  64         return ret;
  65 }
  66
  67 static int psci_enter_idle_state(struct cpuidle_device *dev,
  68                                 struct cpuidle_driver *drv, int idx)
  69 {
  70         u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states);
  71
  72         return psci_enter_state(idx, state[idx]);
  73 }
  74
  75 static struct cpuidle_driver psci_idle_driver __initdata = {
  76         .name = "psci_idle",
  77         .owner = THIS_MODULE,
  78         /*
  79          * PSCI idle states relies on architectural WFI to
  80          * be represented as state index 0.
  81          */
  82         .states[0] = {
  83                 .enter                  = psci_enter_idle_state,
  84                 .exit_latency           = 1,
  85                 .target_residency       = 1,
  86                 .power_usage            = UINT_MAX,
  87                 .name                   = "WFI",
  88                 .desc                   = "ARM WFI",
  89         }
  90 };
  91
  92 static const struct of_device_id psci_idle_state_match[] __initconst = {
  93         { .compatible = "arm,idle-state",
  94           .data = psci_enter_idle_state },
  95         { },
  96 };
  97
  98 static int __init psci_dt_parse_state_node(struct device_node *np, u32 *state)
  99 {
 100         int err = of_property_read_u32(np, "arm,psci-suspend-param", state);
 101
 102         if (err) {
 103                 pr_warn("%pOF missing arm,psci-suspend-param property\n", np);
 104                 return err;
 105         }
 106
 107         if (!psci_power_state_is_valid(*state)) {
 108                 pr_warn("Invalid PSCI power state %#x\n", *state);
 109                 return -EINVAL;
 110         }
 111
 112         return 0;
 113 }
 114
 115 static int __init psci_dt_cpu_init_idle(struct cpuidle_driver *drv,
 116                                         struct device_node *cpu_node,
 117                                         unsigned int state_count, int cpu)
 118 {
 119         int i, ret = 0;
 120         u32 *psci_states;
 121         struct device_node *state_node;
 122         struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
 123
 124         state_count++; /* Add WFI state too */
 125         psci_states = kcalloc(state_count, sizeof(*psci_states), GFP_KERNEL);
 126         if (!psci_states)
 127                 return -ENOMEM;
 128
 129         for (i = 1; i < state_count; i++) {
 130                 state_node = of_get_cpu_state_node(cpu_node, i - 1);
 131                 if (!state_node)
 132                         break;
 133
 134                 ret = psci_dt_parse_state_node(state_node, &psci_states[i]);
 135                 of_node_put(state_node);
 136
 137                 if (ret)
 138                         goto free_mem;
 139
 140                 pr_debug("psci-power-state %#x index %d\n", psci_states[i], i);
 141         }
 142
 143         if (i != state_count) {
 144                 ret = -ENODEV;
 145                 goto free_mem;
 146         }
 147
 148         /* Currently limit the hierarchical topology to be used in OSI mode. */
 149         if (psci_has_osi_support()) {
 150                 data->dev = psci_dt_attach_cpu(cpu);
 151                 if (IS_ERR(data->dev)) {
 152                         ret = PTR_ERR(data->dev);
 153                         goto free_mem;
 154                 }
 155
 156                 /*
 157                  * Using the deepest state for the CPU to trigger a potential
 158                  * selection of a shared state for the domain, assumes the
 159                  * domain states are all deeper states.
 160                  */
 161                 if (data->dev)
 162                         drv->states[state_count - 1].enter =
 163                                 psci_enter_domain_idle_state;
 164         }
 165
 166         /* Idle states parsed correctly, store them in the per-cpu struct. */
 167         data->psci_states = psci_states;
 168         return 0;
 169
 170 free_mem:
 171         kfree(psci_states);
 172         return ret;
 173 }
 174
 175 static __init int psci_cpu_init_idle(struct cpuidle_driver *drv,
 176                                      unsigned int cpu, unsigned int state_count)
 177 {
 178         struct device_node *cpu_node;
 179         int ret;
 180
 181         /*
 182          * If the PSCI cpu_suspend function hook has not been initialized
 183          * idle states must not be enabled, so bail out
 184          */
 185         if (!psci_ops.cpu_suspend)
 186                 return -EOPNOTSUPP;
 187
 188         cpu_node = of_cpu_device_node_get(cpu);
 189         if (!cpu_node)
 190                 return -ENODEV;
 191
 192         ret = psci_dt_cpu_init_idle(drv, cpu_node, state_count, cpu);
 193
 194         of_node_put(cpu_node);
 195
 196         return ret;
 197 }
 198
 199 static int __init psci_idle_init_cpu(int cpu)
 200 {
 201         struct cpuidle_driver *drv;
 202         struct device_node *cpu_node;
 203         const char *enable_method;
 204         int ret = 0;
 205
 206         cpu_node = of_cpu_device_node_get(cpu);
 207         if (!cpu_node)
 208                 return -ENODEV;
 209
 210         /*
 211          * Check whether the enable-method for the cpu is PSCI, fail
 212          * if it is not.
 213          */
 214         enable_method = of_get_property(cpu_node, "enable-method", NULL);
 215         if (!enable_method || (strcmp(enable_method, "psci")))
 216                 ret = -ENODEV;
 217
 218         of_node_put(cpu_node);
 219         if (ret)
 220                 return ret;
 221
 222         drv = kmemdup(&psci_idle_driver, sizeof(*drv), GFP_KERNEL);
 223         if (!drv)
 224                 return -ENOMEM;
 225
 226         drv->cpumask = (struct cpumask *)cpumask_of(cpu);
 227
 228         /*
 229          * Initialize idle states data, starting at index 1, since
 230          * by default idle state 0 is the quiescent state reached
 231          * by the cpu by executing the wfi instruction.
 232          *
 233          * If no DT idle states are detected (ret == 0) let the driver
 234          * initialization fail accordingly since there is no reason to
 235          * initialize the idle driver if only wfi is supported, the
 236          * default archictectural back-end already executes wfi
 237          * on idle entry.
 238          */
 239         ret = dt_init_idle_driver(drv, psci_idle_state_match, 1);
 240         if (ret <= 0) {
 241                 ret = ret ? : -ENODEV;
 242                 goto out_kfree_drv;
 243         }
 244
 245         /*
 246          * Initialize PSCI idle states.
 247          */
 248         ret = psci_cpu_init_idle(drv, cpu, ret);
 249         if (ret) {
 250                 pr_err("CPU %d failed to PSCI idle\n", cpu);
 251                 goto out_kfree_drv;
 252         }
 253
 254         ret = cpuidle_register(drv, NULL);
 255         if (ret)
 256                 goto out_kfree_drv;
 257
 258         return 0;
 259
 260 out_kfree_drv:
 261         kfree(drv);
 262         return ret;
 263 }
 264
 265 /*
 266  * psci_idle_init - Initializes PSCI cpuidle driver
 267  *
 268  * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails
 269  * to register cpuidle driver then rollback to cancel all CPUs
 270  * registration.
 271  */
 272 static int __init psci_idle_init(void)
 273 {
 274         int cpu, ret;
 275         struct cpuidle_driver *drv;
 276         struct cpuidle_device *dev;
 277
 278         for_each_possible_cpu(cpu) {
 279                 ret = psci_idle_init_cpu(cpu);
 280                 if (ret)
 281                         goto out_fail;
 282         }
 283
 284         return 0;
 285
 286 out_fail:
 287         while (--cpu >= 0) {
 288                 dev = per_cpu(cpuidle_devices, cpu);
 289                 drv = cpuidle_get_cpu_driver(dev);
 290                 cpuidle_unregister(drv);
 291                 kfree(drv);
 292         }
 293
 294         return ret;
 295 }
 296 device_initcall(psci_idle_init);