[PATCH v4 10/10] ARM/ARM64: KVM: Emulate PSCI v0.2 CPU_SUSPEND

[anup.patel@linaro.org (Anup Patel)]

This patch adds emulation of PSCI v0.2 CPU_SUSPEND function call for
KVM ARM/ARM64. This is a VCPU-level function call which can suspend
current VCPU or all VCPUs within current VCPU's affinity level.

The CPU_SUSPEND emulation is not tested much because currently there
is no CPUIDLE driver in Linux kernel that uses PSCI CPU_SUSPEND. The
PSCI CPU_SUSPEND implementation in ARM64 kernel was tested using a
Simple CPUIDLE driver which is not published due to unstable DT-bindings
for PSCI.
(For more info, http://lwn.net/Articles/574950/)

Even if we had stable DT-bindings for PSCI and CPUIDLE driver that
uses PSCI CPU_SUSPEND then still we need to define SUSPEND states
for KVM ARM/ARM64. Due to this, the CPU_SUSPEND emulation added
by this patch only pause a VCPU and to wakeup a VCPU we need to
explicity call PSCI CPU_ON from Guest.

Signed-off-by: Anup Patel <anup.patel@linaro.org>
Signed-off-by: Pranavkumar Sawargaonkar <pranavkumar@linaro.org>
---
 arch/arm/include/asm/kvm_host.h   |    5 +++
 arch/arm/include/asm/kvm_psci.h   |    1 +
 arch/arm/kvm/psci.c               |   88 +++++++++++++++++++++++++++++++++++--
 arch/arm/kvm/reset.c              |    4 ++
 arch/arm64/include/asm/kvm_host.h |    5 +++
 arch/arm64/include/asm/kvm_psci.h |    1 +
 arch/arm64/kvm/reset.c            |    4 ++
 7 files changed, 104 insertions(+), 4 deletions(-)

diff --git a/arch/arm/include/asm/kvm_host.h b/arch/arm/include/asm/kvm_host.h
index 193ceaf..2cc36a6 100644
--- a/arch/arm/include/asm/kvm_host.h
+++ b/arch/arm/include/asm/kvm_host.h
@@ -131,6 +131,11 @@ struct kvm_vcpu_arch {
 	/* Don't run the guest on this vcpu */
 	bool pause;
 
+	/* PSCI suspend state */
+	bool suspend;
+	u32 suspend_entry;
+	u32 suspend_context_id;
+
 	/* IO related fields */
 	struct kvm_decode mmio_decode;
 
diff --git a/arch/arm/include/asm/kvm_psci.h b/arch/arm/include/asm/kvm_psci.h
index 6bda945..6a05ada 100644
--- a/arch/arm/include/asm/kvm_psci.h
+++ b/arch/arm/include/asm/kvm_psci.h
@@ -22,6 +22,7 @@
 #define KVM_ARM_PSCI_0_2	2
 
 int kvm_psci_version(struct kvm_vcpu *vcpu);
+void kvm_psci_reset(struct kvm_vcpu *vcpu);
 int kvm_psci_call(struct kvm_vcpu *vcpu);
 
 #endif /* __ARM_KVM_PSCI_H__ */
diff --git a/arch/arm/kvm/psci.c b/arch/arm/kvm/psci.c
index 675866e..482b0f6 100644
--- a/arch/arm/kvm/psci.c
+++ b/arch/arm/kvm/psci.c
@@ -15,6 +15,7 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <linux/smp.h>
 #include <linux/kvm_host.h>
 #include <linux/wait.h>
 
@@ -27,9 +28,81 @@
  * as described in ARM document number ARM DEN 0022A.
  */
 
+struct psci_suspend_info {
+	struct kvm_vcpu *vcpu;
+	unsigned long saved_entry;
+	unsigned long saved_context_id;
+};
+
+static void psci_do_suspend(void *context)
+{
+	struct psci_suspend_info *sinfo = context;
+
+	sinfo->vcpu->arch.pause = true;
+	sinfo->vcpu->arch.suspend = true;
+	sinfo->vcpu->arch.suspend_entry = sinfo->saved_entry;
+	sinfo->vcpu->arch.suspend_context_id = sinfo->saved_context_id;
+}
+
+static unsigned long kvm_psci_vcpu_suspend(struct kvm_vcpu *vcpu)
+{
+	int i;
+	unsigned long mpidr;
+	unsigned long target_affinity;
+	unsigned long target_affinity_mask;
+	unsigned long lowest_affinity_level;
+	struct kvm *kvm = vcpu->kvm;
+	struct kvm_vcpu *tmp;
+	struct psci_suspend_info sinfo;
+
+	target_affinity = kvm_vcpu_get_mpidr(vcpu);
+	lowest_affinity_level = (*vcpu_reg(vcpu, 1) >> 24) & 0x3;
+
+	/* Determine target affinity mask */
+	target_affinity_mask = MPIDR_HWID_BITMASK;
+	switch (lowest_affinity_level) {
+	case 0: /* All affinity levels are valid */
+		target_affinity_mask &= ~0x0UL;
+		break;
+	case 1: /* Aff0 ignored */
+		target_affinity_mask &= ~0xFFUL;
+		break;
+	case 2: /* Aff0 and Aff1 ignored */
+		target_affinity_mask &= ~0xFFFFUL;
+		break;
+	case 3: /* Aff0, Aff1, and Aff2 ignored */
+		target_affinity_mask &= ~0xFFFFFFUL;
+		break;
+	default:
+		return KVM_PSCI_RET_INVAL;
+	};
+
+	/* Ignore other bits of target affinity */
+	target_affinity &= target_affinity_mask;
+
+	/* Prepare suspend info */
+	sinfo.vcpu = NULL;
+	sinfo.saved_entry = *vcpu_reg(vcpu, 2);
+	sinfo.saved_context_id = *vcpu_reg(vcpu, 3);
+
+	/* Suspend all VCPUs within target affinity */
+	kvm_for_each_vcpu(i, tmp, kvm) {
+		mpidr = kvm_vcpu_get_mpidr(tmp);
+		if (((mpidr & target_affinity_mask) == target_affinity) &&
+		    !tmp->arch.suspend) {
+			sinfo.vcpu = tmp;
+			smp_call_function_single(tmp->cpu,
+						 psci_do_suspend, &sinfo, 1);
+		}
+	}
+
+	return KVM_PSCI_RET_SUCCESS;
+}
+
 static void kvm_psci_vcpu_off(struct kvm_vcpu *vcpu)
 {
 	vcpu->arch.pause = true;
+	vcpu->arch.suspend = false;
 }
 
 static unsigned long kvm_psci_vcpu_on(struct kvm_vcpu *source_vcpu,
@@ -179,6 +252,10 @@ static int kvm_psci_0_2_call(struct kvm_vcpu *vcpu)
 		 */
 		val = 2;
 		break;
+	case KVM_PSCI_0_2_FN_CPU_SUSPEND:
+	case KVM_PSCI_0_2_FN64_CPU_SUSPEND:
+		val = kvm_psci_vcpu_suspend(vcpu);
+		break;
 	case KVM_PSCI_0_2_FN_CPU_OFF:
 		kvm_psci_vcpu_off(vcpu);
 		val = KVM_PSCI_RET_SUCCESS;
@@ -216,10 +293,6 @@ static int kvm_psci_0_2_call(struct kvm_vcpu *vcpu)
 		val = KVM_PSCI_RET_SUCCESS;
 		ret = 0;
 		break;
-	case KVM_PSCI_0_2_FN_CPU_SUSPEND:
-	case KVM_PSCI_0_2_FN64_CPU_SUSPEND:
-		val = KVM_PSCI_RET_NI;
-		break;
 	default:
 		return -EINVAL;
 	}
@@ -253,6 +326,13 @@ static int kvm_psci_0_1_call(struct kvm_vcpu *vcpu)
 	return 1;
 }
 
+void kvm_psci_reset(struct kvm_vcpu *vcpu)
+{
+	vcpu->arch.suspend = false;
+	vcpu->arch.suspend_entry = 0;
+	vcpu->arch.suspend_context_id = 0;
+}
+
 /**
  * kvm_psci_call - handle PSCI call if r0 value is in range
  * @vcpu: Pointer to the VCPU struct
diff --git a/arch/arm/kvm/reset.c b/arch/arm/kvm/reset.c
index f558c07..220c892 100644
--- a/arch/arm/kvm/reset.c
+++ b/arch/arm/kvm/reset.c
@@ -26,6 +26,7 @@
 #include <asm/cputype.h>
 #include <asm/kvm_arm.h>
 #include <asm/kvm_coproc.h>
+#include <asm/kvm_psci.h>
 
 #include <kvm/arm_arch_timer.h>
 
@@ -79,5 +80,8 @@ int kvm_reset_vcpu(struct kvm_vcpu *vcpu)
 	/* Reset arch_timer context */
 	kvm_timer_vcpu_reset(vcpu, cpu_vtimer_irq);
 
+	/* Reset PSCI state */
+	kvm_psci_reset(vcpu);
+
 	return 0;
 }
