ARM: KVM: Fix MPIDR computing to support virtual clusters

In order to be able to support more than 4 A7 or A15 CPUs,
we need to fix the MPIDR computing to reflect the fact that
both A15 and A7 can only exist in clusters of at most 4 CPUs.

Fix the MPIDR computing to allow virtual clusters to be exposed
to the guest.

Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Christoffer Dall <christoffer.dall@linaro.org>

1 files changed, 6 insertions, 4 deletions

diff --git a/arch/arm/kvm/coproc.c b/arch/arm/kvm/coproc.c
index a629f2c1d0f9..631e6bd0e05f 100644
--- a/arch/arm/kvm/coproc.c
+++ b/arch/arm/kvm/coproc.c
@@ -74,11 +74,13 @@ int kvm_handle_cp14_access(struct kvm_vcpu *vcpu, struct kvm_run *run)
 static void reset_mpidr(struct kvm_vcpu *vcpu, const struct coproc_reg *r)
 {
         /*
-         * Compute guest MPIDR. No need to mess around with different clusters
-         * but we read the 'U' bit from the underlying hardware directly.
+         * Compute guest MPIDR. We build a virtual cluster out of the
+         * vcpu_id, but we read the 'U' bit from the underlying
+         * hardware directly.
          */
-        vcpu->arch.cp15[c0_MPIDR] = (read_cpuid_mpidr() & MPIDR_SMP_BITMASK)
-                                        | vcpu->vcpu_id;
+        vcpu->arch.cp15[c0_MPIDR] = ((read_cpuid_mpidr() & MPIDR_SMP_BITMASK) |
+                                     ((vcpu->vcpu_id >> 2) << MPIDR_LEVEL_BITS) |
+                                     (vcpu->vcpu_id & 3));
 }
 
 /* TRM entries A7:4.3.31 A15:4.3.28 - RO WI */