[Qemu-devel] [PATCH v4] Use Aff1 with mpidr

[Pavel Fedin <p.fedin@samsung.com>]

In order to support up to 128 cores with GIC-500 (GICv3 implementation)
affinity1 must be used. GIC-500 support up to 32 clusters with up to
8 cores in a cluster. So for example, if one wishes to have 16 cores,
the options are: 2 clusters of 8 cores each, 4 clusters with 4 cores each.
It is possible to specify the desired layout by setting arm_cpus_per_cluster
variable.
Unfortunately KVM currently has its own idea about CPU affinity assignment
scheme. Therefore, if we use KVM we override CPU IDs with those provided by
the KVM.

Signed-off-by: Shlomo Pongratz <shlomo.pongratz@huawei.com>
Signed-off-by: Pavel Fedin <p.fedin@samsung.com>
---
 Changes since v3:
1. Fixed and/or updated comments and descriptions
2. CPUS_PER_CLUSTER replaced by a variable which can be changed by machine initialization
code
3. PSCI looks up processor directly by ID, no more ID->index conversion
4. Added ID fixup for 32-bit KVM
5. Debugging macros removed
6. mpidr field name changed to mp_affinity
---
 hw/arm/virt.c        |  2 +-
 target-arm/cpu-qom.h |  3 +++
 target-arm/cpu.c     | 12 ++++++++++++
 target-arm/helper.c  |  9 +++------
 target-arm/kvm32.c   | 15 +++++++++++++++
 target-arm/kvm64.c   | 15 +++++++++++++++
 target-arm/psci.c    | 19 +++++++++++++++++--
 7 files changed, 66 insertions(+), 9 deletions(-)

diff --git a/hw/arm/virt.c b/hw/arm/virt.c
index 0a75cc8..ab94fdb 100644
--- a/hw/arm/virt.c
+++ b/hw/arm/virt.c
@@ -306,7 +306,7 @@ static void fdt_add_cpu_nodes(const VirtBoardInfo *vbi)
                                         "enable-method", "psci");
         }
 
-        qemu_fdt_setprop_cell(vbi->fdt, nodename, "reg", cpu);
+        qemu_fdt_setprop_cell(vbi->fdt, nodename, "reg", armcpu->mp_affinity);
         g_free(nodename);
     }
 }
diff --git a/target-arm/cpu-qom.h b/target-arm/cpu-qom.h
index ed5a644..36064d4 100644
--- a/target-arm/cpu-qom.h
+++ b/target-arm/cpu-qom.h
@@ -159,6 +159,7 @@ typedef struct ARMCPU {
     uint64_t id_aa64mmfr1;
     uint32_t dbgdidr;
     uint32_t clidr;
+    uint64_t mp_affinity; /* MP ID without feature bits */
     /* The elements of this array are the CCSIDR values for each cache,
      * in the order L1DCache, L1ICache, L2DCache, L2ICache, etc.
      */
@@ -171,6 +172,8 @@ typedef struct ARMCPU {
     uint64_t rvbar;
 } ARMCPU;
 
+extern int arm_cpus_per_cluster;
+
 #define TYPE_AARCH64_CPU "aarch64-cpu"
 #define AARCH64_CPU_CLASS(klass) \
     OBJECT_CLASS_CHECK(AArch64CPUClass, (klass), TYPE_AARCH64_CPU)
diff --git a/target-arm/cpu.c b/target-arm/cpu.c
index 4a888ab..fa9f1c0 100644
--- a/target-arm/cpu.c
+++ b/target-arm/cpu.c
@@ -383,17 +383,29 @@ static inline void unset_feature(CPUARMState *env, int feature)
     env->features &= ~(1ULL << feature);
 }
 
