1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * Copyright (C) 2020 - Google LLC
4  * Author: David Brazdil <dbrazdil@google.com>
5  */
6 
7 #include <asm/kvm_asm.h>
8 #include <asm/kvm_hyp.h>
9 #include <asm/kvm_mmu.h>
10 #include <linux/arm-smccc.h>
11 #include <linux/kvm_host.h>
12 #include <uapi/linux/psci.h>
13 
14 #include <nvhe/memory.h>
15 #include <nvhe/trap_handler.h>
16 
17 void kvm_hyp_cpu_entry(unsigned long r0);
18 void kvm_hyp_cpu_resume(unsigned long r0);
19 
20 void __noreturn __host_enter(struct kvm_cpu_context *host_ctxt);
21 
22 /* Config options set by the host. */
23 struct kvm_host_psci_config __ro_after_init kvm_host_psci_config;
24 
25 #define INVALID_CPU_ID	UINT_MAX
26 
27 struct psci_boot_args {
28 	atomic_t lock;
29 	unsigned long pc;
30 	unsigned long r0;
31 };
32 
33 #define PSCI_BOOT_ARGS_UNLOCKED		0
34 #define PSCI_BOOT_ARGS_LOCKED		1
35 
36 #define PSCI_BOOT_ARGS_INIT					\
37 	((struct psci_boot_args){				\
38 		.lock = ATOMIC_INIT(PSCI_BOOT_ARGS_UNLOCKED),	\
39 	})
40 
41 static DEFINE_PER_CPU(struct psci_boot_args, cpu_on_args) = PSCI_BOOT_ARGS_INIT;
42 static DEFINE_PER_CPU(struct psci_boot_args, suspend_args) = PSCI_BOOT_ARGS_INIT;
43 
44 #define	is_psci_0_1(what, func_id)					\
45 	(kvm_host_psci_config.psci_0_1_ ## what ## _implemented &&	\
46 	 (func_id) == kvm_host_psci_config.function_ids_0_1.what)
47 
is_psci_0_1_call(u64 func_id)48 static bool is_psci_0_1_call(u64 func_id)
49 {
50 	return (is_psci_0_1(cpu_suspend, func_id) ||
51 		is_psci_0_1(cpu_on, func_id) ||
52 		is_psci_0_1(cpu_off, func_id) ||
53 		is_psci_0_1(migrate, func_id));
54 }
55 
is_psci_0_2_call(u64 func_id)56 static bool is_psci_0_2_call(u64 func_id)
57 {
58 	/* SMCCC reserves IDs 0x00-1F with the given 32/64-bit base for PSCI. */
59 	return (PSCI_0_2_FN(0) <= func_id && func_id <= PSCI_0_2_FN(31)) ||
60 	       (PSCI_0_2_FN64(0) <= func_id && func_id <= PSCI_0_2_FN64(31));
61 }
62 
psci_call(unsigned long fn,unsigned long arg0,unsigned long arg1,unsigned long arg2)63 static unsigned long psci_call(unsigned long fn, unsigned long arg0,
64 			       unsigned long arg1, unsigned long arg2)
65 {
66 	struct arm_smccc_res res;
67 
68 	arm_smccc_1_1_smc(fn, arg0, arg1, arg2, &res);
69 	return res.a0;
70 }
71 
psci_forward(struct kvm_cpu_context * host_ctxt)72 static unsigned long psci_forward(struct kvm_cpu_context *host_ctxt)
73 {
74 	return psci_call(cpu_reg(host_ctxt, 0), cpu_reg(host_ctxt, 1),
75 			 cpu_reg(host_ctxt, 2), cpu_reg(host_ctxt, 3));
76 }
77 
find_cpu_id(u64 mpidr)78 static unsigned int find_cpu_id(u64 mpidr)
79 {
80 	unsigned int i;
81 
82 	/* Reject invalid MPIDRs */
83 	if (mpidr & ~MPIDR_HWID_BITMASK)
84 		return INVALID_CPU_ID;
85 
86 	for (i = 0; i < NR_CPUS; i++) {
87 		if (cpu_logical_map(i) == mpidr)
88 			return i;
89 	}
90 
91 	return INVALID_CPU_ID;
92 }
93 
try_acquire_boot_args(struct psci_boot_args * args)94 static __always_inline bool try_acquire_boot_args(struct psci_boot_args *args)
95 {
96 	return atomic_cmpxchg_acquire(&args->lock,
97 				      PSCI_BOOT_ARGS_UNLOCKED,
98 				      PSCI_BOOT_ARGS_LOCKED) ==
99 		PSCI_BOOT_ARGS_UNLOCKED;
100 }
101 
release_boot_args(struct psci_boot_args * args)102 static __always_inline void release_boot_args(struct psci_boot_args *args)
103 {
104 	atomic_set_release(&args->lock, PSCI_BOOT_ARGS_UNLOCKED);
105 }
106 
psci_cpu_on(u64 func_id,struct kvm_cpu_context * host_ctxt)107 static int psci_cpu_on(u64 func_id, struct kvm_cpu_context *host_ctxt)
108 {
109 	DECLARE_REG(u64, mpidr, host_ctxt, 1);
110 	DECLARE_REG(unsigned long, pc, host_ctxt, 2);
111 	DECLARE_REG(unsigned long, r0, host_ctxt, 3);
112 
113 	unsigned int cpu_id;
114 	struct psci_boot_args *boot_args;
115 	struct kvm_nvhe_init_params *init_params;
116 	int ret;
117 
118 	/*
119 	 * Find the logical CPU ID for the given MPIDR. The search set is
120 	 * the set of CPUs that were online at the point of KVM initialization.
121 	 * Booting other CPUs is rejected because their cpufeatures were not
122 	 * checked against the finalized capabilities. This could be relaxed
123 	 * by doing the feature checks in hyp.
124 	 */
125 	cpu_id = find_cpu_id(mpidr);
126 	if (cpu_id == INVALID_CPU_ID)
127 		return PSCI_RET_INVALID_PARAMS;
128 
129 	boot_args = per_cpu_ptr(&cpu_on_args, cpu_id);
130 	init_params = per_cpu_ptr(&kvm_init_params, cpu_id);
131 
132 	/* Check if the target CPU is already being booted. */
133 	if (!try_acquire_boot_args(boot_args))
134 		return PSCI_RET_ALREADY_ON;
135 
136 	boot_args->pc = pc;
137 	boot_args->r0 = r0;
138 	wmb();
139 
140 	ret = psci_call(func_id, mpidr,
141 			__hyp_pa(&kvm_hyp_cpu_entry),
142 			__hyp_pa(init_params));
143 
144 	/* If successful, the lock will be released by the target CPU. */
145 	if (ret != PSCI_RET_SUCCESS)
146 		release_boot_args(boot_args);
147 
148 	return ret;
149 }
150 
psci_cpu_suspend(u64 func_id,struct kvm_cpu_context * host_ctxt)151 static int psci_cpu_suspend(u64 func_id, struct kvm_cpu_context *host_ctxt)
152 {
153 	DECLARE_REG(u64, power_state, host_ctxt, 1);
154 	DECLARE_REG(unsigned long, pc, host_ctxt, 2);
155 	DECLARE_REG(unsigned long, r0, host_ctxt, 3);
156 
157 	struct psci_boot_args *boot_args;
158 	struct kvm_nvhe_init_params *init_params;
159 
160 	boot_args = this_cpu_ptr(&suspend_args);
161 	init_params = this_cpu_ptr(&kvm_init_params);
162 
163 	/*
164 	 * No need to acquire a lock before writing to boot_args because a core
165 	 * can only suspend itself. Racy CPU_ON calls use a separate struct.
166 	 */
167 	boot_args->pc = pc;
168 	boot_args->r0 = r0;
169 
170 	/*
171 	 * Will either return if shallow sleep state, or wake up into the entry
172 	 * point if it is a deep sleep state.
173 	 */
174 	return psci_call(func_id, power_state,
175 			 __hyp_pa(&kvm_hyp_cpu_resume),
176 			 __hyp_pa(init_params));
177 }
178 
psci_system_suspend(u64 func_id,struct kvm_cpu_context * host_ctxt)179 static int psci_system_suspend(u64 func_id, struct kvm_cpu_context *host_ctxt)
180 {
181 	DECLARE_REG(unsigned long, pc, host_ctxt, 1);
182 	DECLARE_REG(unsigned long, r0, host_ctxt, 2);
183 
184 	struct psci_boot_args *boot_args;
185 	struct kvm_nvhe_init_params *init_params;
186 
187 	boot_args = this_cpu_ptr(&suspend_args);
188 	init_params = this_cpu_ptr(&kvm_init_params);
189 
190 	/*
191 	 * No need to acquire a lock before writing to boot_args because a core
192 	 * can only suspend itself. Racy CPU_ON calls use a separate struct.
193 	 */
194 	boot_args->pc = pc;
195 	boot_args->r0 = r0;
196 
197 	/* Will only return on error. */
198 	return psci_call(func_id,
199 			 __hyp_pa(&kvm_hyp_cpu_resume),
200 			 __hyp_pa(init_params), 0);
201 }
202 
kvm_host_psci_cpu_entry(bool is_cpu_on)203 asmlinkage void __noreturn kvm_host_psci_cpu_entry(bool is_cpu_on)
204 {
205 	struct psci_boot_args *boot_args;
206 	struct kvm_cpu_context *host_ctxt;
207 
208 	host_ctxt = &this_cpu_ptr(&kvm_host_data)->host_ctxt;
209 
210 	if (is_cpu_on)
211 		boot_args = this_cpu_ptr(&cpu_on_args);
212 	else
213 		boot_args = this_cpu_ptr(&suspend_args);
214 
215 	cpu_reg(host_ctxt, 0) = boot_args->r0;
216 	write_sysreg_el2(boot_args->pc, SYS_ELR);
217 
218 	if (is_cpu_on)
219 		release_boot_args(boot_args);
220 
221 	__host_enter(host_ctxt);
222 }
223 
psci_0_1_handler(u64 func_id,struct kvm_cpu_context * host_ctxt)224 static unsigned long psci_0_1_handler(u64 func_id, struct kvm_cpu_context *host_ctxt)
225 {
226 	if (is_psci_0_1(cpu_off, func_id) || is_psci_0_1(migrate, func_id))
227 		return psci_forward(host_ctxt);
228 	if (is_psci_0_1(cpu_on, func_id))
229 		return psci_cpu_on(func_id, host_ctxt);
230 	if (is_psci_0_1(cpu_suspend, func_id))
231 		return psci_cpu_suspend(func_id, host_ctxt);
232 
233 	return PSCI_RET_NOT_SUPPORTED;
234 }
235 
psci_0_2_handler(u64 func_id,struct kvm_cpu_context * host_ctxt)236 static unsigned long psci_0_2_handler(u64 func_id, struct kvm_cpu_context *host_ctxt)
237 {
238 	switch (func_id) {
239 	case PSCI_0_2_FN_PSCI_VERSION:
240 	case PSCI_0_2_FN_CPU_OFF:
241 	case PSCI_0_2_FN64_AFFINITY_INFO:
242 	case PSCI_0_2_FN64_MIGRATE:
243 	case PSCI_0_2_FN_MIGRATE_INFO_TYPE:
244 	case PSCI_0_2_FN64_MIGRATE_INFO_UP_CPU:
245 		return psci_forward(host_ctxt);
246 	/*
247 	 * SYSTEM_OFF/RESET should not return according to the spec.
248 	 * Allow it so as to stay robust to broken firmware.
249 	 */
250 	case PSCI_0_2_FN_SYSTEM_OFF:
251 	case PSCI_0_2_FN_SYSTEM_RESET:
252 		return psci_forward(host_ctxt);
253 	case PSCI_0_2_FN64_CPU_SUSPEND:
254 		return psci_cpu_suspend(func_id, host_ctxt);
255 	case PSCI_0_2_FN64_CPU_ON:
256 		return psci_cpu_on(func_id, host_ctxt);
257 	default:
258 		return PSCI_RET_NOT_SUPPORTED;
259 	}
260 }
261 
psci_1_0_handler(u64 func_id,struct kvm_cpu_context * host_ctxt)262 static unsigned long psci_1_0_handler(u64 func_id, struct kvm_cpu_context *host_ctxt)
263 {
264 	switch (func_id) {
265 	case PSCI_1_0_FN_PSCI_FEATURES:
266 	case PSCI_1_0_FN_SET_SUSPEND_MODE:
267 	case PSCI_1_1_FN64_SYSTEM_RESET2:
268 		return psci_forward(host_ctxt);
269 	case PSCI_1_0_FN64_SYSTEM_SUSPEND:
270 		return psci_system_suspend(func_id, host_ctxt);
271 	default:
272 		return psci_0_2_handler(func_id, host_ctxt);
273 	}
274 }
275 
kvm_host_psci_handler(struct kvm_cpu_context * host_ctxt)276 bool kvm_host_psci_handler(struct kvm_cpu_context *host_ctxt)
277 {
278 	DECLARE_REG(u64, func_id, host_ctxt, 0);
279 	unsigned long ret;
280 
281 	switch (kvm_host_psci_config.version) {
282 	case PSCI_VERSION(0, 1):
283 		if (!is_psci_0_1_call(func_id))
284 			return false;
285 		ret = psci_0_1_handler(func_id, host_ctxt);
286 		break;
287 	case PSCI_VERSION(0, 2):
288 		if (!is_psci_0_2_call(func_id))
289 			return false;
290 		ret = psci_0_2_handler(func_id, host_ctxt);
291 		break;
292 	default:
293 		if (!is_psci_0_2_call(func_id))
294 			return false;
295 		ret = psci_1_0_handler(func_id, host_ctxt);
296 		break;
297 	}
298 
299 	cpu_reg(host_ctxt, 0) = ret;
300 	cpu_reg(host_ctxt, 1) = 0;
301 	cpu_reg(host_ctxt, 2) = 0;
302 	cpu_reg(host_ctxt, 3) = 0;
303 	return true;
304 }
305