lib: sbi: add Ssdbltrp ISA extension support

Add Ssdbltrp trap handler support for S-mode double trap handling. If
the trap is received while in VS-mode, then the trap is redirected to
S-mode. If caught while in HS-mode, then an error is returned to the top
trap handler which will panic.

Signed-off-by: Clément Léger <[email protected]>
Reviewed-by: Samuel Holland <[email protected]>

4 files changed, 39 insertions, 0 deletions

diff --git a/lib/sbi/objects.mk b/lib/sbi/objects.mk
index 0b114bbd..a6f7c5fa 100644
--- a/lib/sbi/objects.mk
+++ b/lib/sbi/objects.mk
@@ -67,6 +67,7 @@ libsbi-objs-y += sbi_console.o
 libsbi-objs-y += sbi_domain_context.o
 libsbi-objs-y += sbi_domain_data.o
 libsbi-objs-y += sbi_domain.o
+libsbi-objs-y += sbi_double_trap.o
 libsbi-objs-y += sbi_emulate_csr.o
 libsbi-objs-y += sbi_fifo.o
 libsbi-objs-y += sbi_fwft.o
diff --git a/lib/sbi/sbi_double_trap.c b/lib/sbi/sbi_double_trap.c
new file mode 100644
index 00000000..96f2fa55
--- /dev/null
+++ b/lib/sbi/sbi_double_trap.c
@@ -0,0 +1,30 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2024 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <[email protected]>
+ */
+
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_sse.h>
+#include <sbi/sbi_trap.h>
+
+int sbi_double_trap_handler(struct sbi_trap_context *tcntx)
+{
+	struct sbi_trap_regs *regs = &tcntx->regs;
+	const struct sbi_trap_info *trap = &tcntx->trap;
+	bool prev_virt = sbi_regs_from_virt(regs);
+
+	if (sbi_mstatus_prev_mode(regs->mstatus) != PRV_S)
+		return SBI_ERR_INVALID_PARAM;
+
+	/* Exception was taken in VS-mode, redirect it to S-mode */
+	if (prev_virt)
+		return sbi_trap_redirect(regs, trap);
+
+	return SBI_ENOTSUPP;
+}
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index d9aef31a..dc0863a3 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -117,6 +117,9 @@ static void mstatus_init(struct sbi_scratch *scratch)
 		menvcfg_val |= ((uint64_t)csr_read(CSR_MENVCFGH)) << 32;
 #endif
 
+		/* Disable double trap by default */
+		menvcfg_val &= ~ENVCFG_DTE;
+
 #define __set_menvcfg_ext(__ext, __bits)				\
 		if (sbi_hart_has_extension(scratch, __ext))		\
 			menvcfg_val |= __bits;
@@ -684,6 +687,7 @@ const struct sbi_hart_ext_data sbi_hart_ext[] = {
 	__SBI_HART_EXT_DATA(smnpm, SBI_HART_EXT_SMNPM),
 	__SBI_HART_EXT_DATA(zicfilp, SBI_HART_EXT_ZICFILP),
 	__SBI_HART_EXT_DATA(zicfiss, SBI_HART_EXT_ZICFISS),
+	__SBI_HART_EXT_DATA(ssdbltrp, SBI_HART_EXT_SSDBLTRP),
 };
 
 _Static_assert(SBI_HART_EXT_MAX == array_size(sbi_hart_ext),
diff --git a/lib/sbi/sbi_trap.c b/lib/sbi/sbi_trap.c
index fd70e67a..d2de0c8f 100644
--- a/lib/sbi/sbi_trap.c
+++ b/lib/sbi/sbi_trap.c
@@ -350,6 +350,10 @@ struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx)
 		rc  = sbi_store_access_handler(tcntx);
 		msg = "store fault handler failed";
 		break;
+	case CAUSE_DOUBLE_TRAP:
+		rc  = sbi_double_trap_handler(tcntx);
+		msg = "double trap handler failed";
+		break;
 	default:
 		/* If the trap came from S or U mode, redirect it there */
 		msg = "trap redirect failed";