3 files changed, 130 insertions, 1 deletions

diff --git a/arch/arm/include/asm/arch-fsl-lsch3/config.h b/arch/arm/include/asm/arch-fsl-lsch3/config.h
index f632c82..da551e8 100644
--- a/arch/arm/include/asm/arch-fsl-lsch3/config.h
+++ b/arch/arm/include/asm/arch-fsl-lsch3/config.h
@@ -8,7 +8,7 @@
 #define _ASM_ARMV8_FSL_LSCH3_CONFIG_
 
 #include <fsl_ddrc_version.h>
-
+#define CONFIG_MP
 #define CONFIG_SYS_FSL_OCRAM_BASE	0x18000000	/* initial RAM */
 /* Link Definitions */
 #define CONFIG_SYS_INIT_SP_ADDR		(CONFIG_SYS_FSL_OCRAM_BASE + 0xfff0)
@@ -19,6 +19,7 @@
 #define CONFIG_SYS_FSL_DDR3_ADDR		0x08210000
 #define CONFIG_SYS_FSL_GUTS_ADDR		(CONFIG_SYS_IMMR + 0x00E00000)
 #define CONFIG_SYS_FSL_PMU_ADDR			(CONFIG_SYS_IMMR + 0x00E30000)
+#define CONFIG_SYS_FSL_RST_ADDR			(CONFIG_SYS_IMMR + 0x00E60000)
 #define CONFIG_SYS_FSL_CH3_CLK_GRPA_ADDR	(CONFIG_SYS_IMMR + 0x00300000)
 #define CONFIG_SYS_FSL_CH3_CLK_GRPB_ADDR	(CONFIG_SYS_IMMR + 0x00310000)
 #define CONFIG_SYS_FSL_CH3_CLK_CTRL_ADDR	(CONFIG_SYS_IMMR + 0x00370000)
diff --git a/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h b/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
index 18e66bd..ee1d651 100644
--- a/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
+++ b/arch/arm/include/asm/arch-fsl-lsch3/immap_lsch3.h
@@ -113,4 +113,39 @@ struct ccsr_clk_ctrl {
 		u8  res_04[0x20-0x04];
 	} clkcncsr[8];
 };
+
+struct ccsr_reset {
+	u32 rstcr;			/* 0x000 */
+	u32 rstcrsp;			/* 0x004 */
+	u8 res_008[0x10-0x08];		/* 0x008 */
+	u32 rstrqmr1;			/* 0x010 */
+	u32 rstrqmr2;			/* 0x014 */
+	u32 rstrqsr1;			/* 0x018 */
+	u32 rstrqsr2;			/* 0x01c */
+	u32 rstrqwdtmrl;		/* 0x020 */
+	u32 rstrqwdtmru;		/* 0x024 */
+	u8 res_028[0x30-0x28];		/* 0x028 */
+	u32 rstrqwdtsrl;		/* 0x030 */
+	u32 rstrqwdtsru;		/* 0x034 */
+	u8 res_038[0x60-0x38];		/* 0x038 */
+	u32 brrl;			/* 0x060 */
+	u32 brru;			/* 0x064 */
+	u8 res_068[0x80-0x68];		/* 0x068 */
+	u32 pirset;			/* 0x080 */
+	u32 pirclr;			/* 0x084 */
+	u8 res_088[0x90-0x88];		/* 0x088 */
+	u32 brcorenbr;			/* 0x090 */
+	u8 res_094[0x100-0x94];		/* 0x094 */
+	u32 rcw_reqr;			/* 0x100 */
+	u32 rcw_completion;		/* 0x104 */
+	u8 res_108[0x110-0x108];	/* 0x108 */
+	u32 pbi_reqr;			/* 0x110 */
+	u32 pbi_completion;		/* 0x114 */
+	u8 res_118[0xa00-0x118];	/* 0x118 */
+	u32 qmbm_warmrst;		/* 0xa00 */
+	u32 soc_warmrst;		/* 0xa04 */
+	u8 res_a08[0xbf8-0xa08];	/* 0xa08 */
+	u32 ip_rev1;			/* 0xbf8 */
+	u32 ip_rev2;			/* 0xbfc */
+};
 #endif /* __ARCH_FSL_LSCH3_IMMAP_H */
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__ */