[PATCH v5 12/13] ARM: KVM: Fix guest view of MPIDR

[Christoffer Dall <c.dall@virtualopensystems.com>]

From: Marc Zyngier <marc.zyngier@arm.com>

A guest may need to know which CPU it has booted on (and Linux does).
Now that we can run KVM on a SMP host, QEMU may be running on any
CPU. In that case, directly reading MPIDR will give an inconsistent
view on the guest CPU number (among other problems).

The solution is to use the VMPIDR register, which is computed by
using the host MPIDR and overriding the low bits with KVM vcpu_id.

Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
---
 arch/arm/include/asm/kvm_host.h |    1 +
 arch/arm/kernel/asm-offsets.c   |    1 +
 arch/arm/kvm/arm.c              |    4 ++++
 arch/arm/kvm/interrupts.S       |    8 ++++++++
 4 files changed, 14 insertions(+), 0 deletions(-)

diff --git a/arch/arm/include/asm/kvm_host.h b/arch/arm/include/asm/kvm_host.h
index a0ffbe8..7fcc412 100644
--- a/arch/arm/include/asm/kvm_host.h
+++ b/arch/arm/include/asm/kvm_host.h
@@ -63,6 +63,7 @@ struct kvm_vcpu_arch {
 	/* System control coprocessor (cp15) */
 	struct {
 		u32 c0_MIDR;		/* Main ID Register */
+		u32 c0_MPIDR;		/* MultiProcessor ID Register */
 		u32 c1_SCTLR;		/* System Control Register */
 		u32 c1_ACTLR;		/* Auxilliary Control Register */
 		u32 c1_CPACR;		/* Coprocessor Access Control */
diff --git a/arch/arm/kernel/asm-offsets.c b/arch/arm/kernel/asm-offsets.c
index c126cfb..1c6e2ee 100644
--- a/arch/arm/kernel/asm-offsets.c
+++ b/arch/arm/kernel/asm-offsets.c
@@ -148,6 +148,7 @@ int main(void)
 #ifdef CONFIG_KVM_ARM_HOST
   DEFINE(VCPU_KVM,		offsetof(struct kvm_vcpu, kvm));
   DEFINE(VCPU_MIDR,		offsetof(struct kvm_vcpu, arch.cp15.c0_MIDR));
+  DEFINE(VCPU_MPIDR,		offsetof(struct kvm_vcpu, arch.cp15.c0_MPIDR));
   DEFINE(VCPU_SCTLR,		offsetof(struct kvm_vcpu, arch.cp15.c1_SCTLR));
   DEFINE(VCPU_CPACR,		offsetof(struct kvm_vcpu, arch.cp15.c1_CPACR));
   DEFINE(VCPU_TTBR0,		offsetof(struct kvm_vcpu, arch.cp15.c2_TTBR0));
diff --git a/arch/arm/kvm/arm.c b/arch/arm/kvm/arm.c
index 6e384e2..9c5c38e 100644
--- a/arch/arm/kvm/arm.c
+++ b/arch/arm/kvm/arm.c
@@ -32,6 +32,7 @@
 #include <asm/ptrace.h>
 #include <asm/mman.h>
 #include <asm/tlbflush.h>
+#include <asm/cputype.h>
 #include <asm/kvm_arm.h>
 #include <asm/kvm_asm.h>
 #include <asm/kvm_mmu.h>
@@ -270,6 +271,9 @@ int kvm_arch_vcpu_init(struct kvm_vcpu *vcpu)
 			[sctlr] "=r" (sctlr));
 	vcpu->arch.cp15.c1_SCTLR = sctlr & ~1U;
 
+	/* Compute guest MPIDR */
+	vcpu->arch.cp15.c0_MPIDR = (read_cpuid_mpidr() & ~0xff) | vcpu->vcpu_id;
+
 	return 0;
 }
 
diff --git a/arch/arm/kvm/interrupts.S b/arch/arm/kvm/interrupts.S
index d516bf4..fbc26ca 100644
--- a/arch/arm/kvm/interrupts.S
+++ b/arch/arm/kvm/interrupts.S
@@ -245,6 +245,10 @@ ENTRY(__kvm_vcpu_run)
 	ldr	r1, [r0, #VCPU_MIDR]
 	mcr	p15, 4, r1, c0, c0, 0
 
+	@ Write guest view of MPIDR into VMPIDR
+	ldr	r1, [r0, #VCPU_MPIDR]
+	mcr	p15, 4, r1, c0, c0, 5
+
 	@ Load guest registers
 	add	r0, r0, #(VCPU_USR_SP)
 	load_mode_state r0, usr
@@ -291,6 +295,10 @@ __kvm_vcpu_return:
 	mrc	p15, 0, r2, c0, c0, 0
 	mcr	p15, 4, r2, c0, c0, 0
 
+	@ Back to hardware MPIDR
+	mrc	p15, 0, r2, c0, c0, 5
+	mcr	p15, 4, r2, c0, c0, 5
+
 	@ Set VMID == 0
 	mov	r2, #0
 	mov	r3, #0