arm: psci: support multiple affinity levels in MPIDR

PSCI actually takes CPU parameters by the MPIDR id, which may
differ from the logical id of the CPU. This patch is the first step
into properly handling the CPU affinity levels in the MPIDR.

Signed-off-by: Antonios Motakis <antonios.motakis@huawei.com>
[Jan: add missing processor.h include to setup.c]
Signed-off-by: Jan Kiszka <jan.kiszka@siemens.com>

diff --git a/hypervisor/arch/arm/include/asm/control.h b/hypervisor/arch/arm/include/asm/control.h
index f050e76a8698a6d3fa9f10cdcacd8a1f34a983f4..1d64b9827b19dbefe6f9834fd3b5c7889fa99319 100644 (file)
--- a/hypervisor/arch/arm/include/asm/control.h
+++ b/hypervisor/arch/arm/include/asm/control.h
@@ -38,6 +38,7 @@ struct registers* arch_handle_exit(struct per_cpu *cpu_data,
 bool arch_handle_phys_irq(struct per_cpu *cpu_data, u32 irqn);
 void arch_reset_self(struct per_cpu *cpu_data);
 void arch_shutdown_self(struct per_cpu *cpu_data);
+unsigned int arm_cpu_by_mpidr(struct cell *cell, unsigned long mpidr);
 
 void __attribute__((noreturn)) vmreturn(struct registers *guest_regs);
 void __attribute__((noreturn)) arch_shutdown_mmu(struct per_cpu *cpu_data);

diff --git a/hypervisor/arch/arm/include/asm/percpu.h b/hypervisor/arch/arm/include/asm/percpu.h
index ae026c47e19744bacb3cf66bef0d7cc2e029ba37..0e9eac87abb767801ae271663c057b0abc80b829 100644 (file)
--- a/hypervisor/arch/arm/include/asm/percpu.h
+++ b/hypervisor/arch/arm/include/asm/percpu.h
@@ -62,6 +62,7 @@ struct per_cpu {
        bool flush_vcpu_caches;
        int shutdown_state;
        bool shutdown;
+       unsigned long mpidr;
        bool failed;
 } __attribute__((aligned(PAGE_SIZE)));
 

diff --git a/hypervisor/arch/arm/lib.c b/hypervisor/arch/arm/lib.c
index 038bf9af5c10fb3b63804260f1235be14e0bffcc..2fdcbe6b86f4026fde585582056917c9e996b6c7 100644 (file)
--- a/hypervisor/arch/arm/lib.c
+++ b/hypervisor/arch/arm/lib.c
@@ -10,9 +10,12 @@
  * the COPYING file in the top-level directory.
  */
 
+#include <jailhouse/control.h>
 #include <jailhouse/processor.h>
 #include <jailhouse/string.h>
 #include <jailhouse/types.h>
+#include <asm/control.h>
+#include <asm/percpu.h>
 #include <asm/sysregs.h>
 
 unsigned long phys_processor_id(void)
@@ -22,3 +25,14 @@ unsigned long phys_processor_id(void)
        arm_read_sysreg(MPIDR_EL1, mpidr);
        return mpidr & MPIDR_CPUID_MASK;
 }
+
+unsigned int arm_cpu_by_mpidr(struct cell *cell, unsigned long mpidr)
+{
+       unsigned int cpu;
+
+       for_each_cpu(cpu, cell->cpu_set)
+               if (mpidr == (per_cpu(cpu)->mpidr & MPIDR_CPUID_MASK))
+                       return cpu;
+
+       return -1;
+}

diff --git a/hypervisor/arch/arm/psci.c b/hypervisor/arch/arm/psci.c
index 3ebbd50e82aa7262d630cee4a9bdc2e45c3e6ddd..e0a6703886e2c00b6f43940dc49824c4c13c2dfd 100644 (file)
--- a/hypervisor/arch/arm/psci.c
+++ b/hypervisor/arch/arm/psci.c
@@ -78,11 +78,10 @@ int psci_wait_cpu_stopped(unsigned int cpu_id)
 static long psci_emulate_cpu_on(struct per_cpu *cpu_data,
                                struct trap_context *ctx)
 {
-       unsigned int target = ctx->regs[1];
        unsigned int cpu;
        struct psci_mbox *mbox;
 
-       cpu = arm_cpu_virt2phys(cpu_data->cell, target);
+       cpu = arm_cpu_by_mpidr(cpu_data->cell, ctx->regs[1]);
        if (cpu == -1)
                /* Virtual id not in set */
                return PSCI_DENIED;
@@ -97,7 +96,7 @@ static long psci_emulate_cpu_on(struct per_cpu *cpu_data,
 static long psci_emulate_affinity_info(struct per_cpu *cpu_data,
                                       struct trap_context *ctx)
 {
-       unsigned int cpu = arm_cpu_virt2phys(cpu_data->cell, ctx->regs[1]);
+       unsigned int cpu = arm_cpu_by_mpidr(cpu_data->cell, ctx->regs[1]);
 
        if (cpu == -1)
                /* Virtual id not in set */

diff --git a/hypervisor/arch/arm/setup.c b/hypervisor/arch/arm/setup.c
index f9f59d876dbeaaf03403de51e514d31cb4941339..db5fef187a079655e203a647a1820d6ba53e6933 100644 (file)
--- a/hypervisor/arch/arm/setup.c
+++ b/hypervisor/arch/arm/setup.c
@@ -17,6 +17,7 @@
 #include <asm/sysregs.h>
 #include <jailhouse/control.h>
 #include <jailhouse/paging.h>
+#include <jailhouse/processor.h>
 #include <jailhouse/string.h>
 
 unsigned int cache_line_size;
@@ -56,6 +57,7 @@ int arch_cpu_init(struct per_cpu *cpu_data)
 
        cpu_data->psci_mbox.entry = 0;
        cpu_data->virt_id = cpu_data->cpu_id;
+       cpu_data->mpidr = phys_processor_id();
 
        /*
         * Copy the registers to restore from the linux stack here, because we