From 3bddf9ac070e9721fd32313a9ceccb92274fb3a0 Mon Sep 17 00:00:00 2001
From: Mikhail Krichanov <mikhailkrichanov@gmail.com>
Date: Tue, 9 Jul 2024 14:03:39 +0300
Subject: [PATCH] Ring3: Defined ReturnToCore() for ARM.

---
 .../Core/Dxe/SysCall/ARM/CoreBootServices.S   | 29 +++++++++++++++++--
 .../Core/Dxe/SysCall/ARM/InitializeMsr.c      | 11 +++++++
 2 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S b/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
index 60df4c47e5..0804cf453f 100644
--- a/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
+++ b/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
@@ -80,8 +80,8 @@ ASM_FUNC(ArmCallRing3)
 
   // Disable interrupts.
   mrs   R4, CPSR
-  orr   R5, R4, #0x80
-  msr   CPSR_c, R5
+  cpsid if
+  isb
 
   // Set SP_usr to gRing3CallStackTop.
   push  {R1}
@@ -100,6 +100,31 @@ ASM_FUNC(ArmCallRing3)
   push  {R2}
   rfefd SP
 
+//------------------------------------------------------------------------------
+// VOID
+// EFIAPI
+// ReturnToCore (
+//   IN EFI_STATUS Status,
+//   IN UINTN      CoreSp
+//   );
+//------------------------------------------------------------------------------
+ASM_FUNC(ReturnToCore)
+  // Switch to Core Stack.
+  mov   SP, R1
+
+  // Restore registers and Stack.
+#if (FixedPcdGet32(PcdVFPEnabled))
+  vpop  {d0-d15}
+#endif
+
+  pop   {R4-R12, LR}
+
+  // Enable interrupts.
+  cpsie if
+  isb
+
+  bx    LR
+
 //------------------------------------------------------------------------------
 // VOID
 // EFIAPI
diff --git a/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c b/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
index f07ba49e2c..8fc8db82ad 100644
--- a/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
+++ b/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
@@ -22,6 +22,13 @@ ArmCallRing3 (
   IN VOID            *CoreStack
   );
 
+VOID
+EFIAPI
+ReturnToCore (
+  IN EFI_STATUS Status,
+  IN UINTN      CoreSp
+  );
+
 VOID
 EFIAPI
 ArmSetPan (
@@ -46,6 +53,10 @@ SysCallBootService (
   EFI_STATUS              Status;
   EFI_PHYSICAL_ADDRESS    Physical;
 
+  if (Type == SysCallReturnToCore) {
+    ReturnToCore (*(EFI_STATUS *)CoreRbp, mCoreSp);
+  }
+
   Status = CoreAllocatePages (
              AllocateAnyPages,
              EfiRing3MemoryType,