[PATCH v3 3/3] lib: sbi: domain FP/Vector context support for context switch

[dave.patel@riscstar.com]

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

This patch adds proper support for per-domain floating-point (FP) and
vector (V) contexts in the domain context switch logic. Each domain
now maintains its own FP and vector state, which is saved and restored
during domain switches.

Changes include:

- Added `fp_ctx` and `vec_ctx` members to `struct sbi_domain`.
- Introduced `sbi_fp_domain_init/exit()` and `sbi_vector_domain_init/exit()`
  to allocate and free per-domain FP and vector context.
- Modified `sbi_domain_register()` to initialize FP/Vector context per domain.
- Updated `switch_to_next_domain_context()` to save/restore FP and vector
  contexts safely:
    - Ensures FS/VS fields in `mstatus` are enabled (set to Initial) only if Off.
    - Restores original FS/VS bits after context switch.
    - Adds NULL checks to handle domains without FP or Vector extensions.
- Updated domain context deinit to free FP and vector contexts per domain.
- Added runtime checks for FP and vector extensions where needed.
- Corrected handling of MSTATUS FS/VS bits to avoid unsafe full-bit writes.

This improves support for multi-domain systems with FP and Vector
extensions, and prevents corruption of FP/Vector state during domain
switches.

Signed-off-by: Dave Patel <dave.patel@riscstar.com>
---
 include/sbi/sbi_domain.h     |  4 +++
 include/sbi/sbi_fp.h         |  5 +++
 include/sbi/sbi_vector.h     |  5 +++
 lib/sbi/sbi_domain.c         | 22 ++++++++++++
 lib/sbi/sbi_domain_context.c | 42 +++++++++++++++++++++++
 lib/sbi/sbi_fp.c             | 37 ++++++++++++++++++++
 lib/sbi/sbi_vector.c         | 65 +++++++++++++++++++++++++++++-------
 7 files changed, 168 insertions(+), 12 deletions(-)

diff --git a/include/sbi/sbi_domain.h b/include/sbi/sbi_domain.h
index 882b62c2..8f0bdbb6 100644
--- a/include/sbi/sbi_domain.h
+++ b/include/sbi/sbi_domain.h
@@ -217,6 +217,10 @@ struct sbi_domain {
 	bool fw_region_inited;
 	/** per-domain wired-IRQ courier state */
 	void *virq_priv;
+    /** per-domain float context state */
+	void *fp_ctx;
+    /** per-domain vector context state */
+    	void *vec_ctx;
 };

 /** The root domain instance */
diff --git a/include/sbi/sbi_fp.h b/include/sbi/sbi_fp.h
index 8079bb3b..6ba04b61 100644
--- a/include/sbi/sbi_fp.h
+++ b/include/sbi/sbi_fp.h
@@ -10,6 +10,9 @@

 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_types.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_console.h>

 #if defined(__riscv_f) || defined(__riscv_d)

@@ -32,5 +35,7 @@ struct sbi_fp_context { };

 void sbi_fp_save(struct sbi_fp_context *dst);
 void sbi_fp_restore(const struct sbi_fp_context *src);
+int  sbi_fp_domain_init(struct sbi_domain *dom);
+void sbi_fp_domain_exit(struct sbi_domain *dom);

 #endif //__SBI_VECTOR_H__
diff --git a/include/sbi/sbi_vector.h b/include/sbi/sbi_vector.h
index 4ecfaa0b..345be3bf 100644
--- a/include/sbi/sbi_vector.h
+++ b/include/sbi/sbi_vector.h
@@ -10,6 +10,9 @@
 #define __SBI_VECTOR_H__

 #include <sbi/sbi_types.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_console.h>

 #define SBI_MAX_VLENB CONFIG_SBI_MAX_VLENB

