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);