From a3ef09d41eb79bb4f6b1ce66cf2f947e21d84da5 Mon Sep 17 00:00:00 2001
From: David Karoly <karolyd577@gmail.com>
Date: Mon, 28 Mar 2022 20:35:25 +0200
Subject: [PATCH] boot/efi/arm: implement PL2 to PL1 transition

Change-Id: Ib30a943f68430ecbe33d8c7fe88c67267f9deff9
Reviewed-on: https://review.haiku-os.org/c/haiku/+/5150
Reviewed-by: Adrien Destugues <pulkomandy@gmail.com>
Tested-by: Commit checker robot <no-reply+buildbot@haiku-os.org>
---
 src/system/boot/platform/efi/arch/arm/entry.S | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/src/system/boot/platform/efi/arch/arm/entry.S b/src/system/boot/platform/efi/arch/arm/entry.S
index b457667daa..e4b9274c29 100644
--- a/src/system/boot/platform/efi/arch/arm/entry.S
+++ b/src/system/boot/platform/efi/arch/arm/entry.S
@@ -6,6 +6,7 @@
 #include <asm_defs.h>
 
 
+	.arch_extension virt
 	.text
 
 /*	status_t arch_enter_kernel(uint32_t ttbr, struct kernel_args *kernelArgs,
@@ -18,6 +19,28 @@
 */
 FUNCTION(arch_enter_kernel):
 
+	// check whether we are running in HYP mode
+	mrs		r9, cpsr
+	and		r9, r9, #0x1f
+
+	// proceed to _pl1_entry if we're not in HYP mode
+	cmp		r9, #0x1a
+	bne		_pl1_entry
+
+	// set SVC mode in SPSR
+	mrs		r9, cpsr
+	bic		r9, r9, #0x1f
+	orr		r9, r9, #0x13
+	msr		spsr_cxsf, r9
+
+	// load PL1 entry point address
+	adr		lr, _pl1_entry
+	msr		elr_hyp, lr
+
+	// drop to PL1 SVC mode
+	eret
+
+_pl1_entry:
 
 	mov		r5,r0
 	mov		r4,r2