[PATCH 00/13] arm64: Virtualization Host Extension support

@@ -74,6 +74,11 @@ static inline bool is_hyp_mode_mismatched(void)
 {
 	return !!(__boot_cpu_mode & BOOT_CPU_MODE_MISMATCH);
 }
+
+static inline bool is_kernel_in_hyp_mode(void)
+{
+	return false;
+}
 #endif
 
 #endif /* __ASSEMBLY__ */

@@ -23,6 +23,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include <asm/ptrace.h>
+
 /*
  * __boot_cpu_mode records what mode CPUs were booted in.
  * A correctly-implemented bootloader must start all CPUs in the same mode:

@@ -50,6 +52,14 @@ static inline bool is_hyp_mode_mismatched(void)
 	return __boot_cpu_mode[0] != __boot_cpu_mode[1];
 }
 
+static inline bool is_kernel_in_hyp_mode(void)
+{
+	u64 el;
+
+	asm("mrs %0, CurrentEL" : "=r" (el));
+	return el == CurrentEL_EL2;
+}
+
 /* The section containing the hypervisor text */
 extern char __hyp_text_start[];
 extern char __hyp_text_end[];

@@ -25,8 +25,9 @@
 #define ARM64_WORKAROUND_DEVICE_LOAD_ACQUIRE	1
 #define ARM64_WORKAROUND_845719			2
 #define ARM64_HAS_SYSREG_GIC_CPUIF		3
+#define ARM64_HAS_VIRT_HOST_EXTN		4
 
-#define ARM64_NCAPS				4
+#define ARM64_NCAPS				5
 
 #ifndef __ASSEMBLY__
 

@@ -21,6 +21,7 @@
 #include <linux/types.h>
 #include <asm/cpu.h>
 #include <asm/cpufeature.h>
+#include <asm/virt.h>
 
 static bool
 has_id_aa64pfr0_feature(const struct arm64_cpu_capabilities *entry)

@@ -31,6 +32,11 @@ has_id_aa64pfr0_feature(const struct arm64_cpu_capabilities *entry)
 	return (val & entry->register_mask) == entry->register_value;
 }
 