diff --git a/arch/arm64/include/asm/kvm_host.h b/arch/arm64/include/asm/kvm_host.h
index 92242ce..b2c97dc 100644
--- a/arch/arm64/include/asm/kvm_host.h
+++ b/arch/arm64/include/asm/kvm_host.h
@@ -119,6 +119,11 @@ struct kvm_vcpu_arch {
 	/* Don't run the guest */
 	bool pause;
 
+	/* PSCI suspend state */
+	bool suspend;
+	u64 suspend_entry;
+	u64 suspend_context_id;
+
 	/* IO related fields */
 	struct kvm_decode mmio_decode;
 
diff --git a/arch/arm64/include/asm/kvm_psci.h b/arch/arm64/include/asm/kvm_psci.h
index bc39e55..4da675d 100644
--- a/arch/arm64/include/asm/kvm_psci.h
+++ b/arch/arm64/include/asm/kvm_psci.h
@@ -22,6 +22,7 @@
 #define KVM_ARM_PSCI_0_2	2
 
 int kvm_psci_version(struct kvm_vcpu *vcpu);
+void kvm_psci_reset(struct kvm_vcpu *vcpu);
 int kvm_psci_call(struct kvm_vcpu *vcpu);
 
 #endif /* __ARM64_KVM_PSCI_H__ */
diff --git a/arch/arm64/kvm/reset.c b/arch/arm64/kvm/reset.c
index 70a7816..aca9f65 100644
--- a/arch/arm64/kvm/reset.c
+++ b/arch/arm64/kvm/reset.c
@@ -29,6 +29,7 @@
 #include <asm/ptrace.h>
 #include <asm/kvm_arm.h>
 #include <asm/kvm_coproc.h>
+#include <asm/kvm_psci.h>
 
 /*
  * ARMv8 Reset Values
@@ -108,5 +109,8 @@ int kvm_reset_vcpu(struct kvm_vcpu *vcpu)
 	/* Reset timer */
 	kvm_timer_vcpu_reset(vcpu, cpu_vtimer_irq);
 
+	/* Reset PSCI state */
+	kvm_psci_reset(vcpu);
+
 	return 0;
 }
-- 
1.7.9.5