From 98c666cf875831f252ec5ee5dece2722f02e8706 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kornel=20Dul=C4=99ba?= <kd@FreeBSD.org>
Date: Mon, 20 Feb 2023 14:48:40 +0000
Subject: [PATCH] arm: Fix initialization of VFP context

Make sure that pcb_vfpsaved is always initialized.
Create a vfp_new_thread helper that is heavily based on the arm64 logic.
While here remove un unnecessary assigment and add an assertion
to make sure that it's been properly initialized before we return
from a VFP exception.

Reported by: Mark Millard <marklmi@yahoo.com>
Tested by: Mark Millard <marklmi@yahoo.com>
Differential Revision: https://reviews.freebsd.org/D38698
---
 sys/arm/arm/machdep.c    |  1 +
 sys/arm/arm/vfp.c        | 33 +++++++++++++++++++++++++++++++--
 sys/arm/arm/vm_machdep.c | 10 +++++++---
 sys/arm/include/vfp.h    |  1 +
 4 files changed, 40 insertions(+), 5 deletions(-)

diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 0c82190181b..8c0d79caffa 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -380,6 +380,7 @@ init_proc0(vm_offset_t kstack)
 	thread0.td_pcb->pcb_fpflags = 0;
 	thread0.td_pcb->pcb_vfpcpu = -1;
 	thread0.td_pcb->pcb_vfpstate.fpscr = VFPSCR_DN;
+	thread0.td_pcb->pcb_vfpsaved = &thread0.td_pcb->pcb_vfpstate;
 	thread0.td_frame = &proc0_tf;
 	pcpup->pc_curpcb = thread0.td_pcb;
 }
diff --git a/sys/arm/arm/vfp.c b/sys/arm/arm/vfp.c
index d51c4b6e061..b417e02f699 100644
--- a/sys/arm/arm/vfp.c
+++ b/sys/arm/arm/vfp.c
@@ -34,6 +34,7 @@ __FBSDID("$FreeBSD$");
 #ifdef VFP
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/limits.h>
 #include <sys/proc.h>
 #include <sys/imgact_elf.h>
 #include <sys/kernel.h>
@@ -251,17 +252,45 @@ vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 	curpcb = curthread->td_pcb;
 	cpu = PCPU_GET(cpuid);
 	if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) {
-		if (curpcb->pcb_vfpsaved == NULL)
-			curpcb->pcb_vfpsaved = &curpcb->pcb_vfpstate;
 		vfp_restore(curpcb->pcb_vfpsaved);
 		curpcb->pcb_vfpcpu = cpu;
 		PCPU_SET(fpcurthread, curthread);
 	}
 
 	critical_exit();
+
+	KASSERT(curpcb->pcb_vfpsaved == &curpcb->pcb_vfpstate,
+	    ("Kernel VFP state in use when entering userspace"));
+
 	return (0);
 }
 
+/*
+ * Update the VFP state for a forked process or new thread. The PCB will
+ * have been copied from the old thread.
+ * The code is heavily based on arm64 logic.
+ */
+void
+vfp_new_thread(struct thread *newtd, struct thread *oldtd, bool fork)
+{
+	struct pcb *newpcb;
+
+	newpcb = newtd->td_pcb;
+
+	/* Kernel threads start with clean VFP */
+	if ((oldtd->td_pflags & TDP_KTHREAD) != 0) {
+		newpcb->pcb_fpflags &=
+		    ~(PCB_FP_STARTED | PCB_FP_KERN | PCB_FP_NOSAVE);
+	} else {
+		MPASS((newpcb->pcb_fpflags & (PCB_FP_KERN|PCB_FP_NOSAVE)) == 0);
+		if (!fork) {
+			newpcb->pcb_fpflags &= ~PCB_FP_STARTED;
+		}
+	}
+
+	newpcb->pcb_vfpsaved = &newpcb->pcb_vfpstate;
+	newpcb->pcb_vfpcpu = UINT_MAX;
+}
 /*
  * Restore the given state to the VFP hardware.
  */
diff --git a/sys/arm/arm/vm_machdep.c b/sys/arm/arm/vm_machdep.c
index d899e2cd584..14459616291 100644
--- a/sys/arm/arm/vm_machdep.c
+++ b/sys/arm/arm/vm_machdep.c
@@ -137,9 +137,9 @@ cpu_fork(struct thread *td1, struct proc *p2, struct thread *td2, int flags)
 	pcb2->pcb_regs.sf_sp = STACKALIGN(td2->td_frame);
 	pcb2->pcb_regs.sf_tpidrurw = (register_t)get_tls();
 
-	pcb2->pcb_vfpcpu = -1;
-	pcb2->pcb_vfpsaved = &pcb2->pcb_vfpstate;
-	pcb2->pcb_vfpstate.fpscr = initial_fpscr;
+#ifdef VFP
+	vfp_new_thread(td2, td1, true);
+#endif
 
 	tf = td2->td_frame;
 	tf->tf_spsr &= ~PSR_C;
@@ -216,6 +216,10 @@ cpu_copy_thread(struct thread *td, struct thread *td0)
 	td->td_frame->tf_spsr &= ~PSR_C;
 	td->td_frame->tf_r0 = 0;
 
+#ifdef VFP
+	vfp_new_thread(td, td0, false);
+#endif
+
 	/* Setup to release spin count in fork_exit(). */
 	td->td_md.md_spinlock_count = 1;
 	td->td_md.md_saved_cspr = PSR_SVC32_MODE;
diff --git a/sys/arm/include/vfp.h b/sys/arm/include/vfp.h
index e15e088970b..5a27cc68afd 100644
--- a/sys/arm/include/vfp.h
+++ b/sys/arm/include/vfp.h
@@ -157,6 +157,7 @@ struct vfp_state {
 void	get_vfpcontext(struct thread *, mcontext_vfp_t *);
 void	set_vfpcontext(struct thread *, mcontext_vfp_t *);
 void    vfp_init(void);
+void	vfp_new_thread(struct thread*, struct thread*, bool);
 void    vfp_store(struct vfp_state *, boolean_t);
 void    vfp_discard(struct thread *);
 void	vfp_restore_state(void);