[PATCH v2] mach-snapdragon: Add early EL2 hypervisor switch support

[Aswin Murugan]

Add support for exiting Gunyah hypervisor and switching to EL2 during
early boot. This is required for platforms that boot U-Boot in a
hypervisor guest environment where U-Boot needs to run at EL2 to
enable other hypervisors like KVM.

The switch is performed via TrustZone SMC call before EL register
configuration in start.S. The implementation:

- Checks current exception level and only executes at EL1
- Uses TrustZone SMC to exit Gunyah and transition to EL2
- Integrates with existing boot0.h infrastructure

A new Kconfig option CONFIG_QCOM_EL2_GUNYAH_EXIT_SUPPORT enables this
functionality.

Signed-off-by: Aswin Murugan <aswin.murugan at oss.qualcomm.com>
---
Change in v2:
- EL2 switch is made at early stage instead of doing at the very last stage in
  the previous version
- This change is based on the patch [1]

[1] https://lore.kernel.org/all/20260508-qcom_spl-v7-4-7d0e22aaaa8f@seznam.cz/

Link to v1: https://lore.kernel.org/u-boot/20260419173829.1074404-1-aswin.murugan@oss.qualcomm.com/
---
 arch/arm/mach-snapdragon/Kconfig              |  7 ++++
 arch/arm/mach-snapdragon/include/mach/boot0.h |  3 ++
 .../include/mach/gunyah_exit_boot0.h          | 36 +++++++++++++++++++
 3 files changed, 46 insertions(+)
 create mode 100644 arch/arm/mach-snapdragon/include/mach/gunyah_exit_boot0.h

diff --git a/arch/arm/mach-snapdragon/Kconfig b/arch/arm/mach-snapdragon/Kconfig
index f863daf6bb9..931d25c1023 100644
--- a/arch/arm/mach-snapdragon/Kconfig
+++ b/arch/arm/mach-snapdragon/Kconfig
@@ -56,6 +56,13 @@ config BOOT0_MSM8916_PSCI_WORKAROUND
 	help
 	  Select this if you are building U-Boot proper for an msm8916 board that
 	  uses the buggy PSCI implementation.
+
+config QCOM_EL2_GUNYAH_EXIT_SUPPORT
+	bool "Enable early EL2 switch by exiting Gunyah"
+	help
+	  Exit Gunyah hypervisor and switch to EL2 during early boot. This must
+	  happen before EL register configuration in start.S so that U-Boot can
+	  run properly at EL2.
 endchoice
 
 endif
diff --git a/arch/arm/mach-snapdragon/include/mach/boot0.h b/arch/arm/mach-snapdragon/include/mach/boot0.h
index b3c76d6d97d..032a9fbdb12 100644
--- a/arch/arm/mach-snapdragon/include/mach/boot0.h
+++ b/arch/arm/mach-snapdragon/include/mach/boot0.h
@@ -1,9 +1,12 @@
 /* SPDX-License-Identifier: GPL-2.0+ */
+
 #if defined(CONFIG_SPL_BUILD)
 	b	reset
 #else
 #if defined(CONFIG_BOOT0_MSM8916_PSCI_WORKAROUND)
 #include "msm8916_boot0.h"
+#elif defined(CONFIG_QCOM_EL2_GUNYAH_EXIT_SUPPORT)
+#include "gunyah_exit_boot0.h"
 #else
 	b	reset
 #endif
diff --git a/arch/arm/mach-snapdragon/include/mach/gunyah_exit_boot0.h b/arch/arm/mach-snapdragon/include/mach/gunyah_exit_boot0.h
new file mode 100644
index 00000000000..f69678018ea
--- /dev/null
+++ b/arch/arm/mach-snapdragon/include/mach/gunyah_exit_boot0.h
@@ -0,0 +1,36 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Early Gunyah Hypervisor Exit
+ *
+ * Exit Gunyah hypervisor and switch to EL2 during early boot. This must
+ * happen before EL register configuration in start.S so that U-Boot can
+ * run properly at EL2.
+ */
+
+#include <linux/arm-smccc.h>
+
+/* TrustZone SMC IDs for hypervisor configuration */
+#define TZ_EL2_SWITCH_SMC_ID			0x02000121
+#define TZ_EL2_SWITCH_PARAM_ID			0x00000023
+#define TZ_EL2_SWITCH_PARAM2_EXIT_GUNYAH	0x1
+
+	/* Save FDT address before we modify x0 */
+	mov	x9, x0
+
+	/* Only perform hypervisor switch if we're at EL1 */
+	mrs	x0, CurrentEL
+	cmp	x0, #(1 << 2)
+	bne	1f
+
+	/* Switch to EL2 (exit Gunyah) */
+	ldr	w0, =TZ_EL2_SWITCH_SMC_ID
+	ldr	w1, =TZ_EL2_SWITCH_PARAM_ID
+	mov	w2, wzr
+	mov	w3, wzr
+	ldr	w4, =TZ_EL2_SWITCH_PARAM2_EXIT_GUNYAH
+	smc	#0
+
+	/* Restore FDT address */
+	mov	x0, x9
+1:
+	b	reset
-- 
2.34.1