[rtems commit] bsps/aarch64: Add support for EL2 start

[Joel Sherrill]

Module:    rtems
Branch:    master
Commit:    a27ba3f20f8909cf6081bf8ab3114305e27c8a59
Changeset: http://git.rtems.org/rtems/commit/?id=a27ba3f20f8909cf6081bf8ab3114305e27c8a59

Author:    Kinsey Moore <kinsey.moore at oarcorp.com>
Date:      Fri Jan  8 10:13:42 2021 -0600

bsps/aarch64: Add support for EL2 start

Add the stub necessary to boot on AArch64 under EL2 and drop to EL1 for
normal operation.

---

 bsps/aarch64/shared/start/start.S | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/bsps/aarch64/shared/start/start.S b/bsps/aarch64/shared/start/start.S
index e4dd3c2..3751545 100644
--- a/bsps/aarch64/shared/start/start.S
+++ b/bsps/aarch64/shared/start/start.S
@@ -98,6 +98,31 @@ _start:
 #endif
 	msr SCTLR_EL1, x0
 
+#ifdef BSP_START_IN_HYP_SUPPORT
+	/* Drop from EL2 to EL1 */
+
+	/* Configure HCR_EL2 */
+	mrs x0, HCR_EL2
+	/* Set EL1 Execution state to AArch64 */
+	orr x0, x0, #(1<<31)
+	/* Disable ID traps */
+	bic x0, x0, #(1<<15)
+	bic x0, x0, #(1<<16)
+	bic x0, x0, #(1<<17)
+	bic x0, x0, #(1<<18)
+	msr HCR_EL2, x0
+
+	/* Set to EL1h mode for eret */
+	mov x0, #0b00101
+	msr SPSR_EL2, x0
+
+	/* Set EL1 entry point */
+	adr x0, _el1_start
+	msr ELR_EL2, x0
+	eret
+_el1_start:
+#endif
+
 #ifdef RTEMS_SMP
 	/* Read MPIDR and get current processor index */
         mrs	x7, mpidr_el1