1 files changed, 81 insertions, 0 deletions
diff --git a/roms/u-boot/arch/arm/cpu/armv8/fel_utils.S b/roms/u-boot/arch/arm/cpu/armv8/fel_utils.S
new file mode 100644
index 000000000..7def44ad1
--- /dev/null
+++ b/roms/u-boot/arch/arm/cpu/armv8/fel_utils.S
@@ -0,0 +1,81 @@
+/*
+ * Utility functions for FEL mode, when running SPL in AArch64.
+ *
+ * Copyright (c) 2017 Arm Ltd.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <asm-offsets.h>
+#include <config.h>
+#include <asm/system.h>
+#include <linux/linkage.h>
+
+/*
+ * We don't overwrite save_boot_params() here, to save the FEL state upon
+ * entry, since this would run *after* the RMR reset, which clobbers that
+ * state.
+ * Instead we store the state _very_ early in the boot0 hook, *before*
+ * resetting to AArch64.
+ */
+
+/*
+ * The FEL routines in BROM run in AArch32.
+ * Reset back into 32-bit mode here and restore the saved FEL state
+ * afterwards.
+ * Resetting back into AArch32/EL3 using the RMR always enters the BROM,
+ * but we can use the CPU hotplug mechanism to branch back to our code
+ * immediately.
+ */
+ENTRY(return_to_fel)
+	/*
+	 * the RMR reset will clear all registers, so save the arguments
+	 * (LR and SP) in the fel_stash structure, which we read anyways later
+	 */
+	adr	x2, fel_stash
+	str	w0, [x2]
+	str	w1, [x2, #4]
+
+	adr	x1, fel_stash_addr	// to find the fel_stash address in AA32
+	str	w2, [x1]
+
+	ldr	x0, =0xfa50392f		// CPU hotplug magic
+#ifdef CONFIG_MACH_SUN50I_H616
+	ldr	x2, =(SUNXI_R_CPUCFG_BASE + 0x1c0)
+	str	w0, [x2], #0x4
+#elif CONFIG_MACH_SUN50I_H6
+	ldr	x2, =(SUNXI_RTC_BASE + 0x1b8)	// BOOT_CPU_HP_FLAG_REG
+	str	w0, [x2], #0x4
+#else
+	ldr	x2, =(SUNXI_CPUCFG_BASE + 0x1a4) // offset for CPU hotplug base
+	str	w0, [x2, #0x8]
+#endif
+	adr	x0, back_in_32
+	str	w0, [x2]
+
+	dsb	sy
+	isb	sy
+	mov	x0, #2			// RMR reset into AArch32
+	dsb	sy
+	msr	RMR_EL3, x0
+	isb	sy
+1:	wfi
+	b	1b
+
+/* AArch32 code to restore the state from fel_stash and return back to FEL. */
+back_in_32:
+	.word	0xe59f0028 	// ldr	r0, [pc, #40]	; load fel_stash address
+	.word	0xe5901008 	// ldr	r1, [r0, #8]
+	.word	0xe129f001 	// msr	CPSR_fc, r1
+	.word	0xf57ff06f	// isb
+	.word	0xe590d000 	// ldr	sp, [r0]
+	.word	0xe590e004 	// ldr	lr, [r0, #4]
+	.word	0xe5901010 	// ldr	r1, [r0, #16]
+	.word	0xee0c1f10 	// mcr	15, 0, r1, cr12, cr0, {0} ; VBAR
+	.word	0xe590100c 	// ldr	r1, [r0, #12]
+	.word	0xee011f10 	// mcr	15, 0, r1, cr1, cr0, {0}  ; SCTLR
+	.word	0xf57ff06f	// isb
+	.word	0xe12fff1e 	// bx	lr		; return to FEL
+fel_stash_addr:
+	.word   0x00000000	// receives fel_stash addr, by AA64 code above
+ENDPROC(return_to_fel)