[PATCH kvmtool v3 11/17] aarch64: psci: Implement CPU_SUSPEND

[Oliver Upton <oliver.upton@linux.dev>]

Implement support for PSCI CPU_SUSPEND, leveraging in-kernel suspend
emulation (i.e. a WFI state). Eagerly resume the vCPU for any wakeup
event.

Signed-off-by: Oliver Upton <oliver.upton@linux.dev>
---
 arm/aarch64/kvm-cpu.c | 18 ++++++++++++++++++
 arm/aarch64/psci.c    | 19 +++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/arm/aarch64/kvm-cpu.c b/arm/aarch64/kvm-cpu.c
index 4feed9f41cb0..316e20c7f157 100644
--- a/arm/aarch64/kvm-cpu.c
+++ b/arm/aarch64/kvm-cpu.c
@@ -263,6 +263,16 @@ void kvm_cpu__show_registers(struct kvm_cpu *vcpu)
 	dprintf(debug_fd, " LR:    0x%lx\n", data);
 }
 
+static void handle_wakeup(struct kvm_cpu *vcpu)
+{
+	struct kvm_mp_state mp_state = {
+		.mp_state = KVM_MP_STATE_RUNNABLE,
+	};
+
+	if (ioctl(vcpu->vcpu_fd, KVM_SET_MP_STATE, &mp_state))
+		die_perror("KVM_SET_MP_STATE failed");
+}
+
 bool kvm_cpu__handle_exit(struct kvm_cpu *vcpu)
 {
 	struct kvm_run *run = vcpu->kvm_run;
@@ -271,6 +281,14 @@ bool kvm_cpu__handle_exit(struct kvm_cpu *vcpu)
 	case KVM_EXIT_HYPERCALL:
 		handle_hypercall(vcpu);
 		return true;
+	case KVM_EXIT_SYSTEM_EVENT:
+		switch (run->system_event.type) {
+		case KVM_SYSTEM_EVENT_WAKEUP:
+			handle_wakeup(vcpu);
+			return true;
+		default:
+			return false;
+		}
 	default:
 		return false;
 	}
diff --git a/arm/aarch64/psci.c b/arm/aarch64/psci.c
index 482b9a7442c6..abfdc764b7e0 100644
--- a/arm/aarch64/psci.c
+++ b/arm/aarch64/psci.c
@@ -15,12 +15,27 @@ static void psci_features(struct kvm_cpu *vcpu, struct arm_smccc_res *res)
 		return;
 
 	switch (arg) {
+	case PSCI_0_2_FN_CPU_SUSPEND:
+	case PSCI_0_2_FN64_CPU_SUSPEND:
 	case ARM_SMCCC_VERSION_FUNC_ID:
 		res->a0 = PSCI_RET_SUCCESS;
 		break;
 	}
 }
 
+static void cpu_suspend(struct kvm_cpu *vcpu, struct arm_smccc_res *res)
+{
+	struct kvm_mp_state mp_state = {
+		.mp_state	= KVM_MP_STATE_SUSPENDED,
+	};
+
+	/* Rely on in-kernel emulation of a 'suspended' (i.e. WFI) state. */
+	if (ioctl(vcpu->vcpu_fd, KVM_SET_MP_STATE, &mp_state))
+		die_perror("KVM_SET_MP_STATE failed");
+
+	res->a0 = PSCI_RET_SUCCESS;
+}
+
 void handle_psci(struct kvm_cpu *vcpu, struct arm_smccc_res *res)
 {
 	switch (vcpu->kvm_run->hypercall.nr) {
@@ -30,6 +45,10 @@ void handle_psci(struct kvm_cpu *vcpu, struct arm_smccc_res *res)
 	case PSCI_1_0_FN_PSCI_FEATURES:
 		psci_features(vcpu, res);
 		break;
+	case PSCI_0_2_FN_CPU_SUSPEND:
+	case PSCI_0_2_FN64_CPU_SUSPEND:
+		cpu_suspend(vcpu, res);
+		break;
 	default:
 		res->a0 = PSCI_RET_NOT_SUPPORTED;
 	}
-- 
2.41.0.585.gd2178a4bd4-goog