@@ -25,6 +28,8 @@ struct sbi_vector_context {

 void sbi_vector_save(struct sbi_vector_context *dst);
 void sbi_vector_restore(const struct sbi_vector_context *src);
+int  sbi_vector_domain_init(struct sbi_domain *dom);
+void sbi_vector_domain_exit(struct sbi_domain *dom);

 #endif //__SBI_VECTOR_H__

diff --git a/lib/sbi/sbi_domain.c b/lib/sbi/sbi_domain.c
index 498a1d56..cdc416b4 100644
--- a/lib/sbi/sbi_domain.c
+++ b/lib/sbi/sbi_domain.c
@@ -19,6 +19,8 @@
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_string.h>
 #include <sbi/sbi_virq.h>
+#include <sbi/sbi_vector.h>
+#include <sbi/sbi_fp.h>

 SBI_LIST_HEAD(domain_list);

@@ -703,6 +705,26 @@ int sbi_domain_register(struct sbi_domain *dom,
 		return rc;
 	}

+	/* Init per-domain floating context */
+	rc = sbi_fp_domain_init(dom);
+	if (rc) {
+		sbi_printf("%s: fp init failed for %s (error %d)\n",
+				__func__, dom->name, rc);
+		sbi_list_del(&dom->node);
+		return rc;
+	}
+
+#ifdef __riscv_v
+    /* Init per-domain vector context */
+	rc = sbi_vector_domain_init(dom);
+	if (rc) {
+		sbi_printf("%s: vec init failed for %s (error %d)\n",
+				__func__, dom->name, rc);
+		sbi_list_del(&dom->node);
+		return rc;
+	}
+#endif
+
 	return 0;
 }

diff --git a/lib/sbi/sbi_domain_context.c b/lib/sbi/sbi_domain_context.c
index 158f4990..fb843a91 100644
--- a/lib/sbi/sbi_domain_context.c
+++ b/lib/sbi/sbi_domain_context.c
@@ -18,6 +18,9 @@
 #include <sbi/sbi_domain_context.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_trap.h>
+#include <sbi/sbi_vector.h>
+#include <sbi/sbi_fp.h>
+

 /** Context representation for a hart within a domain */
 struct hart_context {
@@ -143,6 +146,37 @@ static int switch_to_next_domain_context(struct hart_context *ctx,
 	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSQOSID))
 		ctx->srmcfg	= csr_swap(CSR_SRMCFG, dom_ctx->srmcfg);

+    /* Read current mstatus */
+    unsigned long mstatus = csr_read(CSR_MSTATUS);
+    unsigned long new_mstatus = mstatus;
+
+    /* Ensure FS is enabled (not Off) */
+    if ((mstatus & MSTATUS_FS) == 0)
+        new_mstatus |= MSTATUS_FS;
+
+#ifdef __riscv_v
+    /* Ensure VS is enabled (not Off) */
+    if ((mstatus & MSTATUS_VS) == 0)
+        new_mstatus |= MSTATUS_VS;
+#endif
+
+    /* Update mstatus only if needed */
+    if (new_mstatus != mstatus)
+    csr_write(CSR_MSTATUS, new_mstatus);
+
+    /* Save current domain context and restore target domain's F and V context */
+    sbi_fp_save(current_dom->fp_ctx);
+    sbi_fp_restore(target_dom->fp_ctx);
+#ifdef __riscv_v
+    sbi_vector_save(current_dom->vec_ctx);
+    sbi_vector_restore(target_dom->vec_ctx);
+#endif
+
+    /* Restore original mstatus if we modified it */
+    if (new_mstatus != mstatus) {
+        csr_write(CSR_MSTATUS, mstatus);
+    }
+
 	/* Save current trap state and restore target domain's trap state */
 	trap_ctx = sbi_trap_get_context(scratch);
 	sbi_memcpy(&ctx->trap_ctx, trap_ctx, sizeof(*trap_ctx));
@@ -286,5 +320,13 @@ int sbi_domain_context_init(void)

 void sbi_domain_context_deinit(void)
 {
+    struct sbi_domain *dom;
+    sbi_domain_for_each(dom) {
+#ifdef __riscv_v
+        sbi_vector_domain_exit(dom);
+#endif
+        sbi_fp_domain_exit(dom);
+    }
+
 	sbi_domain_unregister_data(&dcpriv);
 }
diff --git a/lib/sbi/sbi_fp.c b/lib/sbi/sbi_fp.c
index 5d72b72e..37acb07c 100644
--- a/lib/sbi/sbi_fp.c
+++ b/lib/sbi/sbi_fp.c
@@ -7,6 +7,7 @@
  */

 #include <sbi/riscv_asm.h>
+#include <sbi/sbi_heap.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_fp.h>

@@ -185,7 +186,43 @@ void sbi_fp_restore(const struct sbi_fp_context *src)

 	csr_write(CSR_FCSR, src->fcsr);
 }
