[PATCH] FPU and Vector Lazy context switch implementation
dave.patel at riscstar.com
dave.patel at riscstar.com
Wed Mar 11 10:11:32 PDT 2026
From: Dave Patel <dave.patel at riscstar.com>
Signed-off-by: Dave Patel <dave.patel at riscstar.com>
---
.gitignore | 1 +
firmware/payloads/test_main.c | 44 +++++++++-
include/sbi/sbi_domain.h | 21 +++++
include/sbi/sbi_fp.h | 33 +++++++
include/sbi/sbi_scratch.h | 5 ++
include/sbi/sbi_trap.h | 3 +
include/sbi/sbi_vector.h | 40 +++++++++
lib/sbi/objects.mk | 2 +
lib/sbi/sbi_fp.c | 138 +++++++++++++++++++++++++++++
lib/sbi/sbi_hart.c | 5 ++
lib/sbi/sbi_illegal_insn.c | 68 ++++++++++++++-
lib/sbi/sbi_trap.c | 37 ++++++++
lib/sbi/sbi_vector.c | 159 ++++++++++++++++++++++++++++++++++
lib/utils/irqchip/aplic.c | 2 +
14 files changed, 552 insertions(+), 6 deletions(-)
create mode 100644 include/sbi/sbi_fp.h
create mode 100644 include/sbi/sbi_vector.h
create mode 100644 lib/sbi/sbi_fp.c
create mode 100644 lib/sbi/sbi_vector.c
diff --git a/.gitignore b/.gitignore
index 94caeb90..7208d3f6 100644
--- a/.gitignore
+++ b/.gitignore
@@ -20,3 +20,4 @@ install/
tags
cscope*
*~
+out/
diff --git a/firmware/payloads/test_main.c b/firmware/payloads/test_main.c
index 03f6ba39..ae88b87a 100644
--- a/firmware/payloads/test_main.c
+++ b/firmware/payloads/test_main.c
@@ -58,9 +58,47 @@ static inline void sbi_ecall_shutdown(void)
__asm__ __volatile__("wfi" ::: "memory"); \
} while (0)
+#include <stdint.h>
+
+// static inline double fpu_test(double a, double b) {
+// double tmp;
+// /* Some dummy FP operations to trigger FP lazy restore */
+// tmp = a * b + a / b;
+// //sbi_printf("S-Mode trap handler [%f]\n", tmp);
+// return tmp;
+// }
+
+__attribute__((noinline))
+double fpu_test(double a, double b)
+{
+ double r;
+
+ asm volatile(
+ "fmul.d %0, %1, %2\n"
+ "fadd.d %0, %0, %1\n"
+ : "=f"(r)
+ : "f"(a), "f"(b)
+ );
+
+ return r;
+}
+
+static inline void vector_test(void) {
+ asm volatile(
+ "vsetvli t0, zero, e64,m8\n"
+ "vmv.v.x v0, t0\n"
+ );
+}
+
+
void test_main(unsigned long a0, unsigned long a1)
{
- sbi_ecall_console_puts("\nTest payload running\n");
- sbi_ecall_shutdown();
- sbi_ecall_console_puts("sbi_ecall_shutdown failed to execute.\n");
+ //sbi_printf("\nOpenSBI S-Mode App: Emulated Aplic\n");
+ sbi_ecall_console_puts("\nTest payload running\n");
+
+ vector_test();
+
+ sbi_ecall_console_puts("\nTest payload running\n");
+ sbi_ecall_shutdown();
+ sbi_ecall_console_puts("sbi_ecall_shutdown failed to execute.\n");
}
diff --git a/include/sbi/sbi_domain.h b/include/sbi/sbi_domain.h
index 882b62c2..95e588d9 100644
--- a/include/sbi/sbi_domain.h
+++ b/include/sbi/sbi_domain.h
@@ -183,6 +183,27 @@ static inline bool sbi_domain_memregion_is_subset(
return false;
}
+/* Floating point context */
+struct sbi_domain_fp_state {
+ bool valid;
+ uint64_t f[32]; /* f0–f31 */
+ uint32_t fcsr;
+};
+
+/* Vector context (VLEN-agnostic raw blob) */
+struct sbi_domain_vec_state {
+ bool valid;
+ uint64_t v[32 * 2]; /* placeholder blob for v0–v31 */
+ uint64_t vtype;
+ uint64_t vl;
+};
+
+/* Extended domain context */
+struct sbi_domain_extctx {
+ struct sbi_domain_fp_state fp;
+ struct sbi_domain_vec_state vec;
+};
+
/** Representation of OpenSBI domain */
struct sbi_domain {
/** Node in linked list of domains */
diff --git a/include/sbi/sbi_fp.h b/include/sbi/sbi_fp.h
new file mode 100644
index 00000000..b2cb9267
--- /dev/null
+++ b/include/sbi/sbi_fp.h
@@ -0,0 +1,33 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2026 RISCstar Solutions.
+ *
+ * Authors:
+ * Dave Patel <dave.patel at riscstar.com>
+ */
+
+#ifndef __SBI_FP_H__
+#define __SBI_FP_H__
+
+/* Floating-point status field (FS) in sstatus */
+#define SSTATUS_FS_SHIFT 13
+#define SSTATUS_FS_MASK (3UL << SSTATUS_FS_SHIFT)
+
+#define SSTATUS_FS_OFF (0UL << SSTATUS_FS_SHIFT)
+#define SSTATUS_FS_INITIAL (1UL << SSTATUS_FS_SHIFT)
+#define SSTATUS_FS_CLEAN (2UL << SSTATUS_FS_SHIFT)
+#define SSTATUS_FS_DIRTY (3UL << SSTATUS_FS_SHIFT)
+
+struct sbi_fp_context {
+ unsigned long f[32];
+ unsigned long fcsr;
+};
+
+struct sbi_fp_context *sbi_current_fp_context(void);
+void sbi_fp_lazy_restore(struct sbi_fp_context *ctx);
+void sbi_lazy_fp_switch(struct sbi_fp_context *ctx);
+void sbi_fp_save(struct sbi_fp_context *dst);
+void sbi_fp_restore(const struct sbi_fp_context *src);
+
+#endif //__SBI_VECTOR_H__
+
diff --git a/include/sbi/sbi_scratch.h b/include/sbi/sbi_scratch.h
index 58d54628..13e4a294 100644
--- a/include/sbi/sbi_scratch.h
+++ b/include/sbi/sbi_scratch.h
@@ -54,6 +54,8 @@
#ifndef __ASSEMBLER__
#include <sbi/sbi_types.h>
+#include <sbi/sbi_fp.h>
+#include <sbi/sbi_vector.h>
/** Representation of per-HART scratch space */
struct sbi_scratch {
@@ -87,6 +89,9 @@ struct sbi_scratch {
unsigned long options;
/** Index of the hart */
unsigned long hartindex;
+ /* Current FP/Vector context owner per hart */
+ struct sbi_fp_context *fp_ctx;
+ struct sbi_vector_context *vec_ctx;
};
/**
diff --git a/include/sbi/sbi_trap.h b/include/sbi/sbi_trap.h
index 731a0c98..70d8bd2e 100644
--- a/include/sbi/sbi_trap.h
+++ b/include/sbi/sbi_trap.h
@@ -239,6 +239,9 @@ struct sbi_trap_context {
struct sbi_trap_regs regs;
/** Trap details */
struct sbi_trap_info trap;
+ /* FP/Vector ownership pointers for lazy context switching */
+ struct sbi_fp_context *fp_ctx;
+ struct sbi_vector_context *vec_ctx;
/** Pointer to previous trap context */
struct sbi_trap_context *prev_context;
};
diff --git a/include/sbi/sbi_vector.h b/include/sbi/sbi_vector.h
new file mode 100644
index 00000000..06302d70
--- /dev/null
+++ b/include/sbi/sbi_vector.h
@@ -0,0 +1,40 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2026 RISCstar Solutions.
+ *
+ * Authors:
+ * Dave Patel <dave.patel at riscstar.com>
+ */
+
+#ifndef __SBI_VECTOR_H__
+#define __SBI_VECTOR_H__
+
+#include <sbi/sbi_types.h>
+
+#define SBI_MAX_VLENB 256
+
+/* Vector status field (VS) in sstatus */
+#define SSTATUS_VS_SHIFT 9
+#define SSTATUS_VS_MASK (3UL << SSTATUS_VS_SHIFT)
+
+#define SSTATUS_VS_OFF (0UL << SSTATUS_VS_SHIFT)
+#define SSTATUS_VS_INITIAL (1UL << SSTATUS_VS_SHIFT)
+#define SSTATUS_VS_CLEAN (2UL << SSTATUS_VS_SHIFT)
+#define SSTATUS_VS_DIRTY (3UL << SSTATUS_VS_SHIFT)
+
+struct sbi_vector_context {
+ unsigned long vl;
+ unsigned long vtype;
+ unsigned long vcsr;
+
+ /* size depends on VLEN */
+ uint8_t vregs[32 * SBI_MAX_VLENB];
+};
+
+struct sbi_vector_context *sbi_current_vector_context(void);
+void sbi_vector_lazy_restore(struct sbi_vector_context *ctx);
+void sbi_lazy_vec_switch(struct sbi_vector_context *ctx);
+void sbi_vector_save(struct sbi_vector_context *dst);
+void sbi_vector_restore(const struct sbi_vector_context *src);
+#endif //__SBI_VECTOR_H__
+
diff --git a/lib/sbi/objects.mk b/lib/sbi/objects.mk
index ea816e92..ca560c2e 100644
--- a/lib/sbi/objects.mk
+++ b/lib/sbi/objects.mk
@@ -106,3 +106,5 @@ libsbi-objs-y += sbi_trap_v_ldst.o
libsbi-objs-y += sbi_unpriv.o
libsbi-objs-y += sbi_expected_trap.o
libsbi-objs-y += sbi_cppc.o
+libsbi-objs-y += sbi_vector.o
+libsbi-objs-y += sbi_fp.o
diff --git a/lib/sbi/sbi_fp.c b/lib/sbi/sbi_fp.c
new file mode 100644
index 00000000..9ae2917f
--- /dev/null
+++ b/lib/sbi/sbi_fp.c
@@ -0,0 +1,138 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2026 RISCstar Solutions.
+ *
+ * Authors:
+ * Dave Patel <dave.patel at riscstar.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_fp.h>
+
+/* Per-hart current owner pointer (SMP-safe) */
+static inline struct sbi_fp_context **fp_owner_ptr(void)
+{
+ struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+ return &scratch->fp_ctx;
+}
+
+/* Get current FP context */
+struct sbi_fp_context *sbi_current_fp_context(void)
+{
+ return *fp_owner_ptr();
+}
+
+void sbi_fp_save(struct sbi_fp_context *dst)
+{
+ ulong sstatus = csr_read(CSR_SSTATUS);
+ if ((sstatus & SSTATUS_FS) == SSTATUS_FS_OFF)
+ return;
+
+ asm volatile(
+ "fsd f0, 0(%0)\n"
+ "fsd f1, 8(%0)\n"
+ "fsd f2, 16(%0)\n"
+ "fsd f3, 24(%0)\n"
+ "fsd f4, 32(%0)\n"
+ "fsd f5, 40(%0)\n"
+ "fsd f6, 48(%0)\n"
+ "fsd f7, 56(%0)\n"
+ "fsd f8, 64(%0)\n"
+ "fsd f9, 72(%0)\n"
+ "fsd f10, 80(%0)\n"
+ "fsd f11, 88(%0)\n"
+ "fsd f12, 96(%0)\n"
+ "fsd f13, 104(%0)\n"
+ "fsd f14, 112(%0)\n"
+ "fsd f15, 120(%0)\n"
+ "fsd f16, 128(%0)\n"
+ "fsd f17, 136(%0)\n"
+ "fsd f18, 144(%0)\n"
+ "fsd f19, 152(%0)\n"
+ "fsd f20, 160(%0)\n"
+ "fsd f21, 168(%0)\n"
+ "fsd f22, 176(%0)\n"
+ "fsd f23, 184(%0)\n"
+ "fsd f24, 192(%0)\n"
+ "fsd f25, 200(%0)\n"
+ "fsd f26, 208(%0)\n"
+ "fsd f27, 216(%0)\n"
+ "fsd f28, 224(%0)\n"
+ "fsd f29, 232(%0)\n"
+ "fsd f30, 240(%0)\n"
+ "fsd f31, 248(%0)\n"
+ :
+ : "r"(dst->f)
+ : "memory"
+ );
+
+ dst->fcsr = csr_read(CSR_FCSR);
+}
+
+void sbi_fp_restore(const struct sbi_fp_context *src)
+{
+ asm volatile(
+ "fld f0, 0(%0)\n"
+ "fld f1, 8(%0)\n"
+ "fld f2, 16(%0)\n"
+ "fld f3, 24(%0)\n"
+ "fld f4, 32(%0)\n"
+ "fld f5, 40(%0)\n"
+ "fld f6, 48(%0)\n"
+ "fld f7, 56(%0)\n"
+ "fld f8, 64(%0)\n"
+ "fld f9, 72(%0)\n"
+ "fld f10, 80(%0)\n"
+ "fld f11, 88(%0)\n"
+ "fld f12, 96(%0)\n"
+ "fld f13, 104(%0)\n"
+ "fld f14, 112(%0)\n"
+ "fld f15, 120(%0)\n"
+ "fld f16, 128(%0)\n"
+ "fld f17, 136(%0)\n"
+ "fld f18, 144(%0)\n"
+ "fld f19, 152(%0)\n"
+ "fld f20, 160(%0)\n"
+ "fld f21, 168(%0)\n"
+ "fld f22, 176(%0)\n"
+ "fld f23, 184(%0)\n"
+ "fld f24, 192(%0)\n"
+ "fld f25, 200(%0)\n"
+ "fld f26, 208(%0)\n"
+ "fld f27, 216(%0)\n"
+ "fld f28, 224(%0)\n"
+ "fld f29, 232(%0)\n"
+ "fld f30, 240(%0)\n"
+ "fld f31, 248(%0)\n"
+ :
+ : "r"(src->f)
+ : "memory"
+ );
+
+ csr_write(CSR_FCSR, src->fcsr);
+}
+
+void sbi_fp_lazy_restore(struct sbi_fp_context *ctx)
+{
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+ sstatus |= SSTATUS_FS_DIRTY;
+ csr_write(CSR_SSTATUS, sstatus);
+
+ unsigned long mstatus = csr_read(CSR_MSTATUS);
+ mstatus |= SSTATUS_FS_DIRTY;
+ csr_write(CSR_MSTATUS, mstatus);
+}
+
+void sbi_lazy_fp_switch(struct sbi_fp_context *ctx)
+{
+ struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+ scratch->fp_ctx = ctx;
+
+ /* Disable FP unit (FS=OFF) to force trap on next FP instruction */
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+ sstatus &= ~SSTATUS_FS;
+ sstatus |= SSTATUS_FS_OFF;
+ csr_write(CSR_SSTATUS, sstatus);
+}
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 60e95bca..ba7ea74d 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -814,6 +814,11 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
csr_write(CSR_VSIE, 0);
csr_write(CSR_VSATP, 0);
} else {
+ /* Disable FP and vector units for lazy restore */
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+ sstatus &= ~SSTATUS_FS;
+ sstatus &= ~SSTATUS_VS;
+ csr_write(CSR_SSTATUS, sstatus);
csr_write(CSR_STVEC, next_addr);
csr_write(CSR_SSCRATCH, 0);
csr_write(CSR_SIE, 0);
diff --git a/lib/sbi/sbi_illegal_insn.c b/lib/sbi/sbi_illegal_insn.c
index fa82264a..11268251 100644
--- a/lib/sbi/sbi_illegal_insn.c
+++ b/lib/sbi/sbi_illegal_insn.c
@@ -19,6 +19,8 @@
#include <sbi/sbi_trap.h>
#include <sbi/sbi_unpriv.h>
#include <sbi/sbi_console.h>
+#include <sbi/sbi_fp.h>
+#include <sbi/sbi_vector.h>
int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
{
@@ -45,6 +47,24 @@ static int misc_mem_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
return truly_illegal_insn(insn, regs);
}
+static int sbi_fp_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+ struct sbi_fp_context *ctx;
+ ctx = sbi_current_fp_context();
+ sbi_fp_lazy_restore(ctx);
+ regs->mstatus |= SSTATUS_FS_DIRTY;
+ return 0;
+}
+
+static int sbi_vector_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+ struct sbi_vector_context *ctx;
+ ctx = sbi_current_vector_context();
+ sbi_vector_lazy_restore(ctx);
+ regs->mstatus |= SSTATUS_VS_DIRTY;
+ return 0;
+}
+
static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
{
bool do_write = false;
@@ -110,6 +130,42 @@ static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
return 0;
}
+static bool is_fp_insn(uint32_t insn)
+{
+ uint32_t opcode = insn & 0x7f;
+
+ if (opcode == 0x53) /* FP arithmetic */
+ return true;
+
+ if (opcode == 0x07 || opcode == 0x27) {
+ uint32_t funct3 = (insn >> 12) & 7;
+
+ /* FP loads/stores use funct3 = 2 or 3 */
+ if (funct3 == 2 || funct3 == 3)
+ return true;
+ }
+
+ return false;
+}
+
+static bool is_vector_insn(uint32_t insn)
+{
+ uint32_t opcode = insn & 0x7f;
+
+ if (opcode == 0x57) /* vector arithmetic */
+ return true;
+
+ if (opcode == 0x07 || opcode == 0x27) {
+ uint32_t funct3 = (insn >> 12) & 7;
+
+ /* vector memory encodings */
+ if (funct3 >= 5)
+ return true;
+ }
+
+ return false;
+}
+
static const illegal_insn_func illegal_insn_table[32] = {
truly_illegal_insn, /* 0 */
truly_illegal_insn, /* 1 */
@@ -120,7 +176,7 @@ static const illegal_insn_func illegal_insn_table[32] = {
truly_illegal_insn, /* 6 */
truly_illegal_insn, /* 7 */
truly_illegal_insn, /* 8 */
- truly_illegal_insn, /* 9 */
+ truly_illegal_insn, /* 9 */
truly_illegal_insn, /* 10 */
sbi_illegal_atomic, /* 11 */
truly_illegal_insn, /* 12 */
@@ -131,7 +187,7 @@ static const illegal_insn_func illegal_insn_table[32] = {
truly_illegal_insn, /* 17 */
truly_illegal_insn, /* 18 */
truly_illegal_insn, /* 19 */
- truly_illegal_insn, /* 20 */
+ truly_illegal_insn, /* 20 */
truly_illegal_insn, /* 21 */
truly_illegal_insn, /* 22 */
truly_illegal_insn, /* 23 */
@@ -171,5 +227,11 @@ int sbi_illegal_insn_handler(struct sbi_trap_context *tcntx)
return truly_illegal_insn(insn, regs);
}
- return illegal_insn_table[(insn & 0x7c) >> 2](insn, regs);
+ if (is_vector_insn(insn)) {
+ return sbi_vector_insn(insn, regs);
+ } else if (is_fp_insn(insn)) {
+ return sbi_fp_insn(insn, regs);
+ } else {
+ return illegal_insn_table[(insn & 0x7c) >> 2](insn, regs);
+ }
}
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index f41db4d1..b4541db8 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)
@@ -92,6 +94,14 @@ static void __noreturn sbi_trap_error(const char *msg, int rc,
sbi_hart_hang();
}
+void sbi_lazy_context_switch(struct sbi_trap_context *old_ctx,
+ struct sbi_trap_context *new_ctx)
+{
+ /* Lazy FP/Vector switch for new context */
+ sbi_lazy_fp_switch(new_ctx->fp_ctx);
+ sbi_lazy_vec_switch(new_ctx->vec_ctx);
+}
+
/**
* Redirect trap to lower privledge mode (S-mode or U-mode)
*
@@ -309,6 +319,22 @@ struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx)
const struct sbi_trap_info *trap = &tcntx->trap;
struct sbi_trap_regs *regs = &tcntx->regs;
ulong mcause = tcntx->trap.cause;
+ static bool fpflag = false;
+ static bool vecflag = false;
+
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+
+ if ((sstatus & SSTATUS_FS) == SSTATUS_FS_DIRTY) {
+ fpflag = true;
+ struct sbi_fp_context *current = scratch->fp_ctx;
+ sbi_fp_save(current);
+ }
+
+ if ((sstatus & SSTATUS_VS) == SSTATUS_VS_DIRTY) {
+ vecflag = true;
+ struct sbi_vector_context *current = scratch->vec_ctx;
+ sbi_vector_save(current);
+ }
/* Update trap context pointer */
tcntx->prev_context = sbi_trap_get_context(scratch);
@@ -373,5 +399,16 @@ trap_done:
sbi_sse_process_pending_events(regs);
sbi_trap_set_context(scratch, tcntx->prev_context);
+
+ if (fpflag == true) {
+ struct sbi_fp_context *current = scratch->fp_ctx;
+ sbi_fp_restore(current);
+ }
+
+ if (vecflag == true) {
+ struct sbi_vector_context *current = scratch->vec_ctx;
+ sbi_vector_restore(current);
+ }
+
return tcntx;
}
diff --git a/lib/sbi/sbi_vector.c b/lib/sbi/sbi_vector.c
new file mode 100644
index 00000000..42478362
--- /dev/null
+++ b/lib/sbi/sbi_vector.c
@@ -0,0 +1,159 @@
+/* SPDX-License-Identifier: GPL-2.0
+ *
+ * Copyright (c) 2026 RISCstar Solutions.
+ *
+ * Authors:
+ * Dave Patel <dave.patel at riscstar.com>
+ */
+
+#include <sbi/sbi_domain.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_vector.h>
+
+/* Per-hart vector owner */
+static inline struct sbi_vector_context **vec_owner_ptr(void)
+{
+ struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+ return &scratch->vec_ctx;
+}
+
+/* Get current vector context */
+struct sbi_vector_context *sbi_current_vector_context(void)
+{
+ return *vec_owner_ptr();
+}
+
+static inline unsigned long vector_vlenb(void)
+{
+ unsigned long vlenb;
+ asm volatile ("csrr %0, vlenb" : "=r"(vlenb));
+ return vlenb;
+}
+
+
+void sbi_vector_save(struct sbi_vector_context *dst)
+{
+ if (!dst)
+ return;
+
+ uint8_t *base = dst->vregs;
+ unsigned long vlenb = vector_vlenb();
+
+ asm volatile("csrr %0, vl" : "=r"(dst->vl));
+ asm volatile("csrr %0, vtype" : "=r"(dst->vtype));
+ asm volatile("csrr %0, vcsr" : "=r"(dst->vcsr));
+
+#define SAVE_VREG(num) \
+ asm volatile("vse8.v v" #num ", (%0)" :: "r"(base + num * vlenb) : "memory")
+
+ SAVE_VREG(0);
+ SAVE_VREG(1);
+ SAVE_VREG(2);
+ SAVE_VREG(3);
+ SAVE_VREG(4);
+ SAVE_VREG(5);
+ SAVE_VREG(6);
+ SAVE_VREG(7);
+ SAVE_VREG(8);
+ SAVE_VREG(9);
+ SAVE_VREG(10);
+ SAVE_VREG(11);
+ SAVE_VREG(12);
+ SAVE_VREG(13);
+ SAVE_VREG(14);
+ SAVE_VREG(15);
+ SAVE_VREG(16);
+ SAVE_VREG(17);
+ SAVE_VREG(18);
+ SAVE_VREG(19);
+ SAVE_VREG(20);
+ SAVE_VREG(21);
+ SAVE_VREG(22);
+ SAVE_VREG(23);
+ SAVE_VREG(24);
+ SAVE_VREG(25);
+ SAVE_VREG(26);
+ SAVE_VREG(27);
+ SAVE_VREG(28);
+ SAVE_VREG(29);
+ SAVE_VREG(30);
+ SAVE_VREG(31);
+
+#undef SAVE_VREG
+}
+
+void sbi_vector_restore(const struct sbi_vector_context *src)
+{
+ if (!src)
+ return;
+
+ const uint8_t *base = src->vregs;
+ unsigned long vlenb = vector_vlenb();
+
+ asm volatile("csrw vl, %0" :: "r"(src->vl));
+ asm volatile("csrw vtype, %0" :: "r"(src->vtype));
+ asm volatile("csrw vcsr, %0" :: "r"(src->vcsr));
+
+#define RESTORE_VREG(num) \
+ asm volatile("vle8.v v" #num ", (%0)" :: "r"(base + num * vlenb) : "memory")
+
+ RESTORE_VREG(0);
+ RESTORE_VREG(1);
+ RESTORE_VREG(2);
+ RESTORE_VREG(3);
+ RESTORE_VREG(4);
+ RESTORE_VREG(5);
+ RESTORE_VREG(6);
+ RESTORE_VREG(7);
+ RESTORE_VREG(8);
+ RESTORE_VREG(9);
+ RESTORE_VREG(10);
+ RESTORE_VREG(11);
+ RESTORE_VREG(12);
+ RESTORE_VREG(13);
+ RESTORE_VREG(14);
+ RESTORE_VREG(15);
+ RESTORE_VREG(16);
+ RESTORE_VREG(17);
+ RESTORE_VREG(18);
+ RESTORE_VREG(19);
+ RESTORE_VREG(20);
+ RESTORE_VREG(21);
+ RESTORE_VREG(22);
+ RESTORE_VREG(23);
+ RESTORE_VREG(24);
+ RESTORE_VREG(25);
+ RESTORE_VREG(26);
+ RESTORE_VREG(27);
+ RESTORE_VREG(28);
+ RESTORE_VREG(29);
+ RESTORE_VREG(30);
+ RESTORE_VREG(31);
+
+#undef RESTORE_VREG
+}
+
+void sbi_vector_lazy_restore(struct sbi_vector_context *ctx)
+{
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+ sstatus |= SSTATUS_VS_INITIAL;
+ csr_write(CSR_SSTATUS, sstatus);
+
+ unsigned long mstatus = csr_read(CSR_MSTATUS);
+ mstatus |= SSTATUS_VS_INITIAL;
+ csr_write(CSR_MSTATUS, mstatus);
+}
+
+void sbi_lazy_vec_switch(struct sbi_vector_context *ctx)
+{
+ struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+ scratch->vec_ctx = ctx;
+
+ /* Disable Vector unit (VS=OFF) to force trap on next vector instruction */
+ unsigned long sstatus = csr_read(CSR_SSTATUS);
+ sstatus &= ~SSTATUS_VS;
+ sstatus |= SSTATUS_VS_OFF;
+ csr_write(CSR_SSTATUS, sstatus);
+}
+
diff --git a/lib/utils/irqchip/aplic.c b/lib/utils/irqchip/aplic.c
index 36e40245..4d0329fb 100644
--- a/lib/utils/irqchip/aplic.c
+++ b/lib/utils/irqchip/aplic.c
@@ -395,6 +395,7 @@ static void aplic_hwirq_eoi(struct sbi_irqchip_device *chip, u32 hwirq)
writel(hwirq, idc + APLIC_CLRIPNUM);
}
+#ifdef APLIC_QEMU_VIRQ_TEST
static void aplic_set_target(struct sbi_irqchip_device *chip, u32 hwirq,
u32 hart_idx)
{
@@ -411,6 +412,7 @@ static void aplic_set_target(struct sbi_irqchip_device *chip, u32 hwirq,
writel(APLIC_ENABLE_IDELIVERY, (void *)(idc + APLIC_IDC_IDELIVERY));
writel(APLIC_ENABLE_ITHRESHOLD, (void *)(idc + APLIC_IDC_ITHRESHOLD));
}
+#endif
static int aplic_hwirq_setup(struct sbi_irqchip_device *chip, u32 hwirq)
{
--
2.43.0
More information about the opensbi
mailing list