From 7bc3e90984eac2deaf776636354104f5e5c364ce Mon Sep 17 00:00:00 2001
From: Richard Campbell <rlcamp@users.noreply.github.com>
Date: Fri, 4 Mar 2022 10:35:25 -0800
Subject: [PATCH] Added arm conditional vfp save/restore

---
 arch/arm/defs.h                     |  2 ++
 arch/arm/getcontext.S               | 21 +++++++++++++++++++++
 arch/arm/include/libucontext/bits.h |  2 ++
 arch/arm/setcontext.S               | 16 ++++++++++++++++
 arch/arm/swapcontext.S              | 20 ++++++++++++++++++++
 5 files changed, 61 insertions(+)

diff --git a/arch/arm/defs.h b/arch/arm/defs.h
index 3b6b9ba..e063ed6 100644
--- a/arch/arm/defs.h
+++ b/arch/arm/defs.h
@@ -2,6 +2,8 @@
 
 #define REG_SZ		(4)
 #define MCONTEXT_GREGS	(32)
+#define VFP_MAGIC_OFFSET (232)
+#define VFP_D8_OFFSET (304)
 
 #define TYPE(__proc)
 
diff --git a/arch/arm/getcontext.S b/arch/arm/getcontext.S
index 57a56c6..0afa150 100644
--- a/arch/arm/getcontext.S
+++ b/arch/arm/getcontext.S
@@ -22,6 +22,27 @@ FUNC(libucontext_getcontext)
 	str	r13, [r0, #REG_OFFSET(13)]
 	str	r14, [r0, #REG_OFFSET(15)]
 
+#ifndef FORCE_SOFT_FLOAT
+#ifndef FORCE_HARD_FLOAT
+        /* test for vfp, set kernel-defined magic number in uc_regspace */
+        push    {r0-r1,fp,lr}
+        mov     r0, #16
+        bl      getauxval
+        tst     r0, #64
+        pop     {r0-r1,fp,lr}
+	moveq	r2, #0
+	ldrne	r2, =#0x56465001
+	str	r2, [r0, #VFP_MAGIC_OFFSET]
+        beq     1f
+#endif
+	/* if vfp detected, save d8-d15 */
+	.fpu	vfp
+	add	r1, r0, #VFP_D8_OFFSET
+	vstmia	r1, {d8-d15}
+	.fpu	softvfp
+1:
+#endif
+
 	/* return 0 */
 	mov	r0, #0
 	mov	pc, lr
diff --git a/arch/arm/include/libucontext/bits.h b/arch/arm/include/libucontext/bits.h
index dc5c379..caad8d4 100644
--- a/arch/arm/include/libucontext/bits.h
+++ b/arch/arm/include/libucontext/bits.h
@@ -23,6 +23,8 @@ typedef struct libucontext_ucontext {
 	struct libucontext_ucontext *uc_link;
 	libucontext_stack_t uc_stack;
 	libucontext_mcontext_t uc_mcontext;
+	unsigned long uc_sigmask[128 / sizeof(long)];
+	unsigned long long uc_regspace[64];
 } libucontext_ucontext_t;
 
 #endif
diff --git a/arch/arm/setcontext.S b/arch/arm/setcontext.S
index 7ab956e..792d8f2 100644
--- a/arch/arm/setcontext.S
+++ b/arch/arm/setcontext.S
@@ -16,6 +16,22 @@ ALIAS(setcontext, libucontext_setcontext)
 ALIAS(__setcontext, libucontext_setcontext)
 
 FUNC(libucontext_setcontext)
+#ifndef FORCE_SOFT_FLOAT
+#ifndef FORCE_HARD_FLOAT
+	/* test for vfp magic number set by getcontext */
+	ldr	r2, [r0, #VFP_MAGIC_OFFSET]
+	ldr	r3, =#0x56465001
+	cmp	r2, r3
+        bne     1f
+#endif
+	/* if vfp in use, restore d8-d15 from uc_regspace */
+	.fpu	vfp
+	add	r14, r0, #VFP_D8_OFFSET
+	vldmia	r14, {d8-d15}
+	.fpu 	softvfp
+1:
+#endif
+
 	/* copy all of the current registers into the ucontext structure */
 	add	r14, r0, #REG_OFFSET(0)
 	ldmia	r14, {r0-r12}
diff --git a/arch/arm/swapcontext.S b/arch/arm/swapcontext.S
index 29c6889..f841182 100644
--- a/arch/arm/swapcontext.S
+++ b/arch/arm/swapcontext.S
@@ -22,6 +22,26 @@ FUNC(libucontext_swapcontext)
 	str	r13, [r0,#REG_OFFSET(13)]
 	str	r14, [r0,#REG_OFFSET(15)]
 
+#ifndef FORCE_SOFT_FLOAT
+#ifndef FORCE_HARD_FLOAT
+	/* test for vfp magic number, copy to other ucontext */
+	ldr	r3, [r1, #VFP_MAGIC_OFFSET]
+	ldr	r4, =#0x56465001
+	str	r3, [r0, #VFP_MAGIC_OFFSET]
+	cmp	r3, r4
+	bne	1f
+#endif
+	/* if vfp in use, save and restore d8-d15 */
+	.fpu	vfp
+	add	r2, r0, #VFP_D8_OFFSET
+	vstmia	r2, {d8-d15}
+
+	add	r14, r1, #VFP_D8_OFFSET
+	vldmia	r14, {d8-d15}
+	.fpu	softvfp
+1:
+#endif
+
 	/* load new registers from the second ucontext structure */
 	add	r14, r1, #REG_OFFSET(0)
 	ldmia	r14, {r0-r12}