+
+int sbi_fp_domain_init(struct sbi_domain *dom)
+{
+    if (!dom)
+        return SBI_EINVAL;
+
+    if (dom->fp_ctx)
+        return SBI_OK;
+
+   sbi_printf("[FPU] Init FP Context \n");
+
+   struct sbi_fp_context *fp;
+   fp = sbi_zalloc(sizeof(*fp));
+   if (!fp)
+       return SBI_ENOMEM;
+
+   dom->fp_ctx = fp;
+
+   return SBI_OK;
+}
+
+void sbi_fp_domain_exit(struct sbi_domain *dom)
+{
+   if (!dom || !dom->fp_ctx)
+        return;
+
+   sbi_free(dom->fp_ctx);
+   dom->fp_ctx = NULL;
+}
+
 #else
+
 void sbi_fp_save(struct sbi_fp_context *dst) {}
 void sbi_fp_restore(const struct sbi_fp_context *src) {}
+int sbi_fp_domain_init(struct sbi_domain *dom)
+{
+	return SBI_OK;
+}
+void sbi_fp_domain_exit(struct sbi_domain *dom) {}
 #endif // FP present
diff --git a/lib/sbi/sbi_vector.c b/lib/sbi/sbi_vector.c
index 5a3f34d7..959566c1 100644
--- a/lib/sbi/sbi_vector.c
+++ b/lib/sbi/sbi_vector.c
@@ -12,21 +12,23 @@
 #include <sbi/sbi_vector.h>
 #include <sbi/sbi_types.h>
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_heap.h>

 #ifdef OPENSBI_CC_SUPPORT_VECTOR
+#define VLEN_MAX 65536

 static inline void vsetvl(ulong vl, ulong vtype)
 {
-    ulong tmp;
-
-    asm volatile(
-	".option push\n\t"
-	".option arch, +v\n\t"
-	"vsetvl %0, %1, %2\n\t"
-	".option pop\n\t"
-	: "=r"(tmp)
-	: "r"(vl), "r"(vtype)
-	: "memory");
+	ulong tmp;
+
+	asm volatile(
+		".option push\n\t"
+		".option arch, +v\n\t"
+		"vsetvl %0, %1, %2\n\t"
+		".option pop\n\t"
+		: "=r"(tmp)
+		: "r"(vl), "r"(vtype)
+		: "memory");
 }

 static inline unsigned long vector_vlenb(void)
@@ -64,7 +66,7 @@ void sbi_vector_save(struct sbi_vector_context *dst)

 	/* Step 1: Save CSRs */
 	READ_CSR(dst->vtype,  vtype);
-	READ_CSR(dst->vl,     vl);
+	READ_CSR(dst->vl,	  vl);
 	READ_CSR(dst->vcsr,   vcsr);
 	READ_CSR(dst->vstart, vstart);

@@ -194,12 +196,41 @@ void sbi_vector_restore(const struct sbi_vector_context *src)

 	/* Restore CSRs first */
 	WRITE_CSR(vtype,  src->vtype);
-	WRITE_CSR(vl,     src->vl);
+	WRITE_CSR(vl,	  src->vl);
 	WRITE_CSR(vcsr,   src->vcsr);
 	WRITE_CSR(vstart, src->vstart);
 #undef WRITE_CSR
 }

+int sbi_vector_domain_init(struct sbi_domain *dom)
+{
+	if (!dom)
+		return SBI_EINVAL;
+
+	if (dom->vec_ctx)
+		return SBI_OK;
+
+	sbi_printf("[Vector] Init Vector Context \n");
+
+	struct sbi_vector_context *v;
+	v = sbi_zalloc(sizeof(*v));
+	if (!v)
+		return SBI_ENOMEM;
+
+	dom->vec_ctx = v;
+
+	return SBI_OK;
+}
+
+void sbi_vector_domain_exit(struct sbi_domain *dom)
+{
+	if (!dom || !dom->vec_ctx)
+		return;
+
+	sbi_free(dom->vec_ctx);
+	dom->vec_ctx = NULL;
+}
+
 #else

 void sbi_vector_save(struct sbi_vector_context *dst)
@@ -212,4 +243,14 @@ void sbi_vector_restore(const struct sbi_vector_context *src)
 	return;
 }

+int sbi_vector_domain_init(struct sbi_domain *dom)
+{
+	return SBI_OK;
+}
+
+void sbi_vector_domain_exit
+{
+	return;
+}
 #endif /* OPENSBI_CC_SUPPORT_VECTOR */
+
--
2.43.0


-- 
opensbi mailing list
opensbi@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/opensbi