[RFC PATCH 5/7] riscv: add double trap driver

["Clément Léger" <cleger@rivosinc.com>]

Add a small driver to request double trap enabling as well as
registering a SSE handler for double trap. This will also be used by KVM
SBI FWFT extension support to detect if it is possible to enable double
trap in VS-mode.

Signed-off-by: Clément Léger <cleger@rivosinc.com>
---
 arch/riscv/include/asm/sbi.h    |  1 +
 drivers/firmware/Kconfig        |  7 +++
 drivers/firmware/Makefile       |  1 +
 drivers/firmware/riscv_dbltrp.c | 95 +++++++++++++++++++++++++++++++++
 include/linux/riscv_dbltrp.h    | 19 +++++++
 5 files changed, 123 insertions(+)
 create mode 100644 drivers/firmware/riscv_dbltrp.c
 create mode 100644 include/linux/riscv_dbltrp.h

diff --git a/arch/riscv/include/asm/sbi.h b/arch/riscv/include/asm/sbi.h
index 744aa1796c92..9cd4ca66487c 100644
--- a/arch/riscv/include/asm/sbi.h
+++ b/arch/riscv/include/asm/sbi.h
@@ -314,6 +314,7 @@ enum sbi_sse_attr_id {
 #define SBI_SSE_ATTR_INTERRUPTED_FLAGS_SPIE	(1 << 2)
 
 #define SBI_SSE_EVENT_LOCAL_RAS		0x00000000
+#define SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP	0x00000001
 #define SBI_SSE_EVENT_GLOBAL_RAS	0x00008000
 #define SBI_SSE_EVENT_LOCAL_PMU		0x00010000
 #define SBI_SSE_EVENT_LOCAL_SOFTWARE	0xffff0000
diff --git a/drivers/firmware/Kconfig b/drivers/firmware/Kconfig
index 59f611288807..a037f6e89942 100644
--- a/drivers/firmware/Kconfig
+++ b/drivers/firmware/Kconfig
@@ -197,6 +197,13 @@ config RISCV_SSE_TEST
 	  Select if you want to enable SSE extension testing at boot time.
 	  This will run a series of test which verifies SSE sanity.
 
+config RISCV_DBLTRP
+	bool "Enable Double trap handling"
+	depends on RISCV_SSE && RISCV_SBI
+	default n
+	help
+	  Select if you want to enable SSE double trap handler.
+
 config SYSFB
 	bool
 	select BOOT_VESA_SUPPORT
diff --git a/drivers/firmware/Makefile b/drivers/firmware/Makefile
index fb7b0c08c56d..ad67a1738c0f 100644
--- a/drivers/firmware/Makefile
+++ b/drivers/firmware/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_RASPBERRYPI_FIRMWARE) += raspberrypi.o
 obj-$(CONFIG_FW_CFG_SYSFS)	+= qemu_fw_cfg.o
 obj-$(CONFIG_RISCV_SSE)		+= riscv_sse.o
 obj-$(CONFIG_RISCV_SSE_TEST)	+= riscv_sse_test.o
+obj-$(CONFIG_RISCV_DBLTRP)	+= riscv_dbltrp.o
 obj-$(CONFIG_SYSFB)		+= sysfb.o
 obj-$(CONFIG_SYSFB_SIMPLEFB)	+= sysfb_simplefb.o
 obj-$(CONFIG_TI_SCI_PROTOCOL)	+= ti_sci.o
diff --git a/drivers/firmware/riscv_dbltrp.c b/drivers/firmware/riscv_dbltrp.c
new file mode 100644
index 000000000000..72f9a067e87a
--- /dev/null
+++ b/drivers/firmware/riscv_dbltrp.c
@@ -0,0 +1,95 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2023 Rivos Inc.
+ */
+
+#define pr_fmt(fmt) "riscv-dbltrp: " fmt
+
+#include <linux/cpu.h>
+#include <linux/init.h>
+#include <linux/riscv_dbltrp.h>
+#include <linux/riscv_sse.h>
+
+#include <asm/sbi.h>
+
+static bool double_trap_enabled;
+
+static int riscv_sse_dbltrp_handle(uint32_t evt, void *arg,
+				   struct pt_regs *regs)
+{
+	__show_regs(regs);
+	panic("Double trap !\n");
+
+	return 0;
+}
+
+struct cpu_dbltrp_data {
+	int error;
+};
+
+static void
+sbi_cpu_enable_double_trap(void *data)
+{
+	struct sbiret ret;
+	struct cpu_dbltrp_data *cdd = data;
+
+	ret = sbi_ecall(SBI_EXT_FWFT, SBI_EXT_FWFT_SET,
+			SBI_FWFT_DOUBLE_TRAP_ENABLE, 1, 0, 0, 0, 0);
+
+	if (ret.error) {
+		cdd->error = 1;
+		pr_err("Failed to enable double trap on cpu %d\n", smp_processor_id());
+	}
+}
+
+static int sbi_enable_double_trap(void)
+{
+	struct cpu_dbltrp_data cdd = {0};
+
+	on_each_cpu(sbi_cpu_enable_double_trap, &cdd, 1);
+	if (cdd.error)
+		return -1;
+
+	double_trap_enabled = true;
+
+	return 0;
+}
+
+bool riscv_double_trap_enabled(void)
+{
+	return double_trap_enabled;
+}
+EXPORT_SYMBOL(riscv_double_trap_enabled);
+
+static int __init riscv_dbltrp(void)
+{
+	struct sse_event *evt;
+
+	if (!riscv_has_extension_unlikely(RISCV_ISA_EXT_SSDBLTRP)) {
+		pr_err("Ssdbltrp extension not available\n");
+		return 1;
+	}
+
+	if (!sbi_probe_extension(SBI_EXT_FWFT)) {
+		pr_err("Can not enable double trap, SBI_EXT_FWFT is not available\n");
+		return 1;
+	}
+
+	if (sbi_enable_double_trap()) {
+		pr_err("Failed to enable double trap on all cpus\n");
+		return 1;
+	}
+
+	evt = sse_event_register(SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP, 0,
+				 riscv_sse_dbltrp_handle, NULL);
+	if (IS_ERR(evt)) {
+		pr_err("SSE double trap register failed\n");
+		return PTR_ERR(evt);
+	}
+
+	sse_event_enable(evt);
+	pr_info("Double trap handling registered\n");
+
+	return 0;
+}
+device_initcall(riscv_dbltrp);
diff --git a/include/linux/riscv_dbltrp.h b/include/linux/riscv_dbltrp.h
new file mode 100644
index 000000000000..6de4f43fae6b
--- /dev/null
+++ b/include/linux/riscv_dbltrp.h
@@ -0,0 +1,19 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2023 Rivos Inc.
+ */
+
+#ifndef __LINUX_RISCV_DBLTRP_H
+#define __LINUX_RISCV_DBLTRP_H
+
+#if defined(CONFIG_RISCV_DBLTRP)
+bool riscv_double_trap_enabled(void);
+#else
+
+static inline bool riscv_double_trap_enabled(void)
+{
+	return false;
+}
+#endif
+
+#endif /* __LINUX_RISCV_DBLTRP_H */
-- 
2.43.0