Arm64 support SIMD registers context to/from native context (#25757)

2 files changed, 60 insertions, 0 deletions

diff --git a/src/pal/src/include/pal/context.h b/src/pal/src/include/pal/context.h
index 146b9a0db9..69acd361bb 100644
--- a/src/pal/src/include/pal/context.h
+++ b/src/pal/src/include/pal/context.h
@@ -277,6 +277,44 @@ inline void *FPREG_Xstate_Ymmh(const ucontext_t *uc)
 #define MCREG_Sp(mc)      ((mc).sp)
 #define MCREG_Pc(mc)      ((mc).pc)
 #define MCREG_Cpsr(mc)    ((mc).pstate)
+
+
+inline
+fpsimd_context* GetNativeSigSimdContext(native_context_t *mc)
+{
+    size_t size = 0;
+
+    do
+    {
+        fpsimd_context* fp = reinterpret_cast<fpsimd_context *>(&mc->uc_mcontext.__reserved[size]);
+
+        if(fp->head.magic == FPSIMD_MAGIC)
+        {
+            _ASSERTE(fp->head.size >= sizeof(fpsimd_context));
+            _ASSERTE(size + fp->head.size <= sizeof(mc->uc_mcontext.__reserved));
+
+            return fp;
+        }
+
+        if (fp->head.size == 0)
+        {
+            break;
+        }
+
+        size += fp->head.size;
+    } while (size + sizeof(fpsimd_context) <= sizeof(mc->uc_mcontext.__reserved));
+
+    _ASSERTE(false);
+
+    return nullptr;
+}
+
+inline
+const fpsimd_context* GetConstNativeSigSimdContext(const native_context_t *mc)
+{
+    return GetNativeSigSimdContext(const_cast<native_context_t*>(mc));
+}
+
 #else
     // For FreeBSD, as found in x86/ucontext.h
 #define MCREG_Rbp(mc)	    ((mc).mc_rbp)
diff --git a/src/pal/src/thread/context.cpp b/src/pal/src/thread/context.cpp
index 10b2db4ed0..b65190da72 100644
--- a/src/pal/src/thread/context.cpp
+++ b/src/pal/src/thread/context.cpp
@@ -465,6 +465,17 @@ void CONTEXTToNativeContext(CONST CONTEXT *lpContext, native_context_t *native)
         {
             FPREG_Xmm(native, i) = lpContext->FltSave.XmmRegisters[i];
         }
+#elif defined(_ARM64_)
+        fpsimd_context* fp = GetNativeSigSimdContext(native);
+        if (fp)
+        {
+            fp->fpsr = lpContext->Fpsr;
+            fp->fpcr = lpContext->Fpcr;
+            for (int i = 0; i < 32; i++)
+            {
+                *(NEON128*) &fp->vregs[i] = lpContext->V[i];
+            }
+        }
 #endif
     }
 
@@ -563,6 +574,17 @@ void CONTEXTFromNativeContext(const native_context_t *native, LPCONTEXT lpContex
         {
             lpContext->FltSave.XmmRegisters[i] = FPREG_Xmm(native, i);
         }
+#elif defined(_ARM64_)
+        const fpsimd_context* fp = GetConstNativeSigSimdContext(native);
+        if (fp)
+        {
+            lpContext->Fpsr = fp->fpsr;
+            lpContext->Fpcr = fp->fpcr;
+            for (int i = 0; i < 32; i++)
+            {
+                lpContext->V[i] = *(NEON128*) &fp->vregs[i];
+            }
+        }
 #endif
     }