Merge branch 'master' of git://git.denx.de/u-boot-fsl-qoriq

1 files changed, 93 insertions, 0 deletions

diff --git a/arch/arm/include/asm/macro.h b/arch/arm/include/asm/macro.h
index f77e4b8..541b443 100644
--- a/arch/arm/include/asm/macro.h
+++ b/arch/arm/include/asm/macro.h
@@ -105,6 +105,99 @@ lr	.req	x30
 	cbz	\xreg1, \master_label
 .endm
 
+.macro armv8_switch_to_el2_m, xreg1
+	/* 64bit EL2 | HCE | SMD | RES1 (Bits[5:4]) | Non-secure EL0/EL1 */
+	mov	\xreg1, #0x5b1
+	msr	scr_el3, \xreg1
+	msr	cptr_el3, xzr		/* Disable coprocessor traps to EL3 */
+	mov	\xreg1, #0x33ff
+	msr	cptr_el2, \xreg1	/* Disable coprocessor traps to EL2 */
+
+	/* Initialize SCTLR_EL2
+	 *
+	 * setting RES1 bits (29,28,23,22,18,16,11,5,4) to 1
+	 * and RES0 bits (31,30,27,26,24,21,20,17,15-13,10-6) +
+	 * EE,WXN,I,SA,C,A,M to 0
+	 */
+	mov	\xreg1, #0x0830
+	movk	\xreg1, #0x30C5, lsl #16
+	msr	sctlr_el2, \xreg1
+
+	/* Return to the EL2_SP2 mode from EL3 */
+	mov	\xreg1, sp
+	msr	sp_el2, \xreg1		/* Migrate SP */
+	mrs	\xreg1, vbar_el3
+	msr	vbar_el2, \xreg1	/* Migrate VBAR */
+	mov	\xreg1, #0x3c9
+	msr	spsr_el3, \xreg1	/* EL2_SP2 | D | A | I | F */
+	msr	elr_el3, lr
+	eret
+.endm
+
+.macro armv8_switch_to_el1_m, xreg1, xreg2
+	/* Initialize Generic Timers */
+	mrs	\xreg1, cnthctl_el2
+	orr	\xreg1, \xreg1, #0x3	/* Enable EL1 access to timers */
+	msr	cnthctl_el2, \xreg1
+	msr	cntvoff_el2, xzr
+
+	/* Initilize MPID/MPIDR registers */
+	mrs	\xreg1, midr_el1
+	mrs	\xreg2, mpidr_el1
+	msr	vpidr_el2, \xreg1
+	msr	vmpidr_el2, \xreg2
+
+	/* Disable coprocessor traps */
+	mov	\xreg1, #0x33ff
+	msr	cptr_el2, \xreg1	/* Disable coprocessor traps to EL2 */
+	msr	hstr_el2, xzr		/* Disable coprocessor traps to EL2 */
+	mov	\xreg1, #3 << 20
+	msr	cpacr_el1, \xreg1	/* Enable FP/SIMD at EL1 */
+
+	/* Initialize HCR_EL2 */
+	mov	\xreg1, #(1 << 31)		/* 64bit EL1 */
+	orr	\xreg1, \xreg1, #(1 << 29)	/* Disable HVC */
+	msr	hcr_el2, \xreg1
+
+	/* SCTLR_EL1 initialization
+	 *
+	 * setting RES1 bits (29,28,23,22,20,11) to 1
+	 * and RES0 bits (31,30,27,21,17,13,10,6) +
+	 * UCI,EE,EOE,WXN,nTWE,nTWI,UCT,DZE,I,UMA,SED,ITD,
+	 * CP15BEN,SA0,SA,C,A,M to 0
+	 */
+	mov	\xreg1, #0x0800
+	movk	\xreg1, #0x30d0, lsl #16
+	msr	sctlr_el1, \xreg1
+
+	/* Return to the EL1_SP1 mode from EL2 */
+	mov	\xreg1, sp
+	msr	sp_el1, \xreg1		/* Migrate SP */
+	mrs	\xreg1, vbar_el2
+	msr	vbar_el1, \xreg1	/* Migrate VBAR */
+	mov	\xreg1, #0x3c5
+	msr	spsr_el2, \xreg1	/* EL1_SP1 | D | A | I | F */
+	msr	elr_el2, lr
+	eret
+.endm
+
+#if defined(CONFIG_GICV3)
+.macro gic_wait_for_interrupt_m xreg1
+0 :	wfi
+	mrs     \xreg1, ICC_IAR1_EL1
+	msr     ICC_EOIR1_EL1, \xreg1
+	cbnz    \xreg1, 0b
+.endm
+#elif defined(CONFIG_GICV2)
+.macro gic_wait_for_interrupt_m xreg1, wreg2
+0 :	wfi
+	ldr     \wreg2, [\xreg1, GICC_AIAR]
+	str     \wreg2, [\xreg1, GICC_AEOIR]
+	and	\wreg2, \wreg2, #3ff
+	cbnz    \wreg2, 0b
+.endm
+#endif
+
 #endif /* CONFIG_ARM64 */
 
 #endif /* __ASSEMBLY__ */