Ring3: Defined ReturnToCore() for ARM.

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

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,