kvm tools: arm: emit the MPIDR in DT instead of cpu_id

kvmtools uses the virtual CPU number to emit the DT CPU nodes.
While this is correct for a flat topology, it fails on anything
else, as the guest expects to find the MPIDR there.

The fix is to ask the kernel for each vcpu MPIDR, and emit this
instead.

Acked-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Marc Zyngier <marc.zyngier@arm.com>
Signed-off-by: Will Deacon <will.deacon@arm.com>
Signed-off-by: Pekka Enberg <penberg@kernel.org>

6 files changed, 70 insertions, 3 deletions

diff --git a/arm/aarch32/include/kvm/kvm-cpu-arch.h b/arm/aarch32/include/kvm/kvm-cpu-arch.h
index b9fda07d..d28ea676 100644
--- a/arm/aarch32/include/kvm/kvm-cpu-arch.h
+++ b/arm/aarch32/include/kvm/kvm-cpu-arch.h
@@ -9,4 +9,8 @@
 	[0] = (!!(cpuid) << KVM_ARM_VCPU_POWER_OFF),			\
 }
 
+#define ARM_MPIDR_HWID_BITMASK	0xFFFFFF
+#define ARM_CPU_ID		0, 0, 0
+#define ARM_CPU_ID_MPIDR	5
+
 #endif /* KVM__KVM_CPU_ARCH_H */
diff --git a/arm/aarch32/kvm-cpu.c b/arm/aarch32/kvm-cpu.c
index 6a012db8..bd71037e 100644
--- a/arm/aarch32/kvm-cpu.c
+++ b/arm/aarch32/kvm-cpu.c
@@ -6,6 +6,32 @@
 #define ARM_CORE_REG(x)	(KVM_REG_ARM | KVM_REG_SIZE_U32 | KVM_REG_ARM_CORE | \
 			 KVM_REG_ARM_CORE_REG(x))
 
+#define ARM_CP15_REG_SHIFT_MASK(x,n)		\
+	(((x) << KVM_REG_ARM_ ## n ## _SHIFT) & KVM_REG_ARM_ ## n ## _MASK)
+
+#define __ARM_CP15_REG(op1,crn,crm,op2)			\
+	(KVM_REG_ARM | KVM_REG_SIZE_U32		|	\
+	 (15 << KVM_REG_ARM_COPROC_SHIFT)	|	\
+	 ARM_CP15_REG_SHIFT_MASK(op1, OPC1)	|	\
+	 ARM_CP15_REG_SHIFT_MASK(crn, 32_CRN)	|	\
+	 ARM_CP15_REG_SHIFT_MASK(crm, CRM)	|	\
+	 ARM_CP15_REG_SHIFT_MASK(op2, 32_OPC2))
+
+#define ARM_CP15_REG(...)	__ARM_CP15_REG(__VA_ARGS__)
+
+unsigned long kvm_cpu__get_vcpu_mpidr(struct kvm_cpu *vcpu)
+{
+	struct kvm_one_reg reg;
+	u32 mpidr;
+
+	reg.id = ARM_CP15_REG(ARM_CPU_ID, ARM_CPU_ID_MPIDR);
+	reg.addr = (u64)(unsigned long)&mpidr;
+	if (ioctl(vcpu->vcpu_fd, KVM_GET_ONE_REG, &reg) < 0)
+		die("KVM_GET_ONE_REG failed (get_mpidr vcpu%ld", vcpu->cpu_id);
+
+	return mpidr;
+}
+
 void kvm_cpu__reset_vcpu(struct kvm_cpu *vcpu)
 {
 	struct kvm *kvm	= vcpu->kvm;
diff --git a/arm/aarch64/include/kvm/kvm-cpu-arch.h b/arm/aarch64/include/kvm/kvm-cpu-arch.h
index d85c5834..7d70c3ba 100644
--- a/arm/aarch64/include/kvm/kvm-cpu-arch.h
+++ b/arm/aarch64/include/kvm/kvm-cpu-arch.h
@@ -10,4 +10,8 @@
 	       (!!(kvm)->cfg.arch.aarch32_guest << KVM_ARM_VCPU_EL1_32BIT))	\
 }
 
+#define ARM_MPIDR_HWID_BITMASK	0xFF00FFFFFFUL
+#define ARM_CPU_ID		3, 0, 0, 0
+#define ARM_CPU_ID_MPIDR	5
+
 #endif /* KVM__KVM_CPU_ARCH_H */
diff --git a/arm/aarch64/kvm-cpu.c b/arm/aarch64/kvm-cpu.c
index 7cdcb70e..059e42c1 100644
--- a/arm/aarch64/kvm-cpu.c
+++ b/arm/aarch64/kvm-cpu.c
@@ -10,6 +10,34 @@
 #define ARM64_CORE_REG(x)	(KVM_REG_ARM64 | KVM_REG_SIZE_U64 | \
 				 KVM_REG_ARM_CORE | KVM_REG_ARM_CORE_REG(x))
 
