[PATCH v4 1/3] mach-snapdragon: Add early EL2 hypervisor switch support

[Aswin Murugan]

Exit Gunyah hypervisor and switch to EL2 during early boot on
Qualcomm Dragonwing family platforms. This must happen before EL
register configuration in start.S so that U-Boot can run properly
at EL2 to support hypervisors requiring EL2 privilege level.

Signed-off-by: Aswin Murugan <aswin.murugan at oss.qualcomm.com>
---
 arch/arm/mach-snapdragon/Kconfig              |  8 +++++
 arch/arm/mach-snapdragon/include/mach/boot0.h |  3 ++
 .../include/mach/gunyah_exit_boot0.h          | 36 +++++++++++++++++++
 3 files changed, 47 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..3a95d7141f9 100644
--- a/arch/arm/mach-snapdragon/Kconfig
+++ b/arch/arm/mach-snapdragon/Kconfig
@@ -56,6 +56,14 @@ 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 on
+	  Qualcomm Dragonwing family platforms. This must happen before EL
+	  register configuration in start.S so that U-Boot can run properly
+	  at EL2 to support hypervisors requiring EL2 privilege level.
 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..80b9f7effa5
--- /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 for Dragonwing Family Platforms
+ *
+ * Exit Gunyah hypervisor and switch to EL2 during early boot on Qualcomm
+ * Dragonwing family platforms. This must happen before EL register
+ * configuration in start.S so that U-Boot can run properly at EL2 to
+ * support hypervisors requiring EL2 privilege level.
+ */
+
+#include <asm/macro.h>
+#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
+
+	/* Only perform hypervisor switch if we're at EL1 */
+	switch_el x9, 3f, 2f, 1f
+
+	/* Save FDT address before we modify x0 */
+1:	mov	x9, x0
+
+	/* 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
+2:
+3:	b	reset
-- 
2.34.1