[PATCH] ArmPkg/ArmSmcPsciResetSystemLib: add missing call to ExitBootServices()

[Ard Biesheuvel <ard.biesheuvel@linaro.org>]

Our poor man's implementation of EnterS3WithImmediateWake () currently
sets a high TPL level to disable interrupts, and simply calls the
PEI entrypoint again after disabling the MMU.

Unfortunately, this is not sufficient: DMA capable devices such as
network controllers or USB controllers may still be enabled and
writing to memory, e.g., in response to incoming network packets.

So instead, do the full ExitBootServices() dance: allocate space and
get the memory map, call ExitBootServices(), and in case it fails, get
the memory map again and call ExitBootServices() again. This ensures
that all cleanup related to DMA capable devices is performed before
doing the warm reset.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>
---
 ArmPkg/Include/Library/ArmLib.h                                    |  1 +
 ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c | 51 ++++++++++++++++++--
 2 files changed, 49 insertions(+), 3 deletions(-)

diff --git a/ArmPkg/Include/Library/ArmLib.h b/ArmPkg/Include/Library/ArmLib.h
index e46df447b33d..ffda50e9d767 100644
--- a/ArmPkg/Include/Library/ArmLib.h
+++ b/ArmPkg/Include/Library/ArmLib.h
@@ -59,6 +59,7 @@ typedef enum {
 
 typedef struct {
   EFI_PHYSICAL_ADDRESS          PhysicalBase;
+  EFI_VIRTUAL_ADDRESS           VirtualBase;
   UINT64                        Length;
   ARM_MEMORY_REGION_ATTRIBUTES  Attributes;
 } ARM_MEMORY_REGION_DESCRIPTOR;
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
index 10ceafd14d5d..c9c42ab3b244 100644
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
@@ -1,7 +1,7 @@
 /** @file
   ResetSystemLib implementation using PSCI calls
 
-  Copyright (c) 2017, Linaro Ltd. All rights reserved.<BR>
+  Copyright (c) 2017 - 2018, Linaro Ltd. All rights reserved.<BR>
 
   This program and the accompanying materials
   are licensed and made available under the terms and conditions of the BSD License
@@ -92,7 +92,13 @@ EnterS3WithImmediateWake (
   VOID
   )
 {
-  VOID (*Reset)(VOID);
+  VOID                        (*Reset)(VOID);
+  EFI_PHYSICAL_ADDRESS        Alloc;
+  EFI_MEMORY_DESCRIPTOR       *MemMap;
+  UINTN                       MemMapSize;
+  UINTN                       MapKey, DescriptorSize;
+  UINT32                      DescriptorVersion;
+  EFI_STATUS                  Status;
 
   if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&
       !EfiAtRuntime ()) {
@@ -103,7 +109,46 @@ EnterS3WithImmediateWake (
     //
     Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);
 
-    gBS->RaiseTPL (TPL_HIGH_LEVEL);
+    //
+    // Obtain the size of the memory map
+    //
+    MemMapSize = 0;
+    Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,
+                    &DescriptorVersion);
+    ASSERT (Status == EFI_BUFFER_TOO_SMALL);
+
+    //
+    // Add some slack to the allocation to cater for changes in the memory
+    // map if ExitBootServices () fails the first time around.
+    //
+    MemMapSize += SIZE_4KB;
+    Status = gBS->AllocatePages (AllocateAnyPages, EfiBootServicesData,
+                    EFI_SIZE_TO_PAGES (MemMapSize), &Alloc);
+    ASSERT_EFI_ERROR (Status);
+
+    MemMap = (EFI_MEMORY_DESCRIPTOR *)(UINTN)Alloc;
+
+    Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,
+                    &DescriptorVersion);
+    ASSERT_EFI_ERROR (Status);
+
+    Status = gBS->ExitBootServices (gImageHandle, MapKey);
+    if (EFI_ERROR (Status)) {
+      //
+      // ExitBootServices () may fail the first time around if an event fired
+      // right after the call to GetMemoryMap() which allocated or freed memory.
+      // Since that first call to ExitBootServices () will disarm the timer,
+      // this is guaranteed not to happen again, so one additional attempt
+      // should suffice.
+      //
+      Status = gBS->GetMemoryMap (&MemMapSize, MemMap, &MapKey, &DescriptorSize,
+                      &DescriptorVersion);
+      ASSERT_EFI_ERROR (Status);
+
+      Status = gBS->ExitBootServices (gImageHandle, MapKey);
+      ASSERT_EFI_ERROR (Status);
+    }
+
     ArmDisableMmu ();
     Reset ();
   }
-- 
2.17.1