+#define ARM64_SYS_REG_SHIFT_MASK(x,n)				\
+	(((x) << KVM_REG_ARM64_SYSREG_ ## n ## _SHIFT) &	\
+	 KVM_REG_ARM64_SYSREG_ ## n ## _MASK)
+
+#define __ARM64_SYS_REG(op0,op1,crn,crm,op2)			\
+	(KVM_REG_ARM64 | KVM_REG_SIZE_U64		|	\
+	 KVM_REG_ARM64_SYSREG				|	\
+	 ARM64_SYS_REG_SHIFT_MASK(op0, OP0)		|	\
+	 ARM64_SYS_REG_SHIFT_MASK(op1, OP1)		|	\
+	 ARM64_SYS_REG_SHIFT_MASK(crn, CRN)		|	\
+	 ARM64_SYS_REG_SHIFT_MASK(crm, CRM)		|	\
+	 ARM64_SYS_REG_SHIFT_MASK(op2, OP2))
+
+#define ARM64_SYS_REG(...)	__ARM64_SYS_REG(__VA_ARGS__)
+
+unsigned long kvm_cpu__get_vcpu_mpidr(struct kvm_cpu *vcpu)
+{
+	struct kvm_one_reg reg;
+	u64 mpidr;
+
+	reg.id = ARM64_SYS_REG(ARM_CPU_ID, ARM_CPU_ID_MPIDR);
+	reg.addr = (u64)&mpidr;
+	if (ioctl(vcpu->vcpu_fd, KVM_GET_ONE_REG, &reg) < 0)
+		die("KVM_GET_ONE_REG failed (get_mpidr vcpu%ld", vcpu->cpu_id);
+
+	return mpidr;
+}
+
 static void reset_vcpu_aarch32(struct kvm_cpu *vcpu)
 {
 	struct kvm *kvm = vcpu->kvm;
diff --git a/arm/fdt.c b/arm/fdt.c
index 5e18c11d..9a34d980 100644
--- a/arm/fdt.c
+++ b/arm/fdt.c
@@ -52,17 +52,20 @@ static void generate_cpu_nodes(void *fdt, struct kvm *kvm)
 
 	for (cpu = 0; cpu < kvm->nrcpus; ++cpu) {
 		char cpu_name[CPU_NAME_MAX_LEN];
+		struct kvm_cpu *vcpu = kvm->cpus[cpu];
+		unsigned long mpidr = kvm_cpu__get_vcpu_mpidr(vcpu);
 
-		snprintf(cpu_name, CPU_NAME_MAX_LEN, "cpu@%d", cpu);
+		mpidr &= ARM_MPIDR_HWID_BITMASK;
+		snprintf(cpu_name, CPU_NAME_MAX_LEN, "cpu@%lx", mpidr);
 
 		_FDT(fdt_begin_node(fdt, cpu_name));
 		_FDT(fdt_property_string(fdt, "device_type", "cpu"));
-		_FDT(fdt_property_string(fdt, "compatible", kvm->cpus[cpu]->cpu_compatible));
+		_FDT(fdt_property_string(fdt, "compatible", vcpu->cpu_compatible));
 
 		if (kvm->nrcpus > 1)
 			_FDT(fdt_property_string(fdt, "enable-method", "psci"));
 
-		_FDT(fdt_property_cell(fdt, "reg", cpu));
+		_FDT(fdt_property_cell(fdt, "reg", mpidr));
 		_FDT(fdt_end_node(fdt));
 	}
 
diff --git a/arm/include/arm-common/kvm-cpu-arch.h b/arm/include/arm-common/kvm-cpu-arch.h
index b514dd56..bef17614 100644
--- a/arm/include/arm-common/kvm-cpu-arch.h
+++ b/arm/include/arm-common/kvm-cpu-arch.h
@@ -45,4 +45,6 @@ static inline bool kvm_cpu__emulate_io(struct kvm *kvm, u16 port, void *data,
 bool kvm_cpu__emulate_mmio(struct kvm *kvm, u64 phys_addr, u8 *data, u32 len,
 			   u8 is_write);
 
+unsigned long kvm_cpu__get_vcpu_mpidr(struct kvm_cpu *vcpu);
+
 #endif /* ARM_COMMON__KVM_CPU_ARCH_H */