ArmPkg/ArmSmcPsciResetSystemLib: implement fallback for warm reboot

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

diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
index d6d26bce5009905f368099aa157af43b501cbbb3..10ceafd14d5de0de5465817615f7650a81ae7073 100644 (file)
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
@@ -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
@@ -89,7 +92,21 @@ EnterS3WithImmediateWake (
   VOID\r
   )\r
 {\r
-  // Not implemented\r
+  VOID (*Reset)(VOID);\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
+    Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);\r
+\r
+    gBS->RaiseTPL (TPL_HIGH_LEVEL);\r
+    ArmDisableMmu ();\r
+    Reset ();\r
+  }\r
 }\r
 \r
 /**\r