ArmPkg/ArmSmcPsciResetSystemLib: implement fallback for warm reboot
[mirror_edk2.git] / ArmPkg / Library / ArmSmcPsciResetSystemLib / ArmSmcPsciResetSystemLib.c
index d6d26bc..10ceafd 100644 (file)
 \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