1 /*
2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License version 2 as
4 * published by the Free Software Foundation.
5 *
6 * This program is distributed in the hope that it will be useful,
7 * but WITHOUT ANY WARRANTY; without even the implied warranty of
8 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 * GNU General Public License for more details.
10 *
11 * Copyright (C) 2013 ARM Limited
12 *
13 * Author: Will Deacon <will.deacon@arm.com>
14 */
15
16 #define pr_fmt(fmt) "psci: " fmt
17
18 #include <linux/init.h>
19 #include <linux/of.h>
20 #include <linux/smp.h>
21 #include <linux/delay.h>
22 #include <linux/mm.h>
23 #include <linux/psci.h>
24 #include <linux/slab.h>
25
26 #include <uapi/linux/psci.h>
27
28 #include <asm/compiler.h>
29 #include <asm/cpu_ops.h>
30 #include <asm/errno.h>
31 #include <asm/smp_plat.h>
32 #include <asm/suspend.h>
33
34 static DEFINE_PER_CPU_READ_MOSTLY(u32 *, psci_power_state);
35
cpu_psci_cpu_init_idle(unsigned int cpu)36 static int __maybe_unused cpu_psci_cpu_init_idle(unsigned int cpu)
37 {
38 int i, ret, count = 0;
39 u32 *psci_states;
40 struct device_node *state_node, *cpu_node;
41
42 cpu_node = of_get_cpu_node(cpu, NULL);
43 if (!cpu_node)
44 return -ENODEV;
45
46 /*
47 * If the PSCI cpu_suspend function hook has not been initialized
48 * idle states must not be enabled, so bail out
49 */
50 if (!psci_ops.cpu_suspend)
51 return -EOPNOTSUPP;
52
53 /* Count idle states */
54 while ((state_node = of_parse_phandle(cpu_node, "cpu-idle-states",
55 count))) {
56 count++;
57 of_node_put(state_node);
58 }
59
60 if (!count)
61 return -ENODEV;
62
63 psci_states = kcalloc(count, sizeof(*psci_states), GFP_KERNEL);
64 if (!psci_states)
65 return -ENOMEM;
66
67 for (i = 0; i < count; i++) {
68 u32 state;
69
70 state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i);
71
72 ret = of_property_read_u32(state_node,
73 "arm,psci-suspend-param",
74 &state);
75 if (ret) {
76 pr_warn(" * %s missing arm,psci-suspend-param property\n",
77 state_node->full_name);
78 of_node_put(state_node);
79 goto free_mem;
80 }
81
82 of_node_put(state_node);
83 pr_debug("psci-power-state %#x index %d\n", state, i);
84 if (!psci_power_state_is_valid(state)) {
85 pr_warn("Invalid PSCI power state %#x\n", state);
86 ret = -EINVAL;
87 goto free_mem;
88 }
89 psci_states[i] = state;
90 }
91 /* Idle states parsed correctly, initialize per-cpu pointer */
92 per_cpu(psci_power_state, cpu) = psci_states;
93 return 0;
94
95 free_mem:
96 kfree(psci_states);
97 return ret;
98 }
99
cpu_psci_cpu_init(unsigned int cpu)100 static int __init cpu_psci_cpu_init(unsigned int cpu)
101 {
102 return 0;
103 }
104
cpu_psci_cpu_prepare(unsigned int cpu)105 static int __init cpu_psci_cpu_prepare(unsigned int cpu)
106 {
107 if (!psci_ops.cpu_on) {
108 pr_err("no cpu_on method, not booting CPU%d\n", cpu);
109 return -ENODEV;
110 }
111
112 return 0;
113 }
114
cpu_psci_cpu_boot(unsigned int cpu)115 static int cpu_psci_cpu_boot(unsigned int cpu)
116 {
117 int err = psci_ops.cpu_on(cpu_logical_map(cpu),
118 __pa_symbol(secondary_entry));
119 if (err)
120 pr_err("failed to boot CPU%d (%d)\n", cpu, err);
121
122 return err;
123 }
124
125 #ifdef CONFIG_HOTPLUG_CPU
cpu_psci_cpu_disable(unsigned int cpu)126 static int cpu_psci_cpu_disable(unsigned int cpu)
127 {
128 /* Fail early if we don't have CPU_OFF support */
129 if (!psci_ops.cpu_off)
130 return -EOPNOTSUPP;
131
132 /* Trusted OS will deny CPU_OFF */
133 if (psci_tos_resident_on(cpu))
134 return -EPERM;
135
136 return 0;
137 }
138
cpu_psci_cpu_die(unsigned int cpu)139 static void cpu_psci_cpu_die(unsigned int cpu)
140 {
141 /*
142 * There are no known implementations of PSCI actually using the
143 * power state field, pass a sensible default for now.
144 */
145 u32 state = PSCI_POWER_STATE_TYPE_POWER_DOWN <<
146 PSCI_0_2_POWER_STATE_TYPE_SHIFT;
147
148 psci_ops.cpu_off(state);
149 }
150
cpu_psci_cpu_kill(unsigned int cpu)151 static int cpu_psci_cpu_kill(unsigned int cpu)
152 {
153 int err;
154 unsigned long start, end;
155
156 if (!psci_ops.affinity_info)
157 return 0;
158 /*
159 * cpu_kill could race with cpu_die and we can
160 * potentially end up declaring this cpu undead
161 * while it is dying. So, try again a few times.
162 */
163
164 start = jiffies;
165 end = start + msecs_to_jiffies(100);
166 do {
167 err = psci_ops.affinity_info(cpu_logical_map(cpu), 0);
168 if (err == PSCI_0_2_AFFINITY_LEVEL_OFF) {
169 pr_info("CPU%d killed (polled %d ms)\n", cpu,
170 jiffies_to_msecs(jiffies - start));
171 return 0;
172 }
173
174 usleep_range(100, 1000);
175 } while (time_before(jiffies, end));
176
177 pr_warn("CPU%d may not have shut down cleanly (AFFINITY_INFO reports %d)\n",
178 cpu, err);
179 return -ETIMEDOUT;
180 }
181 #endif
182
psci_suspend_finisher(unsigned long index)183 static int psci_suspend_finisher(unsigned long index)
184 {
185 u32 *state = __this_cpu_read(psci_power_state);
186
187 return psci_ops.cpu_suspend(state[index - 1],
188 virt_to_phys(cpu_resume));
189 }
190
cpu_psci_cpu_suspend(unsigned long index)191 static int __maybe_unused cpu_psci_cpu_suspend(unsigned long index)
192 {
193 int ret;
194 u32 *state = __this_cpu_read(psci_power_state);
195 /*
196 * idle state index 0 corresponds to wfi, should never be called
197 * from the cpu_suspend operations
198 */
199 if (WARN_ON_ONCE(!index))
200 return -EINVAL;
201
202 if (!psci_power_state_loses_context(state[index - 1]))
203 ret = psci_ops.cpu_suspend(state[index - 1], 0);
204 else
205 ret = cpu_suspend(index, psci_suspend_finisher);
206
207 return ret;
208 }
209
210 const struct cpu_operations cpu_psci_ops = {
211 .name = "psci",
212 #ifdef CONFIG_CPU_IDLE
213 .cpu_init_idle = cpu_psci_cpu_init_idle,
214 .cpu_suspend = cpu_psci_cpu_suspend,
215 #endif
216 .cpu_init = cpu_psci_cpu_init,
217 .cpu_prepare = cpu_psci_cpu_prepare,
218 .cpu_boot = cpu_psci_cpu_boot,
219 #ifdef CONFIG_HOTPLUG_CPU
220 .cpu_disable = cpu_psci_cpu_disable,
221 .cpu_die = cpu_psci_cpu_die,
222 .cpu_kill = cpu_psci_cpu_kill,
223 #endif
224 };
225
226