From 2f7020233fb069b96ed1486cbaec295306897591 Mon Sep 17 00:00:00 2001
From: Yadong Qi <yadong.qi@intel.com>
Date: Fri, 3 Dec 2021 13:46:54 +0800
Subject: [PATCH] bm: trigger vmcall to activate VT-d in eVMM

Add new function trusty_late_init() which is called between
ExitBootService() and kernel jumping.
Currently, only activate_vtd_vmcall() is called in trusty_late_init().

Signed-off-by: Yadong Qi <yadong.qi@intel.com>
---
 include/trusty_common.h          |  2 ++
 libkernelflinger/android.c       |  7 +++++++
 libkernelflinger/trusty_common.c | 14 ++++++++++++++
 3 files changed, 23 insertions(+)

diff --git a/include/trusty_common.h b/include/trusty_common.h
index 6f0f8247..6221a3b6 100644
--- a/include/trusty_common.h
+++ b/include/trusty_common.h
@@ -36,4 +36,6 @@
 
 EFI_STATUS load_tos_image(OUT VOID **bootimage);
 
+VOID trusty_late_init(VOID);
+
 #endif /* _TRUSTY_COMMON_H_ */
diff --git a/libkernelflinger/android.c b/libkernelflinger/android.c
index bb124f9e..36dfedd9 100644
--- a/libkernelflinger/android.c
+++ b/libkernelflinger/android.c
@@ -467,6 +467,13 @@ static inline EFI_STATUS handover_jump(EFI_HANDLE image,
 
 boot:
 
+#ifdef USE_TRUSTY
+        /*
+         * Called after ExitBootService.
+         */
+        trusty_late_init();
+#endif
+
 #if __LP64__
         /* The 64-bit kernel entry is 512 bytes after the start. */
         kernel_start += 512;
diff --git a/libkernelflinger/trusty_common.c b/libkernelflinger/trusty_common.c
index 458f64ba..895684f8 100644
--- a/libkernelflinger/trusty_common.c
+++ b/libkernelflinger/trusty_common.c
@@ -258,3 +258,17 @@ EFI_STATUS load_tos_image(OUT VOID **tosimage)
 
         return EFI_SUCCESS;
 }
+
+static VOID activate_vtd(VOID)
+{
+#define VMCALL_ACTIVATE_VTD 0x56544400ULL        // "VTD"
+        asm volatile ("vmcall" : : "a"(VMCALL_ACTIVATE_VTD));
+}
+
+/*
+ * This function is designed to run after bootloader triggered ExitBootService.
+ */
+VOID trusty_late_init(VOID)
+{
+        activate_vtd();
+}