hypervisor: arm64: PSCI support for SMP on AArch64

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 <antonios.motakis@huawei.com>

hypervisor/arch/arm/include/asm/psci.h

@@ -48,7 +48,7 @@
 
 #define IS_PSCI_FN(hvc)		((((hvc) >> 24) | 0x40) == 0xc4)
 
-#define PSCI_INVALID_ADDRESS	0xffffffff
+#define PSCI_INVALID_ADDRESS	(-1)
 
 #ifndef __ASSEMBLY__
 

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_mpid(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:

hypervisor/arch/arm64/Makefile

@@ -20,6 +20,7 @@ obj-y := entry.o setup.o control.o mmio.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

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);

hypervisor/arch/arm64/include/asm/percpu.h

@@ -25,6 +25,7 @@
 
 #include <jailhouse/printk.h>
 #include <asm/cell.h>
+#include <asm/psci.h>
 #include <asm/spinlock.h>
 
 struct pending_irq;
@@ -60,6 +61,11 @@ struct per_cpu {
 	void *gicr_base;
 
 	bool flush_vcpu_caches;
+
+	__attribute__((aligned(16))) struct psci_mbox psci_mbox;
+	struct psci_mbox guest_mbox;
+
+	unsigned int virt_id;
 	union mpidr mpidr;
 } __attribute__((aligned(PAGE_SIZE)));
 

hypervisor/arch/arm64/psci_low.S

@@ -0,0 +1,65 @@
+/*
+ * Jailhouse AArch64 support
+ *
+ * Copyright (C) 2015 Huawei Technologies Duesseldorf GmbH
+ *
+ * Authors:
+ *  Antonios Motakis <antonios.motakis@huawei.com>
+ *
+ * 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

hypervisor/arch/arm64/setup.c

@@ -15,6 +15,7 @@
 #include <asm/control.h>
 #include <asm/irqchip.h>
 #include <asm/setup.h>
+#include <asm/smp.h>
 
 int arch_init_early(void)
 {
@@ -34,6 +35,10 @@ 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->virt_id = cpu_data->cpu_id;
+	arm_read_sysreg(MPIDR_EL1, cpu_data->mpidr.val);
+
 	err = arch_mmu_cpu_cell_init(cpu_data);
 	if (err)
 		return err;

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_FN(regs[0]))
+		return TRAP_UNHANDLED;
+
+	regs[0] = psci_dispatch(ctx);
+	arch_skip_instruction(ctx);
+
+	return TRAP_HANDLED;
+}
+
+static int arch_handle_hvc(struct trap_context *ctx)
+{
+	unsigned long *regs = ctx->regs;
+
+	if (!IS_PSCI_FN(regs[0]))
+		return TRAP_UNHANDLED;
+
+	regs[0] = psci_dispatch(ctx);
+	arch_skip_instruction(ctx);
+
+	return TRAP_HANDLED;
+}
+
 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;
 	}