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 "dt_idle_states.h" 24 25 static DEFINE_PER_CPU_READ_MOSTLY(u32 *, psci_power_state); 26 27 static int psci_enter_idle_state(struct cpuidle_device *dev, 28 struct cpuidle_driver *drv, int idx) 29 { 30 u32 *state = __this_cpu_read(psci_power_state); 31 32 return CPU_PM_CPU_IDLE_ENTER_PARAM(psci_cpu_suspend_enter, 33 idx, state[idx - 1]); 34 } 35 36 static struct cpuidle_driver psci_idle_driver __initdata = { 37 .name = "psci_idle", 38 .owner = THIS_MODULE, 39 /* 40 * PSCI idle states relies on architectural WFI to 41 * be represented as state index 0. 42 */ 43 .states[0] = { 44 .enter = psci_enter_idle_state, 45 .exit_latency = 1, 46 .target_residency = 1, 47 .power_usage = UINT_MAX, 48 .name = "WFI", 49 .desc = "ARM WFI", 50 } 51 }; 52 53 static const struct of_device_id psci_idle_state_match[] __initconst = { 54 { .compatible = "arm,idle-state", 55 .data = psci_enter_idle_state }, 56 { }, 57 }; 58 59 static int __init psci_dt_parse_state_node(struct device_node *np, u32 *state) 60 { 61 int err = of_property_read_u32(np, "arm,psci-suspend-param", state); 62 63 if (err) { 64 pr_warn("%pOF missing arm,psci-suspend-param property\n", np); 65 return err; 66 } 67 68 if (!psci_power_state_is_valid(*state)) { 69 pr_warn("Invalid PSCI power state %#x\n", *state); 70 return -EINVAL; 71 } 72 73 return 0; 74 } 75 76 static int __init psci_dt_cpu_init_idle(struct device_node *cpu_node, int cpu) 77 { 78 int i, ret = 0, count = 0; 79 u32 *psci_states; 80 struct device_node *state_node; 81 82 /* Count idle states */ 83 while ((state_node = of_parse_phandle(cpu_node, "cpu-idle-states", 84 count))) { 85 count++; 86 of_node_put(state_node); 87 } 88 89 if (!count) 90 return -ENODEV; 91 92 psci_states = kcalloc(count, sizeof(*psci_states), GFP_KERNEL); 93 if (!psci_states) 94 return -ENOMEM; 95 96 for (i = 0; i < count; i++) { 97 state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i); 98 ret = psci_dt_parse_state_node(state_node, &psci_states[i]); 99 of_node_put(state_node); 100 101 if (ret) 102 goto free_mem; 103 104 pr_debug("psci-power-state %#x index %d\n", psci_states[i], i); 105 } 106 107 /* Idle states parsed correctly, initialize per-cpu pointer */ 108 per_cpu(psci_power_state, cpu) = psci_states; 109 return 0; 110 111 free_mem: 112 kfree(psci_states); 113 return ret; 114 } 115 116 static __init int psci_cpu_init_idle(unsigned int cpu) 117 { 118 struct device_node *cpu_node; 119 int ret; 120 121 /* 122 * If the PSCI cpu_suspend function hook has not been initialized 123 * idle states must not be enabled, so bail out 124 */ 125 if (!psci_ops.cpu_suspend) 126 return -EOPNOTSUPP; 127 128 cpu_node = of_cpu_device_node_get(cpu); 129 if (!cpu_node) 130 return -ENODEV; 131 132 ret = psci_dt_cpu_init_idle(cpu_node, cpu); 133 134 of_node_put(cpu_node); 135 136 return ret; 137 } 138 139 static int __init psci_idle_init_cpu(int cpu) 140 { 141 struct cpuidle_driver *drv; 142 struct device_node *cpu_node; 143 const char *enable_method; 144 int ret = 0; 145 146 cpu_node = of_cpu_device_node_get(cpu); 147 if (!cpu_node) 148 return -ENODEV; 149 150 /* 151 * Check whether the enable-method for the cpu is PSCI, fail 152 * if it is not. 153 */ 154 enable_method = of_get_property(cpu_node, "enable-method", NULL); 155 if (!enable_method || (strcmp(enable_method, "psci"))) 156 ret = -ENODEV; 157 158 of_node_put(cpu_node); 159 if (ret) 160 return ret; 161 162 drv = kmemdup(&psci_idle_driver, sizeof(*drv), GFP_KERNEL); 163 if (!drv) 164 return -ENOMEM; 165 166 drv->cpumask = (struct cpumask *)cpumask_of(cpu); 167 168 /* 169 * Initialize idle states data, starting at index 1, since 170 * by default idle state 0 is the quiescent state reached 171 * by the cpu by executing the wfi instruction. 172 * 173 * If no DT idle states are detected (ret == 0) let the driver 174 * initialization fail accordingly since there is no reason to 175 * initialize the idle driver if only wfi is supported, the 176 * default archictectural back-end already executes wfi 177 * on idle entry. 178 */ 179 ret = dt_init_idle_driver(drv, psci_idle_state_match, 1); 180 if (ret <= 0) { 181 ret = ret ? : -ENODEV; 182 goto out_kfree_drv; 183 } 184 185 /* 186 * Initialize PSCI idle states. 187 */ 188 ret = psci_cpu_init_idle(cpu); 189 if (ret) { 190 pr_err("CPU %d failed to PSCI idle\n", cpu); 191 goto out_kfree_drv; 192 } 193 194 ret = cpuidle_register(drv, NULL); 195 if (ret) 196 goto out_kfree_drv; 197 198 return 0; 199 200 out_kfree_drv: 201 kfree(drv); 202 return ret; 203 } 204 205 /* 206 * psci_idle_init - Initializes PSCI cpuidle driver 207 * 208 * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails 209 * to register cpuidle driver then rollback to cancel all CPUs 210 * registration. 211 */ 212 static int __init psci_idle_init(void) 213 { 214 int cpu, ret; 215 struct cpuidle_driver *drv; 216 struct cpuidle_device *dev; 217 218 for_each_possible_cpu(cpu) { 219 ret = psci_idle_init_cpu(cpu); 220 if (ret) 221 goto out_fail; 222 } 223 224 return 0; 225 226 out_fail: 227 while (--cpu >= 0) { 228 dev = per_cpu(cpuidle_devices, cpu); 229 drv = cpuidle_get_cpu_driver(dev); 230 cpuidle_unregister(drv); 231 kfree(drv); 232 } 233 234 return ret; 235 } 236 device_initcall(psci_idle_init); 237