+static bool runs_at_el2(const struct arm64_cpu_capabilities *entry)
+{
+	return is_kernel_in_hyp_mode();
+}
+
 static const struct arm64_cpu_capabilities arm64_features[] = {
 	{
 		.desc = "GIC system register CPU interface",

@@ -39,6 +45,11 @@ static const struct arm64_cpu_capabilities arm64_features[] = {
 		.register_mask = (0xf << 24),
 		.register_value = (1 << 24),
 	},
+	{
+		.desc = "Virtualization Host Extensions",
+		.capability = ARM64_HAS_VIRT_HOST_EXTN,
+		.matches = runs_at_el2,
+	},
 	{},
 };
 

@@ -55,6 +55,20 @@
 
 #ifdef __ASSEMBLY__
 
+#include <asm/kvm_arm.h>
+
+.macro setup_vtcr tmp1, tmp2
+	mov	\tmp1, #(VTCR_EL2_FLAGS & 0xffff)
+	movk	\tmp1, #(VTCR_EL2_FLAGS >> 16), lsl #16
+	/*
+	 * Read the PARange bits from ID_AA64MMFR0_EL1 and set the PS bits in
+	 * VTCR_EL2.
+	 */
+	mrs	\tmp2, id_aa64mmfr0_el1
+	bfi	\tmp1, \tmp2, #16, #3
+	msr	vtcr_el2, \tmp1
+	isb
+.endm
 /*
  * Convert a kernel VA into a HYP VA.
  * reg: VA to be converted.

@@ -87,14 +87,7 @@ __do_hyp_init:
 #endif
 	msr	tcr_el2, x4
 
-	ldr	x4, =VTCR_EL2_FLAGS
-	/*
-	 * Read the PARange bits from ID_AA64MMFR0_EL1 and set the PS bits in
-	 * VTCR_EL2.
-	 */
-	mrs	x5, ID_AA64MMFR0_EL1
-	bfi	x4, x5, #16, #3
-	msr	vtcr_el2, x4
+	setup_vtcr x4, x5
 
 	mrs	x4, mair_el1
 	msr	mair_el2, x4

@@ -23,6 +23,7 @@
 #include <asm/types.h>
 
 /* Hyp Configuration Register (HCR) bits */
+#define HCR_E2H		(UL(1) << 34)
 #define HCR_ID		(UL(1) << 33)
 #define HCR_CD		(UL(1) << 32)
 #define HCR_RW_SHIFT	31

@@ -44,6 +44,8 @@ void kvm_inject_pabt(struct kvm_vcpu *vcpu, unsigned long addr);
 static inline void vcpu_reset_hcr(struct kvm_vcpu *vcpu)
 {
 	vcpu->arch.hcr_el2 = HCR_GUEST_FLAGS;
+	if (is_kernel_in_hyp_mode())
+		vcpu->arch.hcr_el2 |= HCR_E2H;
 	if (test_bit(KVM_ARM_VCPU_EL1_32BIT, vcpu->arch.features))
 		vcpu->arch.hcr_el2 &= ~HCR_RW;
 }

@@ -800,7 +800,8 @@
 .endm
 
 .macro deactivate_traps
-	mov	x2, #HCR_RW
+ifnvhe _S_(mov	x2, #HCR_RW),			_S_(mov	x2, #HCR_RW|HCR_TGE)
+ifnvhe 	nop,					_S_(orr	x2, x2, #HCR_E2H)
 	msr	hcr_el2, x2
 	msr	cptr_el2, xzr
 	msr	hstr_el2, xzr

@@ -36,6 +36,11 @@
  */
 extern u32 __boot_cpu_mode[2];
 
+/*
+ * __run_cpu_mode records the mode the boot CPU uses for the kernel.
+ */
+extern u32 __run_cpu_mode;
+
 void __hyp_set_vectors(phys_addr_t phys_vector_base);
 phys_addr_t __hyp_get_vectors(void);
 

@@ -60,6 +65,16 @@ static inline bool is_kernel_in_hyp_mode(void)
 	return el == CurrentEL_EL2;
 }
 
+static inline bool is_kernel_mode_mismatched(void)
+{
+	u64 el;
+	u32 mode;
+
+	asm("mrs %0, CurrentEL" : "=r" (el));
+	mode = ACCESS_ONCE(__run_cpu_mode);
+	return el != mode;
+}
+
 /* The section containing the hypervisor text */
 extern char __hyp_text_start[];
 extern char __hyp_text_end[];

@@ -578,7 +578,20 @@ ENTRY(set_cpu_boot_mode_flag)
 1:	str	w20, [x1]			// This CPU has booted in EL1
 	dmb	sy
 	dc	ivac, x1			// Invalidate potentially stale cache line
+	adr_l	x1, __run_cpu_mode
+	ldr	w0, [x1]
+	mrs	x20, CurrentEL
+	str	w20, [x1]
+	dmb	sy
+	dc	ivac, x1			// Invalidate potentially stale cache line
+	cbz	x0, skip_el_check
+	cmp	x0, x20
+	bne	mismatched_el
+skip_el_check:
 	ret
+mismatched_el:
+	wfi
+	b	mismatched_el
 ENDPROC(set_cpu_boot_mode_flag)
 
 /*

@@ -593,6 +606,8 @@ ENDPROC(set_cpu_boot_mode_flag)
 ENTRY(__boot_cpu_mode)
 	.long	BOOT_CPU_MODE_EL2
 	.long	BOOT_CPU_MODE_EL1
+ENTRY(__run_cpu_mode)
+	.long	0
 	.popsection
 
 #ifdef CONFIG_SMP

@@ -52,6 +52,7 @@
 #include <asm/sections.h>
 #include <asm/tlbflush.h>
 #include <asm/ptrace.h>
+#include <asm/virt.h>
 
 #define CREATE_TRACE_POINTS
 #include <trace/events/ipi.h>

@@ -112,6 +113,9 @@ int __cpu_up(unsigned int cpu, struct task_struct *idle)
 			pr_crit("CPU%u: failed to come online\n", cpu);
 			ret = -EIO;
 		}
+
+		if (is_kernel_mode_mismatched())
+			panic("CPU%u: incompatible execution level", cpu);
 	} else {
 		pr_err("CPU%u: failed to boot: %d\n", cpu, ret);
 	}

@@ -573,6 +573,9 @@ int kvm_arch_vcpu_ioctl_run(struct kvm_vcpu *vcpu, struct kvm_run *run)
 		 * disabled.  Enabling the interrupts now will have
 		 * the effect of taking the interrupt again, in SVC
 		 * mode this time.
+		 *
+		 * With VHE, the interrupt is likely to have been
+		 * already taken already.
 		 */
 		local_irq_enable();
 

@@ -714,8 +714,10 @@ skip_el1_restore:
 .endm
 
 .macro skip_32bit_state tmp, target
-	// Skip 32bit state if not needed
-	mrs	\tmp, hcr_el2
+	// Skip 32bit state if not needed.
+	// With VHE, we may have cleared the RW bit in HCR_EL2 early,
+	// and have to rely on the memory version instead.
+ifnvhe "mrs	\tmp, hcr_el2",			_S_(ldr	\tmp, [x0, #VCPU_HCR_EL2])
 	tbnz	\tmp, #HCR_RW_SHIFT, \target
 .endm
 

@@ -1053,11 +1055,18 @@ __kvm_vcpu_return:
 	save_guest_32bit_state
 
 	save_timer_state
+ifnvhe "b	1f",		nop
+	alternative_insn	"bl __vgic_v2_disable", \
+				"bl __vgic_v3_disable", \
+				ARM64_HAS_SYSREG_GIC_CPUIF
+1:
 	save_vgic_state
 
 	deactivate_traps
 	deactivate_vm
 
+__kvm_vcpu_return_irq:
+
 	// Host context
 	ldr	x2, [x0, #VCPU_HOST_CONTEXT]
 	kern_hyp_va x2

@@ -1355,8 +1364,47 @@ el1_irq:
 	push	x0, x1
 	push	x2, x3
 	mrs	x0, tpidr_el2
-	mov	x1, #ARM_EXCEPTION_IRQ
-	b	__kvm_vcpu_return
+ifnvhe _S_(mov	x1, #ARM_EXCEPTION_IRQ),	nop
+ifnvhe "b	__kvm_vcpu_return", 		nop
+
+	// Fallthrough for VHE
+	add	x2, x0, #VCPU_CONTEXT
+
+	save_guest_regs
+	bl	__save_shared_sysregs
+	save_timer_state
+	alternative_insn	"bl __vgic_v2_disable", \
+				"bl __vgic_v3_disable", \
+				ARM64_HAS_SYSREG_GIC_CPUIF
+	deactivate_traps
+	deactivate_vm
+
+	isb	// Needed to ensure TGE is on and vectors have been switched
+
+	ldr	x2, [x0, #VCPU_HOST_CONTEXT]
+	restore_shared_sysregs
+
+	mov	x0, x2
+	adrp	x1, handle_arch_irq
+	add	x1, x1, #:lo12:handle_arch_irq
+	ldr	x1, [x1]
+	blr	x1
+
+	// Back from the interrupt handler, finalize the world switch
+	mrs	x0, tpidr_el2
+	add	x2, x0, #VCPU_CONTEXT
+
+	bl	__save_fpsimd
+	bl	__save_sysregs	// We've already saved the shared regs
+	save_guest_32bit_state
+
+	skip_debug_state x3, 1f
+	bl	__save_debug
+1:
+	save_vgic_state
+
+	mov	x1,	#ARM_EXCEPTION_IRQ
+	b	__kvm_vcpu_return_irq
 
 	.ltorg
 

@@ -26,16 +26,30 @@
 #include <asm/kvm_arm.h>
 #include <asm/kvm_mmu.h>
 
+#include "vhe-macros.h"
+
 	.text
 	.pushsection	.hyp.text, "ax"
 
+ENTRY(__vgic_v2_disable)
+	/* Get VGIC VCTRL base into x2 */
+	ldr	x2, [x0, #VCPU_KVM]
+	kern_hyp_va	x2
+	ldr	x2, [x2, #KVM_VGIC_VCTRL]
+	kern_hyp_va	x2
+	cbz	x2, 1f		// disabled
+
+	/* Clear GICH_HCR */
+	str	wzr, [x2, #GICH_HCR]
+1:	ret
+ENDPROC(__vgic_v2_disable)
+
 /*
  * Save the VGIC CPU state into memory
  * x0: Register pointing to VCPU struct
  * Do not corrupt x1!!!
  */
 ENTRY(__save_vgic_v2_state)
-__save_vgic_v2_state:
 	/* Get VGIC VCTRL base into x2 */
 	ldr	x2, [x0, #VCPU_KVM]
 	kern_hyp_va	x2

@@ -75,7 +89,7 @@ CPU_BE(	str	w10, [x3, #VGIC_V2_CPU_ELRSR] )
 	str	w11, [x3, #VGIC_V2_CPU_APR]
 
 	/* Clear GICH_HCR */
-	str	wzr, [x2, #GICH_HCR]
+ifnvhe	_S_(str	wzr, [x2, #GICH_HCR]),		nop
 
 	/* Save list registers */
 	add	x2, x2, #GICH_LR0

@@ -95,7 +109,6 @@ ENDPROC(__save_vgic_v2_state)
  * x0: Register pointing to VCPU struct
  */
 ENTRY(__restore_vgic_v2_state)
-__restore_vgic_v2_state:
 	/* Get VGIC VCTRL base into x2 */
 	ldr	x2, [x0, #VCPU_KVM]
 	kern_hyp_va	x2

@@ -25,6 +25,8 @@
 #include <asm/kvm_asm.h>
 #include <asm/kvm_arm.h>
 
+#include "vhe-macros.h"
+
 	.text
 	.pushsection	.hyp.text, "ax"
 

@@ -35,11 +37,21 @@
 #define LR_OFFSET(n)	(VGIC_V3_CPU_LR + (15 - n) * 8)
 
 /*
+ * Disable the vcpu interface, preventing interrupts from
+ * being delivered.
+ */
+ENTRY(__vgic_v3_disable)
+	// Nuke the HCR register.
+	msr_s	ICH_HCR_EL2, xzr
+	ret
+ENDPROC(__vgic_v3_disable)
+
+/*
  * Save the VGIC CPU state into memory
  * x0: Register pointing to VCPU struct
  * Do not corrupt x1!!!
  */
-.macro	save_vgic_v3_state
+ENTRY(__save_vgic_v3_state)
 	// Compute the address of struct vgic_cpu
 	add	x3, x0, #VCPU_VGIC_CPU
 

@@ -58,7 +70,7 @@
 	str	w7, [x3, #VGIC_V3_CPU_EISR]
 	str	w8, [x3, #VGIC_V3_CPU_ELRSR]
 
-	msr_s	ICH_HCR_EL2, xzr
+ifnvhe	_S_(msr_s	ICH_HCR_EL2, xzr),	nop
 
 	mrs_s	x21, ICH_VTR_EL2
 	mvn	w22, w21

@@ -137,15 +149,17 @@
 	orr	x5, x5, #ICC_SRE_EL2_ENABLE
 	msr_s	ICC_SRE_EL2, x5
 	isb
-	mov	x5, #1
+	// If using VHE, we don't care about EL1 and can early out.
+ifnvhe "mov	x5, #1",			ret
 	msr_s	ICC_SRE_EL1, x5
-.endm
+	ret
+ENDPROC(__save_vgic_v3_state)
 
 /*
  * Restore the VGIC CPU state from memory
  * x0: Register pointing to VCPU struct
  */
-.macro	restore_vgic_v3_state
+ENTRY(__restore_vgic_v3_state)
 	// Compute the address of struct vgic_cpu
 	add	x3, x0, #VCPU_VGIC_CPU
 

@@ -249,15 +263,6 @@
 	and	x5, x5, #~ICC_SRE_EL2_ENABLE
 	msr_s	ICC_SRE_EL2, x5
 1:
-.endm
-
-ENTRY(__save_vgic_v3_state)
-	save_vgic_v3_state
-	ret
-ENDPROC(__save_vgic_v3_state)
-
-ENTRY(__restore_vgic_v3_state)
-	restore_vgic_v3_state
 	ret
 ENDPROC(__restore_vgic_v3_state)
 

@@ -280,11 +280,6 @@ skip_el1_restore:
 	mrs	x20,	par_el1
 	mrs	x21,	mdscr_el1
 
-	mrs	x22,	tpidr_el0
-	mrs	x23,	tpidrro_el0
-	mrs	x24,	tpidr_el1
-	mrs	x25,	actlr_el1
-
 	stp	x4, x5, [x3]
 	stp	x6, x7, [x3, #16]
 	stp	x8, x9, [x3, #32]

@@ -294,8 +289,6 @@ skip_el1_restore:
 	stp	x16, x17, [x3, #96]
 	stp	x18, x19, [x3, #112]
 	stp	x20, x21, [x3, #128]
-	stp	x22, x23, [x3, #144]
-	stp	x24, x25, [x3, #160]
 .endm
 
 .macro save_debug

@@ -508,8 +501,6 @@ skip_el1_restore:
 	ldp	x16, x17, [x3, #96]
 	ldp	x18, x19, [x3, #112]
 	ldp	x20, x21, [x3, #128]
-	ldp	x22, x23, [x3, #144]
-	ldp	x24, x25, [x3, #160]
 
 	msr	vmpidr_el2,	x4
 	msr	csselr_el1,	x5

@@ -529,11 +520,6 @@ skip_el1_restore:
 	msr_el1(cntkctl,	x19)
 	msr	par_el1,	x20
 	msr	mdscr_el1,	x21
-
-	msr	tpidr_el0,	x22
-	msr	tpidrro_el0,	x23
-	msr	tpidr_el1,	x24
-	msr	actlr_el1,	x25
 .endm
 
 .macro restore_debug

@@ -913,10 +899,12 @@ ifnvhe	nop,					"msr	vbar_el1, x2"
 
 .macro save_timer_state
 	// x0: vcpu pointer
-	ldr	x2, [x0, #VCPU_KVM]
-	kern_hyp_va x2
-	ldr	w3, [x2, #KVM_TIMER_ENABLED]
-	cbz	w3, 1f
+	// x1: return code
+	// x2: vcpu context
+	ldr	x3, [x0, #VCPU_KVM]
+	kern_hyp_va x3
+	ldr	w4, [x3, #KVM_TIMER_ENABLED]
+	cbz	w4, 1f
 
 	mrs_el0(x3, cntv_ctl)
 	and	x3, x3, #3

@@ -931,9 +919,9 @@ ifnvhe	nop,					"msr	vbar_el1, x2"
 
 1:
 	// Allow physical timer/counter access for the host
-	mrs	x2, cnthctl_el2
-	orr	x2, x2, #3
-	msr	cnthctl_el2, x2
+	mrs	x3, cnthctl_el2
+	orr	x3, x3, #3
+	msr	cnthctl_el2, x3
 
 	// Clear cntvoff for the host
 ifnvhe "msr	cntvoff_el2, xzr",		nop

@@ -941,34 +929,43 @@ ifnvhe "msr	cntvoff_el2, xzr",		nop
 
 .macro restore_timer_state
 	// x0: vcpu pointer
+	// x2: vcpu context
 	// Disallow physical timer access for the guest
 	// Physical counter access is allowed
-	mrs	x2, cnthctl_el2
-	orr	x2, x2, #1
-	bic	x2, x2, #2
-	msr	cnthctl_el2, x2
-
-	ldr	x2, [x0, #VCPU_KVM]
-	kern_hyp_va x2
-	ldr	w3, [x2, #KVM_TIMER_ENABLED]
-	cbz	w3, 1f
-
-	ldr	x3, [x2, #KVM_TIMER_CNTVOFF]
-	msr	cntvoff_el2, x3
-	ldr	x2, [x0, #VCPU_TIMER_CNTV_CVAL]
-	msr_el0(cntv_cval, x2)
+	mrs	x3, cnthctl_el2
+	orr	x3, x3, #1
+	bic	x3, x3, #2
+	msr	cnthctl_el2, x3
+
+	ldr	x3, [x0, #VCPU_KVM]
+	kern_hyp_va x3
+	ldr	w4, [x3, #KVM_TIMER_ENABLED]
+	cbz	w4, 1f
+
+	ldr	x4, [x3, #KVM_TIMER_CNTVOFF]
+	msr	cntvoff_el2, x4
+	ldr	x4, [x0, #VCPU_TIMER_CNTV_CVAL]
+	msr_el0(cntv_cval, x4)
 	isb
 
-	ldr	w2, [x0, #VCPU_TIMER_CNTV_CTL]
-	and	x2, x2, #3
-	msr_el0(cntv_ctl, x2)
+	ldr	w4, [x0, #VCPU_TIMER_CNTV_CTL]
+	and	x4, x4, #3
+	msr_el0(cntv_ctl, x4)
 1:
 .endm
 
+__save_shared_sysregs:
+	save_shared_sysregs
+	ret
+
 __save_sysregs:
 	save_sysregs
 	ret
 
+__restore_shared_sysregs:
+	restore_shared_sysregs
+	ret
+
 __restore_sysregs:
 	restore_sysregs
 	ret

@@ -1010,10 +1007,9 @@ ENTRY(__kvm_vcpu_run)
 
 	save_host_regs
 	bl __save_fpsimd
-ifnvhe "bl	__save_sysregs",		nop
-ifnvhe "b	1f",				nop
-	save_shared_sysregs
-1:
+ifnvhe "bl	__save_sysregs",		"nop"
+	bl	__save_shared_sysregs
+
 	compute_debug_state 1f
 	bl	__save_debug
 1:

@@ -1027,6 +1023,7 @@ ifnvhe "b	1f",				nop
 	add	x2, x0, #VCPU_CONTEXT
 
 	bl __restore_sysregs
+	bl __restore_shared_sysregs
 	bl __restore_fpsimd
 
 	skip_debug_state x3, 1f

@@ -1048,6 +1045,7 @@ __kvm_vcpu_return:
 	save_guest_regs
 	bl __save_fpsimd
 	bl __save_sysregs
+	bl __save_shared_sysregs
 
 	skip_debug_state x3, 1f
 	bl	__save_debug

@@ -1064,10 +1062,8 @@ __kvm_vcpu_return:
 	ldr	x2, [x0, #VCPU_HOST_CONTEXT]
 	kern_hyp_va x2
 
-ifnvhe "bl	__restore_sysregs",		nop
-ifnvhe "b	1f",				nop
-	restore_shared_sysregs
-1:
+ifnvhe "bl	__restore_sysregs",		"nop"
+	bl	__restore_shared_sysregs
 	bl __restore_fpsimd
 
 	skip_debug_state x3, 1f

@@ -1159,7 +1155,8 @@ __kvm_hyp_panic:
 	ldr	x2, [x0, #VCPU_HOST_CONTEXT]
 	kern_hyp_va x2
 
-	bl __restore_sysregs
+ifnvhe "bl __restore_sysregs",			"nop"
+	bl __restore_shared_sysregs
 
 1:	adr	x0, __hyp_panic_str
 	adr	x1, 2f

@@ -29,6 +29,7 @@
 #include <asm/asm-offsets.h>
 #include <asm/cache.h>
 #include <asm/cputype.h>
+#include <asm/kvm_mmu.h>
 #include <asm/memory.h>
 #include <asm/thread_info.h>
 #include <asm/pgtable-hwdef.h>

@@ -481,8 +482,16 @@ CPU_LE(	bic	x0, x0, #(3 << 24)	)	// Clear the EE and E0E bits for EL1
 	isb
 	ret
 
+	/* Check for VHE being present */
+2:	mrs	x2, id_aa64mmfr1_el1
+	ubfx	x2, x2, #8, #4
+
 	/* Hyp configuration. */
-2:	mov	x0, #(1 << 31)			// 64-bit EL1
+	mov	x0, #HCR_RW			// 64-bit EL1
+	cbz	x2, set_hcr
+	orr	x0, x0, #HCR_TGE		// Enable Host Extensions
+	orr	x0, x0, #HCR_E2H
+set_hcr:
 	msr	hcr_el2, x0
 
 	/* Generic timers. */

@@ -522,6 +531,9 @@ CPU_LE(	movk	x0, #0x30d0, lsl #16	)	// Clear EE and E0E on LE systems
 
 	/* Coprocessor traps. */
 	mov	x0, #0x33ff
+	cbz	x2, set_cptr
+	orr	x0, x0, #(3 << 20)		// Don't trap FP
+set_cptr:
 	msr	cptr_el2, x0			// Disable copro. traps to EL2
 
 #ifdef CONFIG_COMPAT

@@ -531,6 +543,15 @@ CPU_LE(	movk	x0, #0x30d0, lsl #16	)	// Clear EE and E0E on LE systems
 	/* Stage-2 translation */
 	msr	vttbr_el2, xzr
 
+	cbz	x2, install_el2_stub
+
+	setup_vtcr x4, x5
+
+	mov	w20, #BOOT_CPU_MODE_EL2		// This CPU booted in EL2
+	isb
+	ret
+
+install_el2_stub:
 	/* Hypervisor stub */
 	adrp	x0, __hyp_stub_vectors
 	add	x0, x0, #:lo12:__hyp_stub_vectors

@@ -18,6 +18,7 @@
 #ifndef __ARM_KVM_ASM_H__
 #define __ARM_KVM_ASM_H__
 
+#include <asm/sysreg.h>
 #include <asm/virt.h>
 
 /*

@@ -27,7 +28,7 @@
 #define	MPIDR_EL1	1	/* MultiProcessor Affinity Register */
 #define	CSSELR_EL1	2	/* Cache Size Selection Register */
 #define	SCTLR_EL1	3	/* System Control Register */
-#define	ACTLR_EL1	4	/* Auxiliary Control Register */
+#define	AMAIR_EL1	4	/* Aux Memory Attribute Indirection Register */
 #define	CPACR_EL1	5	/* Coprocessor Access Control */
 #define	TTBR0_EL1	6	/* Translation Table Base Register 0 */
 #define	TTBR1_EL1	7	/* Translation Table Base Register 1 */

@@ -39,13 +40,13 @@
 #define	MAIR_EL1	13	/* Memory Attribute Indirection Register */
 #define	VBAR_EL1	14	/* Vector Base Address Register */
 #define	CONTEXTIDR_EL1	15	/* Context ID Register */
-#define	TPIDR_EL0	16	/* Thread ID, User R/W */
-#define	TPIDRRO_EL0	17	/* Thread ID, User R/O */
-#define	TPIDR_EL1	18	/* Thread ID, Privileged */
-#define	AMAIR_EL1	19	/* Aux Memory Attribute Indirection Register */
-#define	CNTKCTL_EL1	20	/* Timer Control Register (EL1) */
-#define	PAR_EL1		21	/* Physical Address Register */
-#define MDSCR_EL1	22	/* Monitor Debug System Control Register */
+#define	CNTKCTL_EL1	16	/* Timer Control Register (EL1) */
+#define	PAR_EL1		17	/* Physical Address Register */
+#define	MDSCR_EL1	18	/* Monitor Debug System Control Register */
+#define	TPIDR_EL0	19	/* Thread ID, User R/W */
+#define	TPIDRRO_EL0	20	/* Thread ID, User R/O */
+#define	TPIDR_EL1	21	/* Thread ID, Privileged */
+#define	ACTLR_EL1	22	/* Auxiliary Control Register */
 #define DBGBCR0_EL1	23	/* Debug Breakpoint Control Registers (0-15) */
 #define DBGBCR15_EL1	38
 #define DBGBVR0_EL1	39	/* Debug Breakpoint Value Registers (0-15) */

@@ -106,6 +107,29 @@
 
 #define NR_COPRO_REGS	(NR_SYS_REGS * 2)
 
+#define sctlr_EL12              sys_reg(3, 5, 1, 0, 0)
+#define cpacr_EL12              sys_reg(3, 5, 1, 0, 2)
+#define ttbr0_EL12              sys_reg(3, 5, 2, 0, 0)
+#define ttbr1_EL12              sys_reg(3, 5, 2, 0, 1)
+#define tcr_EL12                sys_reg(3, 5, 2, 0, 2)
+#define afsr0_EL12              sys_reg(3, 5, 5, 1, 0)
+#define afsr1_EL12              sys_reg(3, 5, 5, 1, 1)
+#define esr_EL12                sys_reg(3, 5, 5, 2, 0)
+#define far_EL12                sys_reg(3, 5, 6, 0, 0)
+#define mair_EL12               sys_reg(3, 5, 10, 2, 0)
+#define amair_EL12              sys_reg(3, 5, 10, 3, 0)
+#define vbar_EL12               sys_reg(3, 5, 12, 0, 0)
+#define contextidr_EL12         sys_reg(3, 5, 13, 0, 1)
+#define cntkctl_EL12            sys_reg(3, 5, 14, 1, 0)
+#define cntp_tval_EL02          sys_reg(3, 5, 14, 2, 0)
+#define cntp_ctl_EL02           sys_reg(3, 5, 14, 2, 1)
+#define cntp_cval_EL02          sys_reg(3, 5, 14, 2, 2)
+#define cntv_tval_EL02          sys_reg(3, 5, 14, 3, 0)
+#define cntv_ctl_EL02           sys_reg(3, 5, 14, 3, 1)
+#define cntv_cval_EL02          sys_reg(3, 5, 14, 3, 2)
+#define spsr_EL12               sys_reg(3, 5, 4, 0, 0)
+#define elr_EL12                sys_reg(3, 5, 4, 0, 1)
+
 #define ARM_EXCEPTION_IRQ	  0
 #define ARM_EXCEPTION_TRAP	  1
 

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2012,2013 - ARM Ltd
+ * Copyright (C) 2012-2015 - ARM Ltd
  * Author: Marc Zyngier <marc.zyngier@arm.com>
  *
  * This program is free software; you can redistribute it and/or modify

@@ -67,40 +67,52 @@
 	stp	x29, lr, [x3, #80]
 
 	mrs	x19, sp_el0
-	mrs	x20, elr_el2		// pc before entering el2
-	mrs	x21, spsr_el2		// pstate before entering el2
+	str	x19, [x3, #96]
+.endm
 
-	stp	x19, x20, [x3, #96]
-	str	x21, [x3, #112]
+.macro save_el1_state
+	mrs_hyp(x20, ELR)		// pc before entering el2
+	mrs_hyp(x21, SPSR)		// pstate before entering el2
 
 	mrs	x22, sp_el1
-	mrs	x23, elr_el1
-	mrs	x24, spsr_el1
+
+	mrs_el1(x23, elr)
+	mrs_el1(x24, spsr)
+
+	add	x3, x2, #CPU_XREG_OFFSET(31)	// SP_EL0
+	stp	x20, x21, [x3, #8]	// HACK: Store to the regs after SP_EL0
 
 	str	x22, [x2, #CPU_GP_REG_OFFSET(CPU_SP_EL1)]
 	str	x23, [x2, #CPU_GP_REG_OFFSET(CPU_ELR_EL1)]
 	str	x24, [x2, #CPU_SPSR_OFFSET(KVM_SPSR_EL1)]
 .endm
 
-.macro restore_common_regs
+.macro restore_el1_state
 	// x2: base address for cpu context
 	// x3: tmp register
 
+	add	x3, x2, #CPU_XREG_OFFSET(31)    // SP_EL0
+	ldp	x20, x21, [x3, #8] // Same hack again, get guest PC and pstate
+
 	ldr	x22, [x2, #CPU_GP_REG_OFFSET(CPU_SP_EL1)]
 	ldr	x23, [x2, #CPU_GP_REG_OFFSET(CPU_ELR_EL1)]
 	ldr	x24, [x2, #CPU_SPSR_OFFSET(KVM_SPSR_EL1)]
 
+	msr_hyp(ELR, x20) 		// pc on return from el2
+	msr_hyp(SPSR, x21) 		// pstate on return from el2
+
 	msr	sp_el1, x22
-	msr	elr_el1, x23
-	msr	spsr_el1, x24
 
-	add	x3, x2, #CPU_XREG_OFFSET(31)    // SP_EL0
-	ldp	x19, x20, [x3]
-	ldr	x21, [x3, #16]
+	msr_el1(elr, x23)
+	msr_el1(spsr, x24)
+.endm
 
+.macro restore_common_regs
+	// x2: base address for cpu context
+	// x3: tmp register
+
+	ldr	x19, [x2, #CPU_XREG_OFFSET(31)] // SP_EL0
 	msr	sp_el0, x19
-	msr	elr_el2, x20 		// pc on return from el2
-	msr	spsr_el2, x21 		// pstate on return from el2
 
 	add	x3, x2, #CPU_XREG_OFFSET(19)
 	ldp	x19, x20, [x3]

@@ -113,9 +125,15 @@
 
 .macro save_host_regs
 	save_common_regs
+ifnvhe	nop,					"b	skip_el1_save"
+	save_el1_state
+skip_el1_save:
 .endm
 
 .macro restore_host_regs
+ifnvhe	nop,					"b	skip_el1_restore"
+	restore_el1_state
+skip_el1_restore:
 	restore_common_regs
 .endm
 

@@ -159,6 +177,7 @@
 	stp	x6, x7, [x3, #16]
 
 	save_common_regs
+	save_el1_state
 .endm
 
 .macro restore_guest_regs

@@ -184,6 +203,7 @@
 	ldr	x18, [x3, #144]
 
 	// x19-x29, lr, sp*, elr*, spsr*
+	restore_el1_state
 	restore_common_regs
 
 	// Last bits of the 64bit state

@@ -203,6 +223,38 @@
  * In other words, don't touch any of these unless you know what
  * you are doing.
  */
+
+.macro save_shared_sysregs
+	// x2: base address for cpu context
+	// x3: tmp register
+
+	add	x3, x2, #CPU_SYSREG_OFFSET(TPIDR_EL0)
+
+	mrs	x4, tpidr_el0
+	mrs	x5, tpidrro_el0
+	mrs	x6, tpidr_el1
+	mrs	x7, actlr_el1
+
+	stp	x4, x5, [x3]
+	stp	x6, x7, [x3, #16]
+.endm
+
+.macro restore_shared_sysregs
+	// x2: base address for cpu context
+	// x3: tmp register
+
+	add	x3, x2, #CPU_SYSREG_OFFSET(TPIDR_EL0)
+
+	ldp	x4, x5, [x3]
+	ldp	x6, x7, [x3, #16]
+
+	msr	tpidr_el0,      x4
+	msr	tpidrro_el0,    x5
+	msr	tpidr_el1,      x6
+	msr	actlr_el1,      x7
+.endm
+
+
 .macro save_sysregs
 	// x2: base address for cpu context
 	// x3: tmp register

@@ -211,26 +263,27 @@
 
 	mrs	x4,	vmpidr_el2
 	mrs	x5,	csselr_el1
-	mrs	x6,	sctlr_el1
-	mrs	x7,	actlr_el1
-	mrs	x8,	cpacr_el1
-	mrs	x9,	ttbr0_el1
-	mrs	x10,	ttbr1_el1
-	mrs	x11,	tcr_el1
-	mrs	x12,	esr_el1
-	mrs	x13, 	afsr0_el1
-	mrs	x14,	afsr1_el1
-	mrs	x15,	far_el1
-	mrs	x16,	mair_el1
-	mrs	x17,	vbar_el1
-	mrs	x18,	contextidr_el1
-	mrs	x19,	tpidr_el0
-	mrs	x20,	tpidrro_el0
-	mrs	x21,	tpidr_el1
-	mrs	x22, 	amair_el1
-	mrs	x23, 	cntkctl_el1
-	mrs	x24,	par_el1
-	mrs	x25,	mdscr_el1
+	mrs_el1(x6,	sctlr)
+	mrs_el1(x7, 	amair)
+	mrs_el1(x8,	cpacr)
+	mrs_el1(x9,	ttbr0)
+	mrs_el1(x10,	ttbr1)
+	mrs_el1(x11,	tcr)
+	mrs_el1(x12,	esr)
+	mrs_el1(x13, 	afsr0)
+	mrs_el1(x14,	afsr1)
+	mrs_el1(x15,	far)
+	mrs_el1(x16,	mair)
+	mrs_el1(x17,	vbar)
+	mrs_el1(x18,	contextidr)
+	mrs_el1(x19, 	cntkctl)
+	mrs	x20,	par_el1
+	mrs	x21,	mdscr_el1
+
+	mrs	x22,	tpidr_el0
+	mrs	x23,	tpidrro_el0
+	mrs	x24,	tpidr_el1
+	mrs	x25,	actlr_el1
 
 	stp	x4, x5, [x3]
 	stp	x6, x7, [x3, #16]

@@ -460,26 +513,27 @@
 
 	msr	vmpidr_el2,	x4
 	msr	csselr_el1,	x5
-	msr	sctlr_el1,	x6
-	msr	actlr_el1,	x7
-	msr	cpacr_el1,	x8
-	msr	ttbr0_el1,	x9
-	msr	ttbr1_el1,	x10
-	msr	tcr_el1,	x11
-	msr	esr_el1,	x12
-	msr	afsr0_el1,	x13
-	msr	afsr1_el1,	x14
-	msr	far_el1,	x15
-	msr	mair_el1,	x16
-	msr	vbar_el1,	x17
-	msr	contextidr_el1,	x18
-	msr	tpidr_el0,	x19
-	msr	tpidrro_el0,	x20
-	msr	tpidr_el1,	x21
-	msr	amair_el1,	x22
-	msr	cntkctl_el1,	x23
-	msr	par_el1,	x24
-	msr	mdscr_el1,	x25
+	msr_el1(sctlr,		x6)
+	msr_el1(amair,		x7)
+	msr_el1(cpacr,		x8)
+	msr_el1(ttbr0,		x9)
+	msr_el1(ttbr1,		x10)
+	msr_el1(tcr,		x11)
+	msr_el1(esr,		x12)
+	msr_el1(afsr0,		x13)
+	msr_el1(afsr1,		x14)
+	msr_el1(far,		x15)
+	msr_el1(mair,		x16)
+	msr_el1(vbar,		x17)
+	msr_el1(contextidr,	x18)
+	msr_el1(cntkctl,	x19)
+	msr	par_el1,	x20
+	msr	mdscr_el1,	x21
+
+	msr	tpidr_el0,	x22
+	msr	tpidrro_el0,	x23
+	msr	tpidr_el1,	x24
+	msr	actlr_el1,	x25
 .endm
 
 .macro restore_debug

@@ -779,8 +833,11 @@
 .macro activate_traps
 	ldr     x2, [x0, #VCPU_HCR_EL2]
 	msr     hcr_el2, x2
-	mov	x2, #CPTR_EL2_TTA
-	msr	cptr_el2, x2
+	adr	x3, __kvm_hyp_vector
+ifnvhe	nop,					"msr	vbar_el1, x3"
+ifnvhe	nop,					"mrs	x2, cpacr_el1"
+ifnvhe _S_(ldr	x2, =(CPTR_EL2_TTA)),		"orr x2, x2, #(1 << 28)"
+ifnvhe "msr	cptr_el2, x2",			"msr	cpacr_el1, x2"
 
 	mov	x2, #(1 << 15)	// Trap CP15 Cr=15
 	msr	hstr_el2, x2

@@ -803,12 +860,20 @@
 ifnvhe _S_(mov	x2, #HCR_RW),			_S_(mov	x2, #HCR_RW|HCR_TGE)
 ifnvhe 	nop,					_S_(orr	x2, x2, #HCR_E2H)
 	msr	hcr_el2, x2
-	msr	cptr_el2, xzr
+
+ifnvhe	nop,					"mrs	x2, cpacr_el1"
+ifnvhe	nop,					"movn	x3, #(1 << 12), lsl #16"
+ifnvhe	nop,					"and	x2, x2, x3"
+ifnvhe "msr	cptr_el2, xzr",			"msr	cpacr_el1, x2"
 	msr	hstr_el2, xzr
 
 	mrs	x2, mdcr_el2
 	and	x2, x2, #MDCR_EL2_HPMN_MASK
 	msr	mdcr_el2, x2
+
+	adrp	x2, vectors
+	add	x2, x2, #:lo12:vectors
+ifnvhe	nop,					"msr	vbar_el1, x2"
 .endm
 
 .macro activate_vm

@@ -853,15 +918,15 @@ ifnvhe 	nop,					_S_(orr	x2, x2, #HCR_E2H)
 	ldr	w3, [x2, #KVM_TIMER_ENABLED]
 	cbz	w3, 1f
 
-	mrs	x3, cntv_ctl_el0
+	mrs_el0(x3, cntv_ctl)
 	and	x3, x3, #3
 	str	w3, [x0, #VCPU_TIMER_CNTV_CTL]
 	bic	x3, x3, #1		// Clear Enable
-	msr	cntv_ctl_el0, x3
+	msr_el0(cntv_ctl, x3)
 
 	isb
 
-	mrs	x3, cntv_cval_el0
+	mrs_el0(x3, cntv_cval)
 	str	x3, [x0, #VCPU_TIMER_CNTV_CVAL]
 
 1:

@@ -871,7 +936,7 @@ ifnvhe 	nop,					_S_(orr	x2, x2, #HCR_E2H)
 	msr	cnthctl_el2, x2
 
 	// Clear cntvoff for the host
-	msr	cntvoff_el2, xzr
+ifnvhe "msr	cntvoff_el2, xzr",		nop
 .endm
 
 .macro restore_timer_state

@@ -891,12 +956,12 @@ ifnvhe 	nop,					_S_(orr	x2, x2, #HCR_E2H)
 	ldr	x3, [x2, #KVM_TIMER_CNTVOFF]
 	msr	cntvoff_el2, x3
 	ldr	x2, [x0, #VCPU_TIMER_CNTV_CVAL]
-	msr	cntv_cval_el0, x2
+	msr_el0(cntv_cval, x2)
 	isb
 
 	ldr	w2, [x0, #VCPU_TIMER_CNTV_CTL]
 	and	x2, x2, #3
-	msr	cntv_ctl_el0, x2
+	msr_el0(cntv_ctl, x2)
 1:
 .endm
 

@@ -945,8 +1010,10 @@ ENTRY(__kvm_vcpu_run)
 
 	save_host_regs
 	bl __save_fpsimd
-	bl __save_sysregs
-
+ifnvhe "bl	__save_sysregs",		nop
+ifnvhe "b	1f",				nop
+	save_shared_sysregs
+1:
 	compute_debug_state 1f
 	bl	__save_debug
 1:

@@ -997,7 +1064,10 @@ __kvm_vcpu_return:
 	ldr	x2, [x0, #VCPU_HOST_CONTEXT]
 	kern_hyp_va x2
 
-	bl __restore_sysregs
+ifnvhe "bl	__restore_sysregs",		nop
+ifnvhe "b	1f",				nop
+	restore_shared_sysregs
+1:
 	bl __restore_fpsimd
 
 	skip_debug_state x3, 1f

@@ -1104,6 +1174,8 @@ __kvm_hyp_panic:
 	mrs	x6, par_el1
 	mrs	x7, tpidr_el2
 
+ifnvhe	nop,					"b	panic"
+
 	mov	lr, #(PSR_F_BIT | PSR_I_BIT | PSR_A_BIT | PSR_D_BIT |\
 		      PSR_MODE_EL1h)
 	msr	spsr_el2, lr

@@ -1248,7 +1320,7 @@ el1_trap:
 	 * As such, we can use the EL1 translation regime, and don't have
 	 * to distinguish between EL0 and EL1 access.
 	 */
-	mrs	x2, far_el2
+ifnvhe "mrs	x2, far_el2",			"mrs	x2, far_el1"
 	at	s1e1r, x2
 	isb
 

@@ -1262,7 +1334,7 @@ el1_trap:
 	b	2f
 
 1:	mrs	x3, hpfar_el2
-	mrs	x2, far_el2
+ifnvhe "mrs	x2, far_el2",			"mrs	x2, far_el1"
 
 2:	mrs	x0, tpidr_el2
 	str	w1, [x0, #VCPU_ESR_EL2]

@@ -31,6 +31,24 @@
 	alternative_insn	"\nonvhe", "\vhe", ARM64_HAS_VIRT_HOST_EXTN
 .endm
 
+#define mrs_el0(reg, sysreg)	\
+	ifnvhe  _S_(mrs reg, sysreg##_EL0), _S_(mrs_s reg, sysreg##_EL02)
+
+#define msr_el0(sysreg, reg)	\
+	ifnvhe  _S_(msr sysreg##_EL0, reg), _S_(msr_s sysreg##_EL02, reg)
+
+#define mrs_el1(reg, sysreg)	\
+	ifnvhe  _S_(mrs reg, sysreg##_EL1), _S_(mrs_s reg, sysreg##_EL12)
+
+#define msr_el1(sysreg, reg)	\
+	ifnvhe  _S_(msr sysreg##_EL1, reg), _S_(msr_s sysreg##_EL12, reg)
+
+#define mrs_hyp(reg, sysreg)	\
+	ifnvhe  _S_(mrs reg, sysreg##_EL2), _S_(mrs reg, sysreg##_EL1)
+
+#define msr_hyp(sysreg, reg)	\
+	ifnvhe  _S_(msr sysreg##_EL2, reg), _S_(msr sysreg##_EL1, reg)
+
 #endif
 
 #endif	/*__ARM64_VHE_MACROS_H__  */