[PATCH v2 2/6] arm64: initialise SCTLR2_ELx register at boot time

[Yeoreum Yun]

add initialisation for SCTRL2_ELx register at boot time.

Signed-off-by: Yeoreum Yun <yeoreum.yun@xxxxxxx>
---
 arch/arm64/include/asm/el2_setup.h |  6 ++++++
 arch/arm64/include/asm/sysreg.h    | 22 ++++++++++++++++++++++
 arch/arm64/kernel/head.S           |  5 ++++-
 3 files changed, 32 insertions(+), 1 deletion(-)

diff --git a/arch/arm64/include/asm/el2_setup.h b/arch/arm64/include/asm/el2_setup.h
index d755b4d46d77..347ac4cc1283 100644
--- a/arch/arm64/include/asm/el2_setup.h
+++ b/arch/arm64/include/asm/el2_setup.h
@@ -48,6 +48,11 @@
 	isb
 .endm
 
+.macro __init_el2_sctlr2
+	init_sctlr2_elx	2, x0
+	isb
+.endm
+
 .macro __init_el2_hcrx
 	mrs	x0, id_aa64mmfr1_el1
 	ubfx	x0, x0, #ID_AA64MMFR1_EL1_HCX_SHIFT, #4
@@ -411,6 +416,7 @@
  */
 .macro init_el2_state
 	__init_el2_sctlr
+	__init_el2_sctlr2
 	__init_el2_hcrx
 	__init_el2_timers
 	__init_el2_debug
diff --git a/arch/arm64/include/asm/sysreg.h b/arch/arm64/include/asm/sysreg.h
index d5b5f2ae1afa..8b82af5be199 100644
--- a/arch/arm64/include/asm/sysreg.h
+++ b/arch/arm64/include/asm/sysreg.h
@@ -868,6 +868,8 @@
 #define INIT_SCTLR_EL2_MMU_OFF \
 	(SCTLR_EL2_RES1 | ENDIAN_SET_EL2)
 
+#define INIT_SCTLR2_EL2			UL(0)
+
 /* SCTLR_EL1 specific flags. */
 #ifdef CONFIG_CPU_BIG_ENDIAN
 #define ENDIAN_SET_EL1		(SCTLR_EL1_E0E | SCTLR_ELx_EE)
@@ -888,6 +890,8 @@
 	 SCTLR_EL1_LSMAOE | SCTLR_EL1_nTLSMD | SCTLR_EL1_EIS   | \
 	 SCTLR_EL1_TSCXT  | SCTLR_EL1_EOS)
 
+#define INIT_SCTLR2_EL1			UL(0)
+
 /* MAIR_ELx memory attributes (used by Linux) */
 #define MAIR_ATTR_DEVICE_nGnRnE		UL(0x00)
 #define MAIR_ATTR_DEVICE_nGnRE		UL(0x04)
@@ -1164,6 +1168,24 @@
 	msr	hcr_el2, \reg
 #endif
 	.endm
+
+	.macro init_sctlr2_elx, el, tmp
+	mrs_s	\tmp, SYS_ID_AA64MMFR3_EL1
+	ubfx	\tmp, \tmp, #ID_AA64MMFR3_EL1_SCTLRX_SHIFT, #4
+	cbz	\tmp, .Lskip_sctlr2_\@
+	.if	\el == 2
+	mov_q	\tmp, INIT_SCTLR2_EL2
+	msr_s	SYS_SCTLR_EL2, \tmp
+	.else
+	mov_q	\tmp, INIT_SCTLR2_EL1
+	.if	\el == 12
+	msr_s	SYS_SCTLR_EL12, \tmp
+	.else
+	msr_s	SYS_SCTLR_EL1, \tmp
+	.endif
+	.endif
+.Lskip_sctlr2_\@:
+	.endm
 #else
 
 #include <linux/bitfield.h>
diff --git a/arch/arm64/kernel/head.S b/arch/arm64/kernel/head.S
index ca04b338cb0d..0dff7593e50b 100644
--- a/arch/arm64/kernel/head.S
+++ b/arch/arm64/kernel/head.S
@@ -276,6 +276,7 @@ SYM_INNER_LABEL(init_el1, SYM_L_LOCAL)
 	mov_q	x0, INIT_SCTLR_EL1_MMU_OFF
 	pre_disable_mmu_workaround
 	msr	sctlr_el1, x0
+	init_sctlr2_elx	1, x0
 	isb
 	mov_q	x0, INIT_PSTATE_EL1
 	msr	spsr_el1, x0
@@ -298,7 +299,6 @@ SYM_INNER_LABEL(init_el2, SYM_L_LOCAL)
 	msr	sctlr_el2, x0
 	isb
 0:
-
 	init_el2_hcr	HCR_HOST_NVHE_FLAGS
 	init_el2_state
 
@@ -315,12 +315,15 @@ SYM_INNER_LABEL(init_el2, SYM_L_LOCAL)
 
 	/* Set a sane SCTLR_EL1, the VHE way */
 	msr_s	SYS_SCTLR_EL12, x1
+	init_sctlr2_elx	12, x2
 	mov	x2, #BOOT_CPU_FLAG_E2H
 	b	3f
 
 2:
 	msr	sctlr_el1, x1
+	init_sctlr2_elx	1, x2
 	mov	x2, xzr
+
 3:
 	mov	x0, #INIT_PSTATE_EL1
 	msr	spsr_el2, x0
-- 
LEVI:{C3F47F37-75D8-414A-A8BA-3980EC8A46D7}