From ae234e908f1f88289ea5fd613e62c62c65b057c1 Mon Sep 17 00:00:00 2001
From: Mikhail Krichanov <mikhailkrichanov@gmail.com>
Date: Fri, 5 Jul 2024 16:14:21 +0300
Subject: [PATCH] Ring3: Defined CallRing3() for ARM.

---
 MdeModulePkg/Core/Dxe/DxeRing3/ARM/SysCall.S  | 12 +++---
 .../Dxe/SysCall/AARCH64/CoreBootServices.S    |  2 +-
 .../Core/Dxe/SysCall/ARM/CoreBootServices.S   | 41 +++++++++++++++++--
 .../Core/Dxe/SysCall/ARM/InitializeMsr.c      | 31 ++++++++++++--
 4 files changed, 73 insertions(+), 13 deletions(-)

diff --git a/MdeModulePkg/Core/Dxe/DxeRing3/ARM/SysCall.S b/MdeModulePkg/Core/Dxe/DxeRing3/ARM/SysCall.S
index dd9d6d8721..25fcc31ab2 100644
--- a/MdeModulePkg/Core/Dxe/DxeRing3/ARM/SysCall.S
+++ b/MdeModulePkg/Core/Dxe/DxeRing3/ARM/SysCall.S
@@ -5,6 +5,8 @@
 
 #include <AsmMacroIoLib.h>
 
+.extern ASM_PFX(Ring3Call)
+
 //------------------------------------------------------------------------------
 // EFI_STATUS
 // EFIAPI
@@ -14,8 +16,8 @@
 //   );
 //------------------------------------------------------------------------------
 ASM_FUNC(SysCall)
-
-  bx    LR
+  svc #0
+  bx  LR
 
 //------------------------------------------------------------------------------
 // VOID
@@ -24,9 +26,7 @@ ASM_FUNC(SysCall)
 //   IN RING3_CALL_DATA *Data
 //   );
 //
-//   (rcx) RIP of Ring3EntryPoint saved for SYSRET in CallRing3().
-//   (rdx) Data
+//   (r0) Data
 //------------------------------------------------------------------------------
 ASM_FUNC(Ring3EntryPoint)
-
-  bx    LR
+  b ASM_PFX(Ring3Call)
diff --git a/MdeModulePkg/Core/Dxe/SysCall/AARCH64/CoreBootServices.S b/MdeModulePkg/Core/Dxe/SysCall/AARCH64/CoreBootServices.S
index 2b0f498d87..2174d0dbba 100644
--- a/MdeModulePkg/Core/Dxe/SysCall/AARCH64/CoreBootServices.S
+++ b/MdeModulePkg/Core/Dxe/SysCall/AARCH64/CoreBootServices.S
@@ -91,7 +91,7 @@ ASM_FUNC(ArmCallRing3)
   // Prepare Ring3 SP and EntryPoint.
   msr  sp_el0, x1
   msr  elr_el1, x2
-  // Save Core SP and switch to Ring3Call Stack.
+  // Save Core SP and switch to CoreSysCall Stack.
   mov  x5, sp
   str  x5, [x4]
   mov  sp, x3
diff --git a/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S b/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
index 90b000fc0d..cdbbacf501 100644
--- a/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
+++ b/MdeModulePkg/Core/Dxe/SysCall/ARM/CoreBootServices.S
@@ -49,10 +49,45 @@ ASM_FUNC(CoreBootServices)
 //   IN RING3_CALL_DATA *Data
 //   );
 //
-//   (rcx) Data
+//   (r0) Data
+//   (r1) gRing3CallStackTop
+//   (r2) gRing3EntryPoint
+//   (r3) gCoreSysCallStackTop
+//
+//   (On Core Stack) &CoreSp
 //------------------------------------------------------------------------------
-ASM_FUNC(CallRing3)
-    bx    LR
+ASM_FUNC(ArmCallRing3)
+  // Save registers.
+  push  {R4-R12, LR}
+  // R6 is &CoreSp
+  ldr   R6, [SP, #0x28]
+
+#if (FixedPcdGet32(PcdVFPEnabled))
+  // Save vstm registers in case they are used in optimizations.
+  vpush {d0-d15}
+#endif
+
+  // Disable interrupts.
+  mrs   R4, CPSR
+  orr   R5, R4, #0x80
+  msr   CPSR_c, R5
+
+  // Set SP_usr to gRing3CallStackTop.
+  push  {R1}
+  mov   R1, SP
+  ldmia R1, {SP}^
+  pop   {R1}
+  // Set SPSR M[3:0] bits to User mode.
+  and   R4, R4, #0xFFFFFFF0
+
+  // Save Core SP and switch to CoreSysCall Stack.
+  mov   R5, SP
+  str   R5, [R6]
+  mov   SP, R3
+
+  push  {R4}
+  push  {R2}
+  rfefd SP
 
 //------------------------------------------------------------------------------
 // VOID
diff --git a/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c b/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
index 02845996d9..adf5509d06 100644
--- a/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
+++ b/MdeModulePkg/Core/Dxe/SysCall/ARM/InitializeMsr.c
@@ -9,6 +9,18 @@
 
 #include "DxeMain.h"
 
+STATIC UINTN  mCoreSp;
+
+EFI_STATUS
+EFIAPI
+ArmCallRing3 (
+  IN RING3_CALL_DATA *Data,
+  IN VOID            *StackPointer,
+  IN VOID            *EntryPoint,
+  IN VOID            *SysCallStack,
+  IN VOID            *CoreStack
+  );
+
 VOID
 EFIAPI
 ArmSetPan (
@@ -35,7 +47,7 @@ InitializeMsr (
     ArmSetPan ();
   } else {
     DEBUG ((DEBUG_ERROR, "Core: Failed to initialize MSRs for Ring3.\n"));
-    ASSERT (FALSE);
+    // ASSERT (FALSE);
   }
 }
 
@@ -45,7 +57,9 @@ DisableSMAP (
   VOID
   )
 {
-  ArmClearPan ();
+  if (ArmHasPan ()) {
+    ArmClearPan ();
+  }
 }
 
 VOID
@@ -54,5 +68,16 @@ EnableSMAP (
   VOID
   )
 {
-  ArmSetPan ();
+  if (ArmHasPan ()) {
+    ArmSetPan ();
+  }
+}
+
+EFI_STATUS
+EFIAPI
+CallRing3 (
+  IN RING3_CALL_DATA *Data
+  )
+{
+  return ArmCallRing3 (Data, gRing3CallStackTop, gRing3EntryPoint, gCoreSysCallStackTop, &mCoreSp);
 }