[PATCH 4/4] lib: sbi: trap: Save/restore FP and vector state on trap entry/exit

[dave.patel at riscstar.com]

From: Dave Patel <dave.patel at riscstar.com>

Add eager floating-point and vector context save/restore in the OpenSBI trap
handler. The FP and vector state of the current hart is saved on trap entry
and restored before returning from the trap.

This ensures that machine-mode trap handling does not clobber floating-point
or vector state belonging to lower privilege software (e.g. supervisor mode),
providing correct isolation across trap boundaries.

The implementation leverages sbi_fp_save/restore() and sbi_vector_save/restore()
helpers and uses per-hart context pointers stored in sbi_scratch.

This follows an eager context switching model where the full FP and vector state
is preserved unconditionally on every trap, avoiding the need for lazy
enable/disable or trap-on-first-use mechanisms.

Notes:
- Assumes FP and vector units are enabled when trap handling occurs
  (mstatus.FS/VS != Off).
- Context pointers (fp_ctx, vec_ctx) must be valid when used.
- This may introduce additional trap latency due to full state save/restore.

Signed-off-by: Dave Patel <dave.patel at riscstar.com>
---
 lib/sbi/sbi_fp.c   |  2 +-
 lib/sbi/sbi_trap.c | 18 ++++++++++++++++++
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/lib/sbi/sbi_fp.c b/lib/sbi/sbi_fp.c
index 4d5e0f68..24d97937 100644
--- a/lib/sbi/sbi_fp.c
+++ b/lib/sbi/sbi_fp.c
@@ -72,7 +72,7 @@ void sbi_fp_save(struct sbi_fp_context *dst)
 
 void sbi_fp_restore(const struct sbi_fp_context *src)
 {
-    if (!src)
+	if (!src)
         return;
 
 	asm volatile(
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index f41db4d1..6a182216 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -24,6 +24,8 @@
 #include <sbi/sbi_sse.h>
 #include <sbi/sbi_timer.h>
 #include <sbi/sbi_trap.h>
+#include <sbi/sbi_fp.h>
+#include <sbi/sbi_vector.h>
 
 static void sbi_trap_error_one(const struct sbi_trap_context *tcntx,
 			       const char *prefix, u32 hartid, u32 depth)
@@ -310,6 +312,14 @@ struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx)
 	struct sbi_trap_regs *regs = &tcntx->regs;
 	ulong mcause = tcntx->trap.cause;
 
+	/* save floating context */
+	struct sbi_fp_context *fpctx = scratch->fp_ctx;
+	sbi_fp_save(fpctx);
+
+	/* save vector context */
+	struct sbi_vector_context *vecctx = scratch->vec_ctx;
+	sbi_vector_save(vecctx);
+
 	/* Update trap context pointer */
 	tcntx->prev_context = sbi_trap_get_context(scratch);
 	sbi_trap_set_context(scratch, tcntx);
@@ -373,5 +383,13 @@ trap_done:
 		sbi_sse_process_pending_events(regs);
 
 	sbi_trap_set_context(scratch, tcntx->prev_context);
+
+
+	/* restore floating context */
+	sbi_fp_restore(fpctx);
+
+	/* restore vector context */
+	sbi_vector_restore(vecctx);
+
 	return tcntx;
 }
-- 
2.43.0