ArmPkg/ArmSmcPsciResetSystemLib: add missing call to ExitBootServices()

[mirror_edk2.git] / ArmPkg / Library / ArmSmcPsciResetSystemLib / ArmSmcPsciResetSystemLib.c

diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
index d6d26bce5009905f368099aa157af43b501cbbb3..3f7b8ae0b16907a31fc3b5f9cd374cc0e18d9c0c 100644 (file)
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
@@ -1,7 +1,7 @@
 /** @file\r
   ResetSystemLib implementation using PSCI calls\r
 \r
-  Copyright (c) 2017, Linaro Ltd. All rights reserved.<BR>\r
+  Copyright (c) 2017 - 2018, Linaro Ltd. All rights reserved.<BR>\r
 \r
   This program and the accompanying materials\r
   are licensed and made available under the terms and conditions of the BSD License\r
@@ -15,10 +15,13 @@
 \r
 #include <PiDxe.h>\r
 \r
+#include <Library/ArmMmuLib.h>\r
+#include <Library/ArmSmcLib.h>\r
 #include <Library/BaseLib.h>\r
 #include <Library/DebugLib.h>\r
 #include <Library/ResetSystemLib.h>\r
-#include <Library/ArmSmcLib.h>\r
+#include <Library/UefiBootServicesTableLib.h>\r
+#include <Library/UefiRuntimeLib.h>\r
 \r
 #include <IndustryStandard/ArmStdSmc.h>\r
 \r
@@ -78,6 +81,8 @@ ResetShutdown (
   ArmCallSmc (&ArmSmcArgs);\r
 }\r
 \r
+VOID DisableMmuAndReenterPei (VOID);\r
+\r
 /**\r
   This function causes the system to enter S3 and then wake up immediately.\r
 \r
@@ -89,7 +94,64 @@ EnterS3WithImmediateWake (
   VOID\r
   )\r
 {\r
-  // Not implemented\r
+  EFI_PHYSICAL_ADDRESS        Alloc;\r
+  EFI_MEMORY_DESCRIPTOR       *MemMap;\r
+  UINTN                       MemMapSize;\r
+  UINTN                       MapKey, DescriptorSize;\r
+  UINT32                      DescriptorVersion;\r
+  EFI_STATUS                  Status;\r
+\r
+  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&\r
+      !EfiAtRuntime ()) {\r
+    //\r
+    // At boot time, we are the only core running, so we can implement the\r
+    // immediate wake (which is used by capsule update) by disabling the MMU\r
+    // and interrupts, and jumping to the PEI entry point.\r
+    //\r
+\r
+    //\r
+    // Obtain the size of the memory map\r
+    //\r
+    MemMapSize = 0;\r
+    MemMap = NULL;\r
+    Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,\r
+                    &DescriptorVersion);\r
+    ASSERT (Status == EFI_BUFFER_TOO_SMALL);\r
+\r
+    //\r
+    // Add some slack to the allocation to cater for changes in the memory\r
+    // map if ExitBootServices () fails the first time around.\r
+    //\r
+    MemMapSize += SIZE_4KB;\r
+    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData,\r
+                    EFI_SIZE_TO_PAGES (MemMapSize), &Alloc);\r
+    ASSERT_EFI_ERROR (Status);\r
+\r
+    MemMap = (EFI_MEMORY_DESCRIPTOR *)(UINTN)Alloc;\r
+\r
+    Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,\r
+                    &DescriptorVersion);\r
+    ASSERT_EFI_ERROR (Status);\r
+\r
+    Status = gBS->ExitBootServices (gImageHandle, MapKey);\r
+    if (EFI_ERROR (Status)) {\r
+      //\r
+      // ExitBootServices () may fail the first time around if an event fired\r
+      // right after the call to GetMemoryMap() which allocated or freed memory.\r
+      // Since that first call to ExitBootServices () will disarm the timer,\r
+      // this is guaranteed not to happen again, so one additional attempt\r
+      // should suffice.\r
+      //\r
+      Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,\r
+                      &DescriptorVersion);\r
+      ASSERT_EFI_ERROR (Status);\r
+\r
+      Status = gBS->ExitBootServices (gImageHandle, MapKey);\r
+      ASSERT_EFI_ERROR (Status);\r
+    }\r
+\r
+    DisableMmuAndReenterPei ();\r
+  }\r
 }\r
 \r
 /**\r