arm64: Drop to EL1 when we start off in EL2

Signed-off-by: Nikos Nikoleris <nikos.nikoleris@arm.com>
relokin · Mar 8, 2021 · 1468abe · 1468abe
1 parent d8228dc
commit 1468abe
Show file tree

Hide file tree

Showing 4 changed files with 87 additions and 7 deletions.
diff --git a/arm/cstart64.S b/arm/cstart64.S
@@ -7,6 +7,7 @@
  */
 #define __ASSEMBLY__
 #include <auxinfo.h>
+#include <asm/arch_gicv3.h>
 #include <asm/asm-offsets.h>
 #include <asm/ptrace.h>
 #include <asm/processor.h>
@@ -109,6 +110,64 @@ run_main:
 	dsb	\domain
 .endm
 
+drop_to_el1:
+	ldr	x4, =SCTLR_EL2_RES1
+	msr	sctlr_el2, x4
+	isb
+
+	ldr	x4, =HCR_HOST_NVHE_FLAGS
+	msr	hcr_el2, x4
+	isb
+
+	ldr	x4, =SCTLR_EL1_RES1
+	msr	sctlr_el1, x4
+
+	mrs	x4, cnthctl_el2
+	/* Enable EL1 physical timers. */
+	orr	x4, x4, #3
+	msr	cnthctl_el2, x4
+	/* Clear the timer virtual offset. */
+	msr	cntvoff_el2, xzr
+
+	mrs	x4,  id_aa64dfr0_el1
+	sbfx	x5, x4, #8, #4
+	cmp	x5, #1
+	b.lt	1f
+	mrs	x4, pmcr_el0
+	ubfx	x4, x4, #11, #5
+	/* Disable traps, allow access to all PMU counters */
+	msr	mdcr_el2, x4
+
+1:
+	mrs	x4, id_aa64pfr0_el1
+	ubfx	x4, x4, #24, #4
+	cbz	x4, 2f
+
+	/* Allow access to the GICv3 CPU interface */
+	mrs_s	x4, ICC_SRE_EL2
+	orr	x4, x4, #(1 << 0)
+	orr	x4, x4, #(1 << 3)
+	msr_s	ICC_SRE_EL2, x4
+	isb
+
+2:
+	mrs	x4, midr_el1
+	msr	vpidr_el2, x4
+	mrs	x4, mpidr_el1
+	msr	vmpidr_el2, x4
+
+	ldr	x4, =CPTR_EL2_RES1
+	msr	cptr_el2, x4
+
+	ldr	x4, =INIT_PSTATE_EL1
+	msr	spsr_el2, x4
+
+	msr	vttbr_el2, xzr
+	msr	vbar_el2, xzr
+	msr	elr_el2, lr
+
+	eret
+
 /*
  * Used by cpu0 to setup the system and start the actual test.
  * x0 is the address of the dtb
@@ -134,13 +193,8 @@ primary_entry:
 	mrs	x0, CurrentEL
 	cmp	x0, #CurrentEL_EL2
 	b.ne	1f
-	mrs	x0, sctlr_el2
-	bic	x0, x0, SCTLR_EL1_M
-	bic	x0, x0, SCTLR_EL1_C
-	isb
-	msr	sctlr_el2, x0
-	isb
-	b	2f
+	bl drop_to_el1
+
 1:
 	mrs	x0, sctlr_el1
 	bic	x0, x0, SCTLR_EL1_M
@@ -155,6 +209,10 @@ primary_entry:
 	isb
 	mov	sp, x12
 
+	/* enable FP/ASIMD */
+	mov	x4, #(3 << 20)
+	msr	cpacr_el1, x4
+
 	mov	x0, x10
 	mov	x1, x11
 	bl	setup
@@ -163,6 +221,11 @@ primary_entry:
 
 .globl secondary_entry
 secondary_entry:
+	mrs	x0, CurrentEL
+	cmp	x0, #CurrentEL_EL2
+	b.ne	0f
+	bl	drop_to_el1
+0:
 	/* Enable FP/ASIMD */
 	mov	x0, #(3 << 20)
 	msr	cpacr_el1, x0

diff --git a/lib/arm64/asm/arch_gicv3.h b/lib/arm64/asm/arch_gicv3.h
@@ -16,6 +16,8 @@
 #define ICC_EOIR1_EL1			sys_reg(3, 0, 12, 12, 1)
 #define ICC_GRPEN1_EL1			sys_reg(3, 0, 12, 12, 7)
 
+#define ICC_SRE_EL2			sys_reg(3, 4, 12, 9, 5)
+
 #ifndef __ASSEMBLY__
 
 #include <libcflat.h>

diff --git a/lib/arm64/asm/ptrace.h b/lib/arm64/asm/ptrace.h
@@ -48,6 +48,8 @@
 #define PSR_x		0x0000ff00	/* Extension            */
 #define PSR_c		0x000000ff	/* Control              */
 
+#define INIT_PSTATE_EL1	(PSR_D_BIT | PSR_A_BIT | PSR_I_BIT | PSR_F_BIT | PSR_MODE_EL1h)
+
 #ifndef __ASSEMBLY__
 #include <libcflat.h>
 

diff --git a/lib/arm64/asm/sysreg.h b/lib/arm64/asm/sysreg.h
@@ -70,4 +70,17 @@ asm(
 "	.endm\n"
 );
 #endif /* __ASSEMBLY__ */
+
+#define SCTLR_EL1_RES1	(_BITUL(7) | _BITUL(8) | _BITUL(11) | _BITUL(20) | _BITUL(22) | \
+			 _BITUL(23) | _BITUL(28) | _BITUL(29))
+
+#define SCTLR_EL2_RES1	(_BITUL(4) | _BITUL(5) | _BITUL(11) | _BITUL(16) | \
+			 _BITUL(18) | _BITUL(22) | _BITUL(23) | _BITUL(28) | \
+			 _BITUL(29))
+#define CPTR_EL2_RES1	(UL(0xff) | _BITUL(9) | _BITUL(12) | _BITUL(13))
+
+#define HCR_RW_SHIFT	31
+#define HCR_RW		(UL(1) << HCR_RW_SHIFT)
+#define HCR_HOST_NVHE_FLAGS	HCR_RW
+
 #endif /* _ASMARM64_SYSREG_H_ */