From ccdc4cc7e4e884a5a1eb25451363e38a3091c775 Mon Sep 17 00:00:00 2001
From: Alexander Graf <agraf@suse.de>
Date: Tue, 9 Dec 2014 22:56:29 +0100
Subject: HisiPkg: D01: Run bootwrapper before we exit runtime services

When we exit the runtime services, we need to also execute our bootwrapper
code to make sure that we get into HYP mode.

This patch adds wrapping code that allows us to execute the bootwrapper code
when exiting runtime services. However we only run them for non-direct kernel
boot, as that one would run the bootwrapper twice otherwise.

Signed-off-by: Alexander Graf <agraf@suse.de>
Signed-off-by: Wei Xu <xuwei5@huawei.com>
---
 HisiPkg/D01BoardPkg/Bds/Bds.c      | 31 +++++++++++++++++++++++++++++++
 HisiPkg/D01BoardPkg/Bds/BootMenu.c | 28 +++++++++++++++++++---------
 2 files changed, 50 insertions(+), 9 deletions(-)

diff --git a/HisiPkg/D01BoardPkg/Bds/Bds.c b/HisiPkg/D01BoardPkg/Bds/Bds.c
index 267bb4f71..4e2e34679 100644
--- a/HisiPkg/D01BoardPkg/Bds/Bds.c
+++ b/HisiPkg/D01BoardPkg/Bds/Bds.c
@@ -37,6 +37,7 @@ typedef unsigned char   U8;
 #define RAM_TEST_NOWORK_TAG          (0x0A0A0A0A) 
 
 EFI_HANDLE mImageHandle;
+static int skip_enter_bootwrapper;
 
 //************************************************
 #define SEEK_SET  0
@@ -748,6 +749,7 @@ BdsEntry (
 //        }
 
         /*---------------OS-----------------*/
+        skip_enter_bootwrapper = 1;
         BootOptionList (&BootOptionsList);
         BootGo (&BootOptionsList);
         #endif
@@ -763,6 +765,30 @@ BdsEntry (
 
 }
 
+long bootwrapper_saved_stack;
+static void EnterBootwrapper(void)
+{
+#if !defined(__thumb__)
+#error Please compile in thumb mode
+#endif
+
+  /*
+   * This chunk of assembly enters the bootwrapper code, then
+   * immediately returns here and recovers all clobbered registers
+   */
+  asm volatile("ldr	r0, =return_from_bootwrapper	\n\
+		orr	r0, r0, #1			\n\
+		ldr	r4, =bootwrapper_saved_stack	\n\
+		str	sp, [r4]			\n\
+		b	RunBootwrapper			\n\
+		return_from_bootwrapper:		\n\
+		ldr	r4, =bootwrapper_saved_stack	\n\
+		ldr	sp, [r4]"
+		: :
+		: "cc", "r0", "r1", "r2", "r3", "r4", "r5", "r6", "r7", "r8",
+		  "r9", "r10", "r11", "r12", "r14");
+}
+
 /**
   EFI Exit Event
 
@@ -783,6 +809,11 @@ ExitBootServicesEvent (
   )
 {
   ReadBootwrapper();
+
+  if (skip_enter_bootwrapper)
+    return;
+
+  EnterBootwrapper();
 }
 
 EFI_BDS_ARCH_PROTOCOL  gBdsProtocol = {
diff --git a/HisiPkg/D01BoardPkg/Bds/BootMenu.c b/HisiPkg/D01BoardPkg/Bds/BootMenu.c
index 2bd84a024..4bc9cda82 100644
--- a/HisiPkg/D01BoardPkg/Bds/BootMenu.c
+++ b/HisiPkg/D01BoardPkg/Bds/BootMenu.c
@@ -784,22 +784,15 @@ EFI_STATUS LoadLinuxAtSecEnd()
     return Status;
 }
 
-EFI_STATUS
-BootGo (
-  IN LIST_ENTRY *BootOptionsList
-  )
+EFI_STATUS RunBootwrapper()
 {
   EFI_STATUS Status;
 
-  Status = ShutdownUefiBootServices ();
-  if(EFI_ERROR(Status)) {
-  DEBUG((EFI_D_ERROR,"ERROR: Can not shutdown UEFI boot services. Status=0x%X\n", Status));
-  }
-  
   *(UINTN*)(UINTN)(0xe302b000 + 0x18) = 0;
   *(UINTN*)(UINTN)(0xe302b000 + 0x1c) = 0;
 
   *(volatile UINT32 *)(0xe0000000 + 0x100) = TEXT_DDR_BASE;
+
   ArmCleanDataCache();
   *(UINT8*)(0xf4007000) = 'G';
   Status = LoadLinuxAtSecEnd();
@@ -811,6 +804,23 @@ BootGo (
   return Status;
 }
 
+EFI_STATUS
+BootGo (
+  IN LIST_ENTRY *BootOptionsList
+  )
+{
+  EFI_STATUS Status;
+
+  Status = ShutdownUefiBootServices ();
+  if(EFI_ERROR(Status)) {
+  DEBUG((EFI_D_ERROR,"ERROR: Can not shutdown UEFI boot services. Status=0x%X\n", Status));
+  }
+
+  Status = RunBootwrapper();
+
+  return Status;
+}
+
 struct BOOT_MAIN_ENTRY {
   CONST CHAR16* Description;
   EFI_STATUS (*Callback) (IN LIST_ENTRY *BootOptionsList);
-- 
cgit v1.2.3