From: Antonios Motakis <[email protected]> On AArch64 we pretty much rely on PSCI being present for SMP support (turning multiple cores on and off). This patch implements the helpers needed for SMP and plugs in the PSCI code from AArch32.
On AArch64 PSCI calls can be issued via SVC64 hypercalls as well, contrary to AArch32 which uses SVC32 calls only. We add the changes necessary to support the hypercalls that are used by a Linux root cell. CPU hotplug now is still working after we enable Jailhouse. Signed-off-by: Antonios Motakis <[email protected]> --- hypervisor/arch/arm/include/asm/psci.h | 3 +- hypervisor/arch/arm/psci.c | 4 +- hypervisor/arch/arm64/Makefile | 1 + hypervisor/arch/arm64/control.c | 84 ++++++++++++++++++++++++++++++ hypervisor/arch/arm64/include/asm/percpu.h | 5 ++ hypervisor/arch/arm64/psci_low.S | 65 +++++++++++++++++++++++ hypervisor/arch/arm64/setup.c | 5 ++ hypervisor/arch/arm64/traps.c | 34 ++++++++++++ 8 files changed, 199 insertions(+), 2 deletions(-) create mode 100644 hypervisor/arch/arm64/psci_low.S diff --git a/hypervisor/arch/arm/include/asm/psci.h b/hypervisor/arch/arm/include/asm/psci.h index dc8c007..e169bee 100644 --- a/hypervisor/arch/arm/include/asm/psci.h +++ b/hypervisor/arch/arm/include/asm/psci.h @@ -47,9 +47,10 @@ #define PSCI_CPU_IS_OFF 1 #define IS_PSCI_32(hvc) (((hvc) >> 24) == 0x84) +#define IS_PSCI_64(hvc) (((hvc) >> 24) == 0xc4) #define IS_PSCI_UBOOT(hvc) (((hvc) >> 8) == 0x95c1ba) -#define PSCI_INVALID_ADDRESS 0xffffffff +#define PSCI_INVALID_ADDRESS (-1) #ifndef __ASSEMBLY__ diff --git a/hypervisor/arch/arm/psci.c b/hypervisor/arch/arm/psci.c index e0a6703..2b489a0 100644 --- a/hypervisor/arch/arm/psci.c +++ b/hypervisor/arch/arm/psci.c @@ -78,7 +78,7 @@ 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 cpu; + unsigned long cpu; struct psci_mbox *mbox; cpu = arm_cpu_by_mpidr(cpu_data->cell, ctx->regs[1]); @@ -153,10 +153,12 @@ long psci_dispatch(struct trap_context *ctx) return 0; case PSCI_CPU_ON_32: + case PSCI_CPU_ON_64: case PSCI_CPU_ON_V0_1_UBOOT: return psci_emulate_cpu_on(cpu_data, ctx); case PSCI_AFFINITY_INFO_32: + case PSCI_AFFINITY_INFO_64: return psci_emulate_affinity_info(cpu_data, ctx); default: diff --git a/hypervisor/arch/arm64/Makefile b/hypervisor/arch/arm64/Makefile index 540f7e3..f3a9e57 100644 --- a/hypervisor/arch/arm64/Makefile +++ b/hypervisor/arch/arm64/Makefile @@ -20,6 +20,7 @@ obj-y := entry.o setup.o control.o mmio.o caches.o obj-y += ../arm/mmu_cell.o ../arm/paging.o ../arm/dbg-write.o ../arm/lib.o obj-y += exception.o traps.o obj-y += ../arm/irqchip.o ../arm/gic-common.o +obj-y += ../arm/psci.o psci_low.o obj-$(CONFIG_SERIAL_AMBA_PL011) += ../arm/dbg-write-pl011.o obj-$(CONFIG_ARM_GIC) += ../arm/gic-v2.o diff --git a/hypervisor/arch/arm64/control.c b/hypervisor/arch/arm64/control.c index f141e45..4b6643a 100644 --- a/hypervisor/arch/arm64/control.c +++ b/hypervisor/arch/arm64/control.c @@ -12,11 +12,95 @@ #include <jailhouse/control.h> #include <jailhouse/printk.h> +#include <jailhouse/string.h> #include <asm/control.h> #include <asm/irqchip.h> #include <asm/platform.h> #include <asm/traps.h> +static void arch_reset_el1(struct registers *regs) +{ + /* put the cpu in a reset state */ + /* AARCH64_TODO: handle big endian support */ + arm_write_sysreg(SPSR_EL2, RESET_PSR); + arm_write_sysreg(SCTLR_EL1, SCTLR_EL1_RES1); + arm_write_sysreg(CNTKCTL_EL1, 0); + arm_write_sysreg(PMCR_EL0, 0); + + /* wipe any other state to avoid leaking information accross cells */ + memset(regs, 0, sizeof(struct registers)); + + /* AARCH64_TODO: wipe floating point registers */ + + /* wipe special registers */ + arm_write_sysreg(SP_EL0, 0); + arm_write_sysreg(SP_EL1, 0); + arm_write_sysreg(SPSR_EL1, 0); + + /* wipe the system registers */ + arm_write_sysreg(AFSR0_EL1, 0); + arm_write_sysreg(AFSR1_EL1, 0); + arm_write_sysreg(AMAIR_EL1, 0); + arm_write_sysreg(CONTEXTIDR_EL1, 0); + arm_write_sysreg(CPACR_EL1, 0); + arm_write_sysreg(CSSELR_EL1, 0); + arm_write_sysreg(ESR_EL1, 0); + arm_write_sysreg(FAR_EL1, 0); + arm_write_sysreg(MAIR_EL1, 0); + arm_write_sysreg(PAR_EL1, 0); + arm_write_sysreg(TCR_EL1, 0); + arm_write_sysreg(TPIDRRO_EL0, 0); + arm_write_sysreg(TPIDR_EL0, 0); + arm_write_sysreg(TPIDR_EL1, 0); + arm_write_sysreg(TTBR0_EL1, 0); + arm_write_sysreg(TTBR1_EL1, 0); + arm_write_sysreg(VBAR_EL1, 0); + + /* wipe timer registers */ + arm_write_sysreg(CNTP_CTL_EL0, 0); + arm_write_sysreg(CNTP_CVAL_EL0, 0); + arm_write_sysreg(CNTP_TVAL_EL0, 0); + arm_write_sysreg(CNTV_CTL_EL0, 0); + arm_write_sysreg(CNTV_CVAL_EL0, 0); + arm_write_sysreg(CNTV_TVAL_EL0, 0); + + /* AARCH64_TODO: handle PMU registers */ + /* AARCH64_TODO: handle debug registers */ + /* AARCH64_TODO: handle system registers for AArch32 state */ +} + +void arch_reset_self(struct per_cpu *cpu_data) +{ + int err = 0; + unsigned long reset_address; + struct cell *cell = cpu_data->cell; + struct registers *regs = guest_regs(cpu_data); + + if (cell != &root_cell) { + trace_error(-EINVAL); + panic_stop(); + } + + /* + * Note: D-cache cleaning and I-cache invalidation is done on driver + * level after image is loaded. + */ + + err = irqchip_cpu_reset(cpu_data); + if (err) + printk("IRQ setup failed\n"); + + /* Wait for the driver to call cpu_up */ + reset_address = psci_emulate_spin(cpu_data); + + /* Restore an empty context */ + arch_reset_el1(regs); + + arm_write_sysreg(ELR_EL2, reset_address); + + vmreturn(regs); +} + int arch_cell_create(struct cell *cell) { return trace_error(-EINVAL); diff --git a/hypervisor/arch/arm64/include/asm/percpu.h b/hypervisor/arch/arm64/include/asm/percpu.h index e9fa99b..d168dd5 100644 --- a/hypervisor/arch/arm64/include/asm/percpu.h +++ b/hypervisor/arch/arm64/include/asm/percpu.h @@ -26,6 +26,7 @@ #include <jailhouse/printk.h> #include <asm/cell.h> #include <asm/irqchip.h> +#include <asm/psci.h> #include <asm/spinlock.h> struct pending_irq; @@ -51,6 +52,10 @@ struct per_cpu { void *gicr_base; bool flush_vcpu_caches; + + __attribute__((aligned(16))) struct psci_mbox psci_mbox; + struct psci_mbox guest_mbox; + unsigned long mpidr; } __attribute__((aligned(PAGE_SIZE))); diff --git a/hypervisor/arch/arm64/psci_low.S b/hypervisor/arch/arm64/psci_low.S new file mode 100644 index 0000000..f1ee1fc --- /dev/null +++ b/hypervisor/arch/arm64/psci_low.S @@ -0,0 +1,65 @@ +/* + * Jailhouse AArch64 support + * + * Copyright (C) 2015 Huawei Technologies Duesseldorf GmbH + * + * Authors: + * Antonios Motakis <[email protected]> + * + * This work is licensed under the terms of the GNU GPL, version 2. See + * the COPYING file in the top-level directory. + */ + +#include <asm/head.h> +#include <asm/psci.h> + + .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 + ret + + .global _psci_cpu_off + /* x0: struct psci_mbox* */ +_psci_cpu_off: + ldr x2, =PSCI_INVALID_ADDRESS + /* Clear mbox */ + str x2, [x0] + + /* Wait for a CPU_ON call that updates the mbox */ +1: wfe + ldr x3, [x0] + cmp x3, #PSCI_INVALID_ADDRESS + b.eq 1b + + /* Jump to the requested entry, with a parameter */ + ldr x0, [x0, #8] + br x3 + ret + + .global _psci_cpu_on + /* x0: struct psci_mbox*, x1: entry, x2: context */ +_psci_cpu_on: +1: ldxp x4, x5, [x0] + cmp x4, #PSCI_INVALID_ADDRESS + b.ne store_failed + stxp w7, x1, x2, [x0] + cbnz w7, 1b + + dsb ishst + sev + + mov x0, #0 + ret + +store_failed: + mov x0, #PSCI_ALREADY_ON + ret + + .global _psci_suspend_return +_psci_suspend_return: + ret diff --git a/hypervisor/arch/arm64/setup.c b/hypervisor/arch/arm64/setup.c index 838c541..166b7b6 100644 --- a/hypervisor/arch/arm64/setup.c +++ b/hypervisor/arch/arm64/setup.c @@ -12,9 +12,11 @@ #include <jailhouse/entry.h> #include <jailhouse/printk.h> +#include <jailhouse/processor.h> #include <asm/control.h> #include <asm/irqchip.h> #include <asm/setup.h> +#include <asm/smp.h> int arch_init_early(void) { @@ -34,6 +36,9 @@ int arch_cpu_init(struct per_cpu *cpu_data) /* switch to the permanent page tables */ enable_mmu_el2(hv_paging_structs.root_table); + cpu_data->psci_mbox.entry = 0; + cpu_data->mpidr = phys_processor_id(); + err = arch_mmu_cpu_cell_init(cpu_data); if (err) return err; diff --git a/hypervisor/arch/arm64/traps.c b/hypervisor/arch/arm64/traps.c index fec057d..e0051bb 100644 --- a/hypervisor/arch/arm64/traps.c +++ b/hypervisor/arch/arm64/traps.c @@ -30,6 +30,32 @@ void arch_skip_instruction(struct trap_context *ctx) ctx->pc += (instruction_length ? 4 : 2); } +static int arch_handle_smc(struct trap_context *ctx) +{ + unsigned long *regs = ctx->regs; + + if (IS_PSCI_32(regs[0]) || IS_PSCI_64(regs[0])) { + regs[0] = psci_dispatch(ctx); + arch_skip_instruction(ctx); + + return TRAP_HANDLED; + } + + return TRAP_UNHANDLED; +} + +static int arch_handle_hvc(struct trap_context *ctx) +{ + unsigned long *regs = ctx->regs; + + if (IS_PSCI_32(regs[0]) || IS_PSCI_64(regs[0])) { + regs[0] = psci_dispatch(ctx); + return TRAP_HANDLED; + } + + return TRAP_UNHANDLED; +} + static void dump_regs(struct trap_context *ctx) { unsigned char i; @@ -112,6 +138,14 @@ static void arch_handle_trap(struct per_cpu *cpu_data, ret = arch_handle_dabt(&ctx); break; + case ESR_EC_SMC64: + ret = arch_handle_smc(&ctx); + break; + + case ESR_EC_HVC64: + ret = arch_handle_hvc(&ctx); + break; + default: ret = TRAP_UNHANDLED; } -- 2.8.0.rc3 -- You received this message because you are subscribed to the Google Groups "Jailhouse" group. To unsubscribe from this group and stop receiving emails from it, send an email to [email protected]. For more options, visit https://groups.google.com/d/optout.
