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 1/3] 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}

From 9abcd8afe0cb8001e809b4fec20cb08cf8f6eeb7 Mon Sep 17 00:00:00 2001
From: Richard Campbell <rlcamp@users.noreply.github.com>
Date: Fri, 4 Mar 2022 10:40:30 -0800
Subject: [PATCH 2/3] Added passthrough of FORCE_SOFT_FLOAT and
 FORCE_HARD_FLOAT Makefile options

---
 Makefile | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/Makefile b/Makefile
index 1fb87fd..405022b 100644
--- a/Makefile
+++ b/Makefile
@@ -35,6 +35,18 @@ ifeq ($(FREESTANDING),yes)
 	EXPORT_UNPREFIXED = no
 endif
 
+FORCE_SOFT_FLOAT := no
+
+ifeq ($(FORCE_SOFT_FLOAT),yes)
+        CPPFLAGS += -DFORCE_SOFT_FLOAT
+endif
+
+FORCE_HARD_FLOAT := no
+
+ifeq ($(FORCE_HARD_FLOAT),yes)
+        CPPFLAGS += -DFORCE_HARD_FLOAT
+endif
+
 ifeq ($(EXPORT_UNPREFIXED),yes)
 	CPPFLAGS += -DEXPORT_UNPREFIXED
 endif

From 5244775fb93ab9f15ebe7137ef1165355e0a8906 Mon Sep 17 00:00:00 2001
From: Richard Campbell <rlcamp@users.noreply.github.com>
Date: Fri, 4 Mar 2022 10:41:54 -0800
Subject: [PATCH 3/3] Added note to readme about floating point WIP

---
 README.md | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/README.md b/README.md
index 663b7c7..d9d5dbe 100644
--- a/README.md
+++ b/README.md
@@ -101,4 +101,5 @@ ucontext functions:
   save/restore FPU registers or vector registers may be added in a later release as a build-time
   setting -- for now, we assume a soft-float ABI with no optional processor features.  In practice, this
   does not really matter, code using these functions are unlikely to be impacted by this design
-  assumption.
+  assumption. This is a work in progress, as newer compilers will spill even non-floating-point state
+  through floating point registers when allowed to do so.