diff mbox series

[edk2] ArmPkg/ArmSmcPsciResetSystemLib: implement fallback for warm reboot

Message ID 20180606123701.4275-1-ard.biesheuvel@linaro.org
State Superseded
Headers show
Series [edk2] ArmPkg/ArmSmcPsciResetSystemLib: implement fallback for warm reboot | expand

Commit Message

Ard Biesheuvel June 6, 2018, 12:37 p.m. UTC
Implement ResetSystemLib's EnterS3WithImmediateWake() routine using
a jump back to the PEI entry point with interrupts and MMU+caches
disabled. This is only possible at boot time, when we are sure that
the current CPU is the only one up and running. Also, it depends on
the platform whether the PEI code is preserved in memory (it may be
copied to DRAM rather than execute in place), so also add a feature
PCD to selectively enable this feature.

Contributed-under: TianoCore Contribution Agreement 1.1
Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>

---
 ArmPkg/ArmPkg.dec                                                    |  4 ++++
 ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c   | 21 ++++++++++++++++++--
 ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf |  9 +++++++++
 3 files changed, 32 insertions(+), 2 deletions(-)

-- 
2.17.0

_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel

Comments

Leif Lindholm June 6, 2018, 12:49 p.m. UTC | #1
On Wed, Jun 06, 2018 at 02:37:01PM +0200, Ard Biesheuvel wrote:
> Implement ResetSystemLib's EnterS3WithImmediateWake() routine using

> a jump back to the PEI entry point with interrupts and MMU+caches

> disabled. This is only possible at boot time, when we are sure that

> the current CPU is the only one up and running. Also, it depends on

> the platform whether the PEI code is preserved in memory (it may be

> copied to DRAM rather than execute in place), so also add a feature

> PCD to selectively enable this feature.

> 

> Contributed-under: TianoCore Contribution Agreement 1.1

> Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>


It's not pretty, but it lets us start modelling desirable behaviour on
systems we alwready have. So
Reviewed-by: Leif Lindholm <leif.lindholm@linaro.org>


> ---

>  ArmPkg/ArmPkg.dec                                                    |  4 ++++

>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c   | 21 ++++++++++++++++++--

>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf |  9 +++++++++

>  3 files changed, 32 insertions(+), 2 deletions(-)

> 

> diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec

> index debe066b6f7b..3aa229fe2ec9 100644

> --- a/ArmPkg/ArmPkg.dec

> +++ b/ArmPkg/ArmPkg.dec

> @@ -85,6 +85,10 @@ [PcdsFeatureFlag.common]

>    # Define if the GICv3 controller should use the GICv2 legacy

>    gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy|FALSE|BOOLEAN|0x00000042

>  

> +  # Whether to implement warm reboot for capsule update using a jump back to the

> +  # PEI entry point with caches and interrupts disabled.

> +  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot|FALSE|BOOLEAN|0x0000001F

> +

>  [PcdsFeatureFlag.ARM]

>    # Whether to map normal memory as non-shareable. FALSE is the safe choice, but

>    # TRUE may be appropriate to fix performance problems if you don't care about

> diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> index d6d26bce5009..cb75e32771c2 100644

> --- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> +++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> @@ -15,10 +15,13 @@

>  

>  #include <PiDxe.h>

>  

> +#include <Library/ArmMmuLib.h>

> +#include <Library/ArmSmcLib.h>

>  #include <Library/BaseLib.h>

>  #include <Library/DebugLib.h>

>  #include <Library/ResetSystemLib.h>

> -#include <Library/ArmSmcLib.h>

> +#include <Library/UefiBootServicesTableLib.h>

> +#include <Library/UefiRuntimeLib.h>

>  

>  #include <IndustryStandard/ArmStdSmc.h>

>  

> @@ -89,7 +92,21 @@ EnterS3WithImmediateWake (

>    VOID

>    )

>  {

> -  // Not implemented

> +  VOID (*Reset)(VOID);

> +

> +  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&

> +      !EfiAtRuntime ()) {

> +    //

> +    // At boot time, we are the only core running, so we can implement the

> +    // immediate wake (which is used by capsule update) by disabling the MMU

> +    // and interrupts, and jumping to the PEI entry point.

> +    //

> +    Reset = (VOID *)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

> +

> +    gBS->RaiseTPL (TPL_HIGH_LEVEL);

> +    ArmDisableMmu ();

> +    Reset ();

> +  }

