diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.S b/ArmPkg/Library/ArmSmcPsciResetSystemLib/AArch64/Reset.S
new file mode 100644
index 0000000000..1edca941cb
--- /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.
+
+ 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
+
+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 0000000000..9e30c13329
--- /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.
+;
+; 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 0000000000..b6fe2bd82b
--- /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.
+
+ 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
+
+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 0000000000..fa4fbd0e07
--- /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.
+;
+; 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 10ceafd14d..3f7b8ae0b1 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.
+ Copyright (c) 2017 - 2018, Linaro Ltd. All rights reserved.
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 19021cd1e8..3b8925c6f9 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