arm: PSCI emulation

By adding a new field 'guest_mbox' in the cpu_datas, this patch allows
the guests to issue PSCI HVC calls. Currently, only PSCI_CPU_ON,
PSCI_CPU_OFF, and PSCI_VERSION are handled.
A call to CPU_OFF enters the suspend mode through arch_reset_self. When
a core calls CPU_ON, the hypervisor wakes up the other core, which will
take its return address from the guest_mbox, wipe its registers and go
back to EL1. The context argument to PSCI_CPU_ON is currently ignored,
since the whole core is reset.
This patch also traps SMC instructions in order to catch the PSCI
requests done using this way. All others SMC calls are forwarded to
EL3.

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Jan Kiszka <jan.kiszka@siemens.com>

diff --git a/hypervisor/arch/arm/control.c b/hypervisor/arch/arm/control.c
index 3b60996dad1a6a7dc8825764f69cb11911cb1c3f..f621ecb098f4e95413268531d284b2cf042f5609 100644 (file)
--- a/hypervisor/arch/arm/control.c
+++ b/hypervisor/arch/arm/control.c
@@ -89,7 +89,7 @@ static void arch_reset_el1(struct registers *regs)
        arm_write_sysreg(TPIDRPRW, 0);
 }
 
-static void arch_reset_self(struct per_cpu *cpu_data)
+void arch_reset_self(struct per_cpu *cpu_data)
 {
        int err;
        unsigned long reset_address;

diff --git a/hypervisor/arch/arm/include/asm/control.h b/hypervisor/arch/arm/include/asm/control.h
index bb97ff3ace5bb76713b4c99f3e3643b67a790f53..10a46c2a07a3c0d6f5d6657b3abf812f0a2657d9 100644 (file)
--- a/hypervisor/arch/arm/include/asm/control.h
+++ b/hypervisor/arch/arm/include/asm/control.h
@@ -37,6 +37,7 @@ int arch_spin_init(void);
 unsigned long arch_cpu_spin(void);
 struct registers* arch_handle_exit(struct per_cpu *cpu_data,
                                   struct registers *regs);
+void arch_reset_self(struct per_cpu *cpu_data);
 
 void __attribute__((noreturn)) vmreturn(struct registers *guest_regs);
 

diff --git a/hypervisor/arch/arm/include/asm/percpu.h b/hypervisor/arch/arm/include/asm/percpu.h
index 64d0488df5d668f422181a9b544a6761c468ca16..1a6ae7c09be44a6e0003b3573f82520a4032711c 100644 (file)
--- a/hypervisor/arch/arm/include/asm/percpu.h
+++ b/hypervisor/arch/arm/include/asm/percpu.h
@@ -58,6 +58,7 @@ struct per_cpu {
 
        /* The mbox will be accessed with a ldrd, which requires alignment */
        __attribute__((aligned(8))) struct psci_mbox psci_mbox;
+       struct psci_mbox guest_mbox;
 
        bool flush_vcpu_caches;
        int shutdown_state;

diff --git a/hypervisor/arch/arm/include/asm/processor.h b/hypervisor/arch/arm/include/asm/processor.h
index 1fa7a72aea6b0f2a41cbe0d3a1d7adda3d779b11..5316abd60befbdca9060c1743c2537f912f8af72 100644 (file)
--- a/hypervisor/arch/arm/include/asm/processor.h
+++ b/hypervisor/arch/arm/include/asm/processor.h
@@ -171,6 +171,8 @@ struct registers {
 #define wfi()          asm volatile("wfi\n")
 #define sev()          asm volatile("sev\n")
 
+unsigned int smc(unsigned int r0, ...);
+
 static inline void cpu_relax(void)
 {
        asm volatile("" : : : "memory");

diff --git a/hypervisor/arch/arm/include/asm/psci.h b/hypervisor/arch/arm/include/asm/psci.h
index 1883a6dc9f3a302ab7b2448451e9bc8d4fcbc8fd..391574fb6a46ec121ce2cf07ceff25785b280b2a 100644 (file)
--- a/hypervisor/arch/arm/include/asm/psci.h
+++ b/hypervisor/arch/arm/include/asm/psci.h
@@ -39,6 +39,8 @@
 #define PSCI_NOT_PRESENT       (-7)
 #define PSCI_DISABLED          (-8)
 
+#define IS_PSCI_FN(hvc)                ((((hvc) >> 24) & 0x84) == 0x84)
+
 #define PSCI_INVALID_ADDRESS   0xffffffff
 
 #ifndef __ASSEMBLY__
@@ -60,5 +62,10 @@ void psci_suspend(struct per_cpu *cpu_data);
 long psci_resume(unsigned int target);
 long psci_try_resume(unsigned int cpu_id);
 
+long psci_dispatch(struct per_cpu *cpu_data, struct trap_context *ctx);
+
+int psci_cell_init(struct cell *cell);
+unsigned long psci_emulate_spin(struct per_cpu *cpu_data);
+
 #endif /* !__ASSEMBLY__ */
 #endif /* _JAILHOUSE_ASM_PSCI_H */

diff --git a/hypervisor/arch/arm/psci.c b/hypervisor/arch/arm/psci.c
index 132d6a037409c8d4fbce90aa6cf4b26b1dc8d50b..9d0879ca63b175ac4f1637dca3eea6539b8c9142 100644 (file)
--- a/hypervisor/arch/arm/psci.c
+++ b/hypervisor/arch/arm/psci.c
@@ -11,8 +11,10 @@
  */
 
 #include <asm/control.h>
+#include <asm/percpu.h>
 #include <asm/psci.h>
 #include <asm/traps.h>
+#include <jailhouse/control.h>
 
 void _psci_cpu_off(struct psci_mbox *);
 long _psci_cpu_on(struct psci_mbox *, unsigned long, unsigned long);
@@ -72,3 +74,75 @@ int psci_wait_cpu_stopped(unsigned int cpu_id)
 
        return -EBUSY;
 }
+
+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);
+       if (cpu == -1)
+               /* Virtual id not in set */
+               return PSCI_DENIED;
+
+       mbox = &(per_cpu(cpu)->guest_mbox);
+       mbox->entry = ctx->regs[2];
+       mbox->context = ctx->regs[3];
+
+       return psci_resume(cpu);
+}
+
+/* Returns the secondary address set by the guest */
+unsigned long psci_emulate_spin(struct per_cpu *cpu_data)
+{
+       struct psci_mbox *mbox = &(cpu_data->guest_mbox);
+
+       mbox->entry = 0;
+
+       /* Wait for emulate_cpu_on or a trapped mmio to the mbox */
+       while (mbox->entry == 0)
+               psci_suspend(cpu_data);
+
+       return mbox->entry;
+}
+
+int psci_cell_init(struct cell *cell)
+{
+       unsigned int cpu;
+
+       for_each_cpu(cpu, cell->cpu_set) {
+               per_cpu(cpu)->guest_mbox.entry = 0;
+               per_cpu(cpu)->guest_mbox.context = 0;
+       }
+
+       return 0;
+}
+
+long psci_dispatch(struct per_cpu *cpu_data, struct trap_context *ctx)
+{
+       u32 function_id = ctx->regs[0];
+
+       switch (function_id) {
+       case PSCI_VERSION:
+               /* Major[31:16], minor[15:0] */
+               return 2;
+
+       case PSCI_CPU_OFF:
+               /*
+                * The reset function will take care of calling
+                * psci_emulate_spin
+                */
+               arch_reset_self(cpu_data);
+
+               /* Not reached */
+               return 0;
+
+       case PSCI_CPU_ON_32:
+               return psci_emulate_cpu_on(cpu_data, ctx);
+
+       default:
+               return PSCI_NOT_SUPPORTED;
+       }
+}

diff --git a/hypervisor/arch/arm/psci_low.S b/hypervisor/arch/arm/psci_low.S
index 76eeaba9301e03fd2380d748b4fd1b4afee2d14d..58bdc0aab96e7397552169d390d3becf7f920ca6 100644 (file)
--- a/hypervisor/arch/arm/psci_low.S
+++ b/hypervisor/arch/arm/psci_low.S
@@ -13,6 +13,17 @@
 #include <asm/head.h>
 #include <asm/psci.h>
 
+       .arch_extension sec
+       .globl smc
+       /*
+        * Since we trap all SMC instructions, it may be useful to forward them
+        * when it isn't a PSCI call. The shutdown code will also have to issue
+        * a real PSCI_OFF call on secondary CPUs.
+        */
+smc:
+       smc     #0
+       bx      lr
+
        .global _psci_cpu_off
        /* r0: struct psci_mbox* */
 _psci_cpu_off:

diff --git a/hypervisor/arch/arm/setup.c b/hypervisor/arch/arm/setup.c
index 5da2d54cdc4c2aa5df1774b899b1f06682276774..4b8e3594d7292d4e31ee20e0c9d43bdf635a2511 100644 (file)
--- a/hypervisor/arch/arm/setup.c
+++ b/hypervisor/arch/arm/setup.c
@@ -59,7 +59,8 @@ int arch_init_early(void)
 int arch_cpu_init(struct per_cpu *cpu_data)
 {
        int err = 0;
-       unsigned long hcr = HCR_VM_BIT | HCR_IMO_BIT | HCR_FMO_BIT;
+       unsigned long hcr = HCR_VM_BIT | HCR_IMO_BIT | HCR_FMO_BIT
+                         | HCR_TSC_BIT;
 
        cpu_data->psci_mbox.entry = 0;
        cpu_data->virt_id = cpu_data->cpu_id;

diff --git a/hypervisor/arch/arm/traps.c b/hypervisor/arch/arm/traps.c
index 07dc5d479256dd504a16d825094c85fea54413d5..d88dc7f542ed34b351a88e9399d63267f02026b5 100644 (file)
--- a/hypervisor/arch/arm/traps.c
+++ b/hypervisor/arch/arm/traps.c
@@ -17,6 +17,7 @@
 #include <asm/control.h>
 #include <asm/gic_common.h>
 #include <asm/platform.h>
+#include <asm/psci.h>
 #include <asm/traps.h>
 #include <asm/sysregs.h>
 #include <jailhouse/printk.h>
@@ -204,11 +205,28 @@ static void dump_guest_regs(struct per_cpu *cpu_data, struct trap_context *ctx)
        panic_printk("\n");
 }
 
+static int arch_handle_smc(struct per_cpu *cpu_data, struct trap_context *ctx)
+{
+       unsigned long *regs = ctx->regs;
+
+       if (IS_PSCI_FN(regs[0]))
+               regs[0] = psci_dispatch(cpu_data, ctx);
+       else
+               regs[0] = smc(regs[0], regs[1], regs[2], regs[3]);
+
+       arch_skip_instruction(ctx);
+
+       return TRAP_HANDLED;
+}
+
 static int arch_handle_hvc(struct per_cpu *cpu_data, struct trap_context *ctx)
 {
        unsigned long *regs = ctx->regs;
 
-       regs[0] = hypercall(regs[0], regs[1], regs[2]);
+       if (IS_PSCI_FN(regs[0]))
+               regs[0] = psci_dispatch(cpu_data, ctx);
+       else
+               regs[0] = hypercall(regs[0], regs[1], regs[2]);
 
        return TRAP_HANDLED;
 }
@@ -247,6 +265,7 @@ static const trap_handler trap_handlers[38] =
 {
        [ESR_EC_CP15_64]        = arch_handle_cp15_64,
        [ESR_EC_HVC]            = arch_handle_hvc,
+       [ESR_EC_SMC]            = arch_handle_smc,
        [ESR_EC_DABT]           = arch_handle_dabt,
 };