>  }

>  

>  /**

> diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf

> index 5a1ee976e5bc..19021cd1e8b6 100644

> --- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf

> +++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf

> @@ -30,6 +30,15 @@ [Packages]

>    MdePkg/MdePkg.dec

>  

>  [LibraryClasses]

> +  ArmMmuLib

>    ArmSmcLib

>    BaseLib

>    DebugLib

> +  UefiBootServicesTableLib

> +  UefiRuntimeLib

> +

> +[FeaturePcd]

> +  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot

> +

> +[FixedPcd]

> +  gArmTokenSpaceGuid.PcdFvBaseAddress

> -- 

> 2.17.0

> 

> _______________________________________________

> edk2-devel mailing list

> edk2-devel@lists.01.org

> https://lists.01.org/mailman/listinfo/edk2-devel

_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel
Laszlo Ersek June 6, 2018, 1:29 p.m. UTC | #2
On 06/06/18 14:37, Ard Biesheuvel wrote:
> Implement ResetSystemLib's EnterS3WithImmediateWake() routine using

> a jump back to the PEI entry point with interrupts and MMU+caches

> disabled. This is only possible at boot time, when we are sure that

> the current CPU is the only one up and running. Also, it depends on

> the platform whether the PEI code is preserved in memory (it may be

> copied to DRAM rather than execute in place), so also add a feature

> PCD to selectively enable this feature.

> 

> Contributed-under: TianoCore Contribution Agreement 1.1

> Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>

> ---

>  ArmPkg/ArmPkg.dec                                                    |  4 ++++

>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c   | 21 ++++++++++++++++++--

>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf |  9 +++++++++

>  3 files changed, 32 insertions(+), 2 deletions(-)

> 

> diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec

> index debe066b6f7b..3aa229fe2ec9 100644

> --- a/ArmPkg/ArmPkg.dec

> +++ b/ArmPkg/ArmPkg.dec

> @@ -85,6 +85,10 @@ [PcdsFeatureFlag.common]

>    # Define if the GICv3 controller should use the GICv2 legacy

>    gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy|FALSE|BOOLEAN|0x00000042

>  

> +  # Whether to implement warm reboot for capsule update using a jump back to the

> +  # PEI entry point with caches and interrupts disabled.

> +  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot|FALSE|BOOLEAN|0x0000001F

> +

>  [PcdsFeatureFlag.ARM]

>    # Whether to map normal memory as non-shareable. FALSE is the safe choice, but

>    # TRUE may be appropriate to fix performance problems if you don't care about

> diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> index d6d26bce5009..cb75e32771c2 100644

> --- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> +++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> @@ -15,10 +15,13 @@

>  

>  #include <PiDxe.h>

>  

> +#include <Library/ArmMmuLib.h>

> +#include <Library/ArmSmcLib.h>

>  #include <Library/BaseLib.h>

>  #include <Library/DebugLib.h>

>  #include <Library/ResetSystemLib.h>

> -#include <Library/ArmSmcLib.h>

> +#include <Library/UefiBootServicesTableLib.h>

> +#include <Library/UefiRuntimeLib.h>

>  

>  #include <IndustryStandard/ArmStdSmc.h>

>  

> @@ -89,7 +92,21 @@ EnterS3WithImmediateWake (

>    VOID

>    )

>  {

> -  // Not implemented

> +  VOID (*Reset)(VOID);

> +

> +  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&

> +      !EfiAtRuntime ()) {

> +    //

> +    // At boot time, we are the only core running, so we can implement the

> +    // immediate wake (which is used by capsule update) by disabling the MMU

> +    // and interrupts, and jumping to the PEI entry point.

> +    //

> +    Reset = (VOID *)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);


ISO C99 6.3.2.3 Pointers

  1 A pointer to void may be converted to or from a pointer to any
    incomplete or object type. [...]

  [...]

  5 An integer may be converted to any pointer type. Except as
    previously specified, the result is implementation-defined, might
    not be correctly aligned, might not point to an entity of the
    referenced type, and might be a trap representation. [...]

  [...]

  8 A pointer to a function of one type may be converted to a pointer to
    a function of another type and back again; [...]

The point is, converting pointer-to-void to pointer-to-function is
undefined, and I expect at least clang will yell at us for it. However,
converting an integer to pointer-to-function is implementation-defined;
i.e., the C language implementation must support it to an extent, and
must document how it works. (The "previously specified" part is about
null pointer constants, in paragraph 3, which I'm not quoting now.)

Thus, I suggest either

  typedef VOID (*RESET_FUNCTION_PTR) (VOID);
  RESET_FUNCTION_PTR Reset;

  Reset = (RESET_FUNCTION_PTR)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

or else (for kicks and giggles regarding the syntax), just

  Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

Thanks
Laszlo
_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel
Ard Biesheuvel June 7, 2018, 6:57 a.m. UTC | #3
On 6 June 2018 at 15:29, Laszlo Ersek <lersek@redhat.com> wrote:
> On 06/06/18 14:37, Ard Biesheuvel wrote:

>> Implement ResetSystemLib's EnterS3WithImmediateWake() routine using

>> a jump back to the PEI entry point with interrupts and MMU+caches

>> disabled. This is only possible at boot time, when we are sure that

>> the current CPU is the only one up and running. Also, it depends on

>> the platform whether the PEI code is preserved in memory (it may be

>> copied to DRAM rather than execute in place), so also add a feature

>> PCD to selectively enable this feature.

>>

>> Contributed-under: TianoCore Contribution Agreement 1.1

>> Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>

>> ---

>>  ArmPkg/ArmPkg.dec                                                    |  4 ++++

>>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c   | 21 ++++++++++++++++++--

>>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf |  9 +++++++++

>>  3 files changed, 32 insertions(+), 2 deletions(-)

>>

>> diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec

>> index debe066b6f7b..3aa229fe2ec9 100644

>> --- a/ArmPkg/ArmPkg.dec

>> +++ b/ArmPkg/ArmPkg.dec

>> @@ -85,6 +85,10 @@ [PcdsFeatureFlag.common]

>>    # Define if the GICv3 controller should use the GICv2 legacy

>>    gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy|FALSE|BOOLEAN|0x00000042

>>

>> +  # Whether to implement warm reboot for capsule update using a jump back to the

>> +  # PEI entry point with caches and interrupts disabled.

>> +  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot|FALSE|BOOLEAN|0x0000001F

>> +

>>  [PcdsFeatureFlag.ARM]

>>    # Whether to map normal memory as non-shareable. FALSE is the safe choice, but

>>    # TRUE may be appropriate to fix performance problems if you don't care about

>> diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

>> index d6d26bce5009..cb75e32771c2 100644

>> --- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

>> +++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

>> @@ -15,10 +15,13 @@

>>

>>  #include <PiDxe.h>

>>

>> +#include <Library/ArmMmuLib.h>

>> +#include <Library/ArmSmcLib.h>

>>  #include <Library/BaseLib.h>

>>  #include <Library/DebugLib.h>

>>  #include <Library/ResetSystemLib.h>

>> -#include <Library/ArmSmcLib.h>

>> +#include <Library/UefiBootServicesTableLib.h>

>> +#include <Library/UefiRuntimeLib.h>

>>

>>  #include <IndustryStandard/ArmStdSmc.h>

>>

>> @@ -89,7 +92,21 @@ EnterS3WithImmediateWake (

>>    VOID

>>    )

>>  {

>> -  // Not implemented

>> +  VOID (*Reset)(VOID);

>> +

>> +  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&

>> +      !EfiAtRuntime ()) {

>> +    //

>> +    // At boot time, we are the only core running, so we can implement the

>> +    // immediate wake (which is used by capsule update) by disabling the MMU

>> +    // and interrupts, and jumping to the PEI entry point.

>> +    //

>> +    Reset = (VOID *)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

>

> ISO C99 6.3.2.3 Pointers

>

>   1 A pointer to void may be converted to or from a pointer to any

>     incomplete or object type. [...]

>

>   [...]

>

>   5 An integer may be converted to any pointer type. Except as

>     previously specified, the result is implementation-defined, might

>     not be correctly aligned, might not point to an entity of the

>     referenced type, and might be a trap representation. [...]

>

>   [...]

>

>   8 A pointer to a function of one type may be converted to a pointer to

>     a function of another type and back again; [...]

>

> The point is, converting pointer-to-void to pointer-to-function is

> undefined, and I expect at least clang will yell at us for it. However,

> converting an integer to pointer-to-function is implementation-defined;

> i.e., the C language implementation must support it to an extent, and

> must document how it works. (The "previously specified" part is about

> null pointer constants, in paragraph 3, which I'm not quoting now.)

>

> Thus, I suggest either

>

>   typedef VOID (*RESET_FUNCTION_PTR) (VOID);

>   RESET_FUNCTION_PTR Reset;

>

>   Reset = (RESET_FUNCTION_PTR)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

>

> or else (for kicks and giggles regarding the syntax), just

>

>   Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

>


Thanks Laszlo

I will go for option #2 unless Leif has any objections to that?
_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel
Leif Lindholm June 7, 2018, 2:54 p.m. UTC | #4
On Thu, Jun 07, 2018 at 08:57:04AM +0200, Ard Biesheuvel wrote:
> On 6 June 2018 at 15:29, Laszlo Ersek <lersek@redhat.com> wrote:

> > On 06/06/18 14:37, Ard Biesheuvel wrote:

> >> Implement ResetSystemLib's EnterS3WithImmediateWake() routine using

> >> a jump back to the PEI entry point with interrupts and MMU+caches

> >> disabled. This is only possible at boot time, when we are sure that

> >> the current CPU is the only one up and running. Also, it depends on

> >> the platform whether the PEI code is preserved in memory (it may be

> >> copied to DRAM rather than execute in place), so also add a feature

> >> PCD to selectively enable this feature.

> >>

> >> Contributed-under: TianoCore Contribution Agreement 1.1

> >> Signed-off-by: Ard Biesheuvel <ard.biesheuvel@linaro.org>

> >> ---

> >>  ArmPkg/ArmPkg.dec                                                    |  4 ++++

> >>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c   | 21 ++++++++++++++++++--

> >>  ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf |  9 +++++++++

> >>  3 files changed, 32 insertions(+), 2 deletions(-)

> >>

> >> diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec

> >> index debe066b6f7b..3aa229fe2ec9 100644

> >> --- a/ArmPkg/ArmPkg.dec

> >> +++ b/ArmPkg/ArmPkg.dec

> >> @@ -85,6 +85,10 @@ [PcdsFeatureFlag.common]

> >>    # Define if the GICv3 controller should use the GICv2 legacy

> >>    gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy|FALSE|BOOLEAN|0x00000042

> >>

> >> +  # Whether to implement warm reboot for capsule update using a jump back to the

> >> +  # PEI entry point with caches and interrupts disabled.

> >> +  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot|FALSE|BOOLEAN|0x0000001F

> >> +

> >>  [PcdsFeatureFlag.ARM]

> >>    # Whether to map normal memory as non-shareable. FALSE is the safe choice, but

> >>    # TRUE may be appropriate to fix performance problems if you don't care about

> >> diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> >> index d6d26bce5009..cb75e32771c2 100644

> >> --- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> >> +++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c

> >> @@ -15,10 +15,13 @@

> >>

> >>  #include <PiDxe.h>

> >>

> >> +#include <Library/ArmMmuLib.h>

> >> +#include <Library/ArmSmcLib.h>

> >>  #include <Library/BaseLib.h>

> >>  #include <Library/DebugLib.h>

> >>  #include <Library/ResetSystemLib.h>

> >> -#include <Library/ArmSmcLib.h>

> >> +#include <Library/UefiBootServicesTableLib.h>

> >> +#include <Library/UefiRuntimeLib.h>

> >>

> >>  #include <IndustryStandard/ArmStdSmc.h>

> >>

> >> @@ -89,7 +92,21 @@ EnterS3WithImmediateWake (

> >>    VOID

> >>    )

> >>  {

> >> -  // Not implemented

> >> +  VOID (*Reset)(VOID);

> >> +

> >> +  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&

> >> +      !EfiAtRuntime ()) {

> >> +    //

> >> +    // At boot time, we are the only core running, so we can implement the

> >> +    // immediate wake (which is used by capsule update) by disabling the MMU

> >> +    // and interrupts, and jumping to the PEI entry point.

> >> +    //

> >> +    Reset = (VOID *)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

> >

> > ISO C99 6.3.2.3 Pointers

> >

> >   1 A pointer to void may be converted to or from a pointer to any

> >     incomplete or object type. [...]

> >

> >   [...]

> >

> >   5 An integer may be converted to any pointer type. Except as

> >     previously specified, the result is implementation-defined, might

> >     not be correctly aligned, might not point to an entity of the

> >     referenced type, and might be a trap representation. [...]

> >

> >   [...]

> >

> >   8 A pointer to a function of one type may be converted to a pointer to

> >     a function of another type and back again; [...]

> >

> > The point is, converting pointer-to-void to pointer-to-function is

> > undefined, and I expect at least clang will yell at us for it. However,

> > converting an integer to pointer-to-function is implementation-defined;

> > i.e., the C language implementation must support it to an extent, and

> > must document how it works. (The "previously specified" part is about

> > null pointer constants, in paragraph 3, which I'm not quoting now.)

> >

> > Thus, I suggest either

> >

> >   typedef VOID (*RESET_FUNCTION_PTR) (VOID);

> >   RESET_FUNCTION_PTR Reset;

> >

> >   Reset = (RESET_FUNCTION_PTR)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

> >

> > or else (for kicks and giggles regarding the syntax), just

> >

> >   Reset = (VOID (*)(VOID))(UINTN)FixedPcdGet64 (PcdFvBaseAddress);

> 

> Thanks Laszlo

> 

> I will go for option #2 unless Leif has any objections to that?


I think my preference would be the first simply becauase it feels more
TianoCore. But I'm relaly happy with either option.

Regards,

Leif
_______________________________________________
edk2-devel mailing list
edk2-devel@lists.01.org
https://lists.01.org/mailman/listinfo/edk2-devel
diff mbox series

Patch

diff --git a/ArmPkg/ArmPkg.dec b/ArmPkg/ArmPkg.dec
index debe066b6f7b..3aa229fe2ec9 100644
--- a/ArmPkg/ArmPkg.dec
+++ b/ArmPkg/ArmPkg.dec
@@ -85,6 +85,10 @@  [PcdsFeatureFlag.common]
   # Define if the GICv3 controller should use the GICv2 legacy
   gArmTokenSpaceGuid.PcdArmGicV3WithV2Legacy|FALSE|BOOLEAN|0x00000042
 
+  # Whether to implement warm reboot for capsule update using a jump back to the
+  # PEI entry point with caches and interrupts disabled.
+  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot|FALSE|BOOLEAN|0x0000001F
+
 [PcdsFeatureFlag.ARM]
   # Whether to map normal memory as non-shareable. FALSE is the safe choice, but
   # TRUE may be appropriate to fix performance problems if you don't care about
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
index d6d26bce5009..cb75e32771c2 100644
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.c
@@ -15,10 +15,13 @@ 
 
 #include <PiDxe.h>
 
+#include <Library/ArmMmuLib.h>
+#include <Library/ArmSmcLib.h>
 #include <Library/BaseLib.h>
 #include <Library/DebugLib.h>
 #include <Library/ResetSystemLib.h>
-#include <Library/ArmSmcLib.h>
+#include <Library/UefiBootServicesTableLib.h>
+#include <Library/UefiRuntimeLib.h>
 
 #include <IndustryStandard/ArmStdSmc.h>
 
@@ -89,7 +92,21 @@  EnterS3WithImmediateWake (
   VOID
   )
 {
-  // Not implemented
+  VOID (*Reset)(VOID);
+
+  if (FeaturePcdGet (PcdArmReenterPeiForCapsuleWarmReboot) &&
+      !EfiAtRuntime ()) {
+    //
+    // At boot time, we are the only core running, so we can implement the
+    // immediate wake (which is used by capsule update) by disabling the MMU
+    // and interrupts, and jumping to the PEI entry point.
+    //
+    Reset = (VOID *)(UINTN)FixedPcdGet64 (PcdFvBaseAddress);
+
+    gBS->RaiseTPL (TPL_HIGH_LEVEL);
+    ArmDisableMmu ();
+    Reset ();
+  }
 }
 
 /**
diff --git a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
index 5a1ee976e5bc..19021cd1e8b6 100644
--- a/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
+++ b/ArmPkg/Library/ArmSmcPsciResetSystemLib/ArmSmcPsciResetSystemLib.inf
@@ -30,6 +30,15 @@  [Packages]
   MdePkg/MdePkg.dec
 
 [LibraryClasses]
+  ArmMmuLib
   ArmSmcLib
   BaseLib
   DebugLib
+  UefiBootServicesTableLib
+  UefiRuntimeLib
+
+[FeaturePcd]
+  gArmTokenSpaceGuid.PcdArmReenterPeiForCapsuleWarmReboot
+
+[FixedPcd]
+  gArmTokenSpaceGuid.PcdFvBaseAddress