diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.S b/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.S
new file mode 100644
index 000000000000..1edca941cb8f
--- /dev/null
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.S
@@ -0,0 +1,30 @@
+/** @file
+  ResetSystemLib implementation using PSCI calls
+
+  Copyright (c) 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
+  which accompanies this distribution. The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <AsmMacroIoLibV8.h>
+
+ASM_FUNC(DisableMmuAndReenterPei)
+  stp   x29, x30, [sp, #-16]!
+  mov   x29, sp
+
+  bl    ArmDisableMmu
+
+  // no memory accesses after MMU and caches have been disabled
+
+  MOV64 (x0, FixedPcdGet64 (PcdFvBaseAddress))
+  blr   x0
+
+  // never returns
+  nop
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.asm b/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.asm
new file mode 100644
index 000000000000..b3dca150c150
--- /dev/null
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.asm
@@ -0,0 +1,35 @@
+/** @file
+  ResetSystemLib implementation using PSCI calls
+
+  Copyright (c) 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
+  which accompanies this distribution. The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+  AREA Reset, CODE, READONLY
+
+  EXPORT DisableMmuAndReenterPei
+  IMPORT ArmDisableMmu
+
+DisableMmuAndReenterPei
+  stp   x29, x30, [sp, #-16]!
+  mov   x29, sp
+
+  bl    ArmDisableMmu
+
+  ; no memory accesses after MMU and caches have been disabled
+
+  movl  x0, FixedPcdGet64 (PcdFvBaseAddress)
+  blr   x0
+
+  ; never returns
+  nop
+
+  END
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.S b/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.S
new file mode 100644
index 000000000000..b6fe2bd82baa
--- /dev/null
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.S
@@ -0,0 +1,29 @@
+/** @file
+  ResetSystemLib implementation using PSCI calls
+
+  Copyright (c) 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
+  which accompanies this distribution. The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <AsmMacroIoLib.h>
+
+ASM_FUNC(DisableMmuAndReenterPei)
+  push  {lr}
+
+  bl    ArmDisableMmu
+
+  // no memory accesses after MMU and caches have been disabled
+
+  MOV32 (r0, FixedPcdGet64 (PcdFvBaseAddress))
+  blx   r0
+
+  // never returns
+  nop
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.asm b/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.asm
new file mode 100644
index 000000000000..f098b352418b
--- /dev/null
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/Arm/Reset.asm
@@ -0,0 +1,34 @@
+/** @file
+  ResetSystemLib implementation using PSCI calls
+
+  Copyright (c) 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
+  which accompanies this distribution. The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+    INCLUDE AsmMacroExport.inc
+    PRESERVE8
+
+    IMPORT ArmDisableMmu
+
+RVCT_ASM_EXPORT DisableMmuAndReenterPei
+  push  {lr}
+
+  bl    ArmDisableMmu
+
+  ; no memory accesses after MMU and caches have been disabled
+
+  mov32 r0, FixedPcdGet64 (PcdFvBaseAddress)
+  blx   r0
+
+  ; never returns
+  nop
+
+  END
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
index 10ceafd14d5d..3f7b8ae0b169 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
@@ -81,6 +81,8 @@ ResetShutdown (
   ArmCallSmc (&ArmSmcArgs);
 }
 
+VOID DisableMmuAndReenterPei (VOID);
+
 /**
   This function causes the system to enter S3 and then wake up immediately.
 
@@ -92,7 +94,12 @@ EnterS3WithImmediateWake (
   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 ()) {
@@ -101,11 +108,49 @@ EnterS3WithImmediateWake (
     // immediate wake (which is used by capsule update) by disabling the MMU
     // and interrupts, and jumping to the PEI entry point.
     //
-    Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);
 
-    gBS->RaiseTPL (TPL_HIGH_LEVEL);
-    ArmDisableMmu ();
-    Reset ();
+    //
+    // Obtain the size of the memory map
+    //
+    MemMapSize = 0;
+    MemMap = NULL;
+    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);
+    }
+
+    DisableMmuAndReenterPei ();
   }
 }
 
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
index 19021cd1e8b6..3b8925c6f9cd 100644
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
@@ -21,6 +21,14 @@
   VERSION_STRING                 = 1.0
   LIBRARY_CLASS                  = ResetSystemLib
 
+[Sources.AARCH64]
+  AArch64/Reset.S   | GCC
+  AArch64/Reset.asm | MSFT
+
+[Sources.ARM]
+  Arm/Reset.S       | GCC
+  Arm/Reset.asm     | RVCT
+
 [Sources]
   ArmSmcPsciResetSystemLib.c
 