+int arm_cpus_per_cluster = 8;
+
 static void arm_cpu_initfn(Object *obj)
 {
     CPUState *cs = CPU(obj);
     ARMCPU *cpu = ARM_CPU(obj);
     static bool inited;
+    uint32_t Aff1, Aff0;
 
     cs->env_ptr = &cpu->env;
     cpu_exec_init(&cpu->env);
     cpu->cp_regs = g_hash_table_new_full(g_int_hash, g_int_equal,
                                          g_free, g_free);
 
+    /*
+     * We don't support setting cluster ID ([16..23]) (known as Aff2
+     * in later ARM ARM versions), or any of the higher affinity level fields,
+     * so these bits always RAZ.
+     */
+    Aff1 = cs->cpu_index / arm_cpus_per_cluster;
+    Aff0 = cs->cpu_index % arm_cpus_per_cluster;
+    cpu->mp_affinity = (Aff1 << 8) | Aff0;
+
 #ifndef CONFIG_USER_ONLY
     /* Our inbound IRQ and FIQ lines */
     if (kvm_enabled()) {
diff --git a/target-arm/helper.c b/target-arm/helper.c
index 3da0c05..ec0b14d 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -2063,12 +2063,9 @@ static const ARMCPRegInfo strongarm_cp_reginfo[] = {
 
 static uint64_t mpidr_read(CPUARMState *env, const ARMCPRegInfo *ri)
 {
-    CPUState *cs = CPU(arm_env_get_cpu(env));
-    uint32_t mpidr = cs->cpu_index;
-    /* We don't support setting cluster ID ([8..11]) (known as Aff1
-     * in later ARM ARM versions), or any of the higher affinity level fields,
-     * so these bits always RAZ.
-     */
+    ARMCPU *cpu = ARM_CPU(arm_env_get_cpu(env));
+    uint64_t mpidr = cpu->mp_affinity;
+
     if (arm_feature(env, ARM_FEATURE_V7MP)) {
         mpidr |= (1U << 31);
         /* Cores which are uniprocessor (non-coherent)
diff --git a/target-arm/kvm32.c b/target-arm/kvm32.c
index 49b6bab..d7e7d68 100644
--- a/target-arm/kvm32.c
+++ b/target-arm/kvm32.c
@@ -153,10 +153,14 @@ bool kvm_arm_reg_syncs_via_cpreg_list(uint64_t regidx)
     }
 }
 
+#define ARM_MPIDR_HWID_BITMASK 0xFFFFFF
+#define ARM_CPU_ID_MPIDR       0, 0, 0, 5
+
 int kvm_arch_init_vcpu(CPUState *cs)
 {
     int ret;
     uint64_t v;
+    uint32_t mpidr;
     struct kvm_one_reg r;
     ARMCPU *cpu = ARM_CPU(cs);
 
@@ -193,6 +197,17 @@ int kvm_arch_init_vcpu(CPUState *cs)
         return -EINVAL;
     }
 
+    /*
+     * When KVM is in use, PSCI is emulated in-kernel and not by qemu.
+     * Currently KVM has its own idea about MPIDR assignment, so we
+     * override our defaults with what we get from KVM.
+     */
+    ret = kvm_get_one_reg(cs, ARM_CP15_REG32(ARM_CPU_ID_MPIDR), &mpidr);
+    if (ret) {
+        return ret;
+    }
+    cpu->mp_affinity = mpidr & ARM_MPIDR_HWID_BITMASK;
+
     return kvm_arm_init_cpreg_list(cpu);
 }
 
diff --git a/target-arm/kvm64.c b/target-arm/kvm64.c
index 93c1ca8..ac34f51 100644
--- a/target-arm/kvm64.c
+++ b/target-arm/kvm64.c
@@ -77,9 +77,13 @@ bool kvm_arm_get_host_cpu_features(ARMHostCPUClass *ahcc)
     return true;
 }
 
+#define ARM_MPIDR_HWID_BITMASK 0xFF00FFFFFFULL
+#define ARM_CPU_ID_MPIDR       3, 0, 0, 0, 5
+
 int kvm_arch_init_vcpu(CPUState *cs)
 {
     int ret;
+    uint64_t mpidr;
     ARMCPU *cpu = ARM_CPU(cs);
 
     if (cpu->kvm_target == QEMU_KVM_ARM_TARGET_NONE ||
@@ -107,6 +111,17 @@ int kvm_arch_init_vcpu(CPUState *cs)
         return ret;
     }
 
+    /*
+     * When KVM is in use, PSCI is emulated in-kernel and not by qemu.
+     * Currently KVM has its own idea about MPIDR assignment, so we
+     * override our defaults with what we get from KVM.
+     */
+    ret = kvm_get_one_reg(cs, ARM64_SYS_REG(ARM_CPU_ID_MPIDR), &mpidr);
+    if (ret) {
+        return ret;
+    }
+    cpu->mp_affinity = mpidr & ARM_MPIDR_HWID_BITMASK;
+
     return kvm_arm_init_cpreg_list(cpu);
 }
 
diff --git a/target-arm/psci.c b/target-arm/psci.c
index d8fafab..20e4cb6 100644
--- a/target-arm/psci.c
+++ b/target-arm/psci.c
@@ -72,6 +72,21 @@ bool arm_is_psci_call(ARMCPU *cpu, int excp_type)
     }
 }
 
+static CPUState *get_cpu_by_id(uint64_t id)
+{
+    CPUState *cpu;
+
+    CPU_FOREACH(cpu) {
+        ARMCPU *armcpu = ARM_CPU(cpu);
+
+        if (armcpu->mp_affinity == id) {
+            return cpu;
+        }
+    }
+
+    return NULL;
+}
+
 void arm_handle_psci_call(ARMCPU *cpu)
 {
     /*
@@ -121,7 +136,7 @@ void arm_handle_psci_call(ARMCPU *cpu)
 
         switch (param[2]) {
         case 0:
-            target_cpu_state = qemu_get_cpu(mpidr & 0xff);
+            target_cpu_state = get_cpu_by_id(mpidr);
             if (!target_cpu_state) {
                 ret = QEMU_PSCI_RET_INVALID_PARAMS;
                 break;
@@ -153,7 +168,7 @@ void arm_handle_psci_call(ARMCPU *cpu)
         context_id = param[3];
 
         /* change to the cpu we are powering up */
-        target_cpu_state = qemu_get_cpu(mpidr & 0xff);
+        target_cpu_state = get_cpu_by_id(mpidr);
         if (!target_cpu_state) {
             ret = QEMU_PSCI_RET_INVALID_PARAMS;
             break;
-- 
1.9.5.msysgit.0