[RFC PATCH 6/7] riscv: kvm: add SBI FWFT support for SBI_FWFT_DOUBLE_TRAP_ENABLE

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

Add support in KVM SBI FWFT extension to allow VS-mode to request double
trap enabling. Double traps can then be generated by VS-mode, allowing
M-mode to redirect them to S-mode.

Signed-off-by: Clément Léger <cleger@rivosinc.com>
---
 arch/riscv/include/asm/csr.h               |  1 +
 arch/riscv/include/asm/kvm_vcpu_sbi_fwft.h |  2 +-
 arch/riscv/kvm/vcpu_sbi_fwft.c             | 41 ++++++++++++++++++++++
 3 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/arch/riscv/include/asm/csr.h b/arch/riscv/include/asm/csr.h
index 905cdf894a57..ee1b73655bec 100644
--- a/arch/riscv/include/asm/csr.h
+++ b/arch/riscv/include/asm/csr.h
@@ -196,6 +196,7 @@
 /* xENVCFG flags */
 #define ENVCFG_STCE			(_AC(1, ULL) << 63)
 #define ENVCFG_PBMTE			(_AC(1, ULL) << 62)
+#define ENVCFG_DTE			(_AC(1, ULL) << 59)
 #define ENVCFG_CBZE			(_AC(1, UL) << 7)
 #define ENVCFG_CBCFE			(_AC(1, UL) << 6)
 #define ENVCFG_CBIE_SHIFT		4
diff --git a/arch/riscv/include/asm/kvm_vcpu_sbi_fwft.h b/arch/riscv/include/asm/kvm_vcpu_sbi_fwft.h
index 7dc1b80c7e6c..a9e20d655126 100644
--- a/arch/riscv/include/asm/kvm_vcpu_sbi_fwft.h
+++ b/arch/riscv/include/asm/kvm_vcpu_sbi_fwft.h
@@ -11,7 +11,7 @@
 
 #include <asm/sbi.h>
 
-#define KVM_SBI_FWFT_FEATURE_COUNT	1
+#define KVM_SBI_FWFT_FEATURE_COUNT	2
 
 struct kvm_sbi_fwft_config;
 struct kvm_vcpu;
diff --git a/arch/riscv/kvm/vcpu_sbi_fwft.c b/arch/riscv/kvm/vcpu_sbi_fwft.c
index b9b7f8fa6d22..9e8e397eb02f 100644
--- a/arch/riscv/kvm/vcpu_sbi_fwft.c
+++ b/arch/riscv/kvm/vcpu_sbi_fwft.c
@@ -9,10 +9,19 @@
 #include <linux/errno.h>
 #include <linux/err.h>
 #include <linux/kvm_host.h>
+#include <linux/riscv_dbltrp.h>
 #include <asm/sbi.h>
 #include <asm/kvm_vcpu_sbi.h>
 #include <asm/kvm_vcpu_sbi_fwft.h>
 
+#ifdef CONFIG_32BIT
+# define CSR_HENVCFG_DBLTRP	CSR_HENVCFGH
+# define DBLTRP_DTE	(ENVCFG_DTE >> 32)
+#else
+# define CSR_HENVCFG_DBLTRP	CSR_HENVCFG
+# define DBLTRP_DTE	ENVCFG_DTE
+#endif
+
 #define MIS_DELEG (1UL << EXC_LOAD_MISALIGNED | 1UL << EXC_STORE_MISALIGNED)
 
 static int kvm_sbi_fwft_set_misaligned_delegation(struct kvm_vcpu *vcpu,
@@ -36,6 +45,33 @@ static int kvm_sbi_fwft_get_misaligned_delegation(struct kvm_vcpu *vcpu,
 	return SBI_SUCCESS;
 }
 
+static int kvm_sbi_fwft_set_double_trap(struct kvm_vcpu *vcpu,
+					struct kvm_sbi_fwft_config *conf,
+					unsigned long value)
+{
+	if (!riscv_double_trap_enabled())
+		return SBI_ERR_NOT_SUPPORTED;
+
+	if (value)
+		csr_set(CSR_HENVCFG_DBLTRP, DBLTRP_DTE);
+	else
+		csr_clear(CSR_HENVCFG_DBLTRP, DBLTRP_DTE);
+
+	return SBI_SUCCESS;
+}
+
+static int kvm_sbi_fwft_get_double_trap(struct kvm_vcpu *vcpu,
+					struct kvm_sbi_fwft_config *conf,
+					unsigned long *value)
+{
+	if (!riscv_double_trap_enabled())
+		return SBI_ERR_NOT_SUPPORTED;
+
+	*value = (csr_read(CSR_HENVCFG_DBLTRP) & DBLTRP_DTE) != 0;
+
+	return SBI_SUCCESS;
+}
+
 static struct kvm_sbi_fwft_config *
 kvm_sbi_fwft_get_config(struct kvm_vcpu *vcpu, enum sbi_fwft_feature_t feature)
 {
@@ -111,6 +147,11 @@ static const struct kvm_sbi_fwft_feature features[] = {
 		.id = SBI_FWFT_MISALIGNED_DELEG,
 		.set = kvm_sbi_fwft_set_misaligned_delegation,
 		.get = kvm_sbi_fwft_get_misaligned_delegation,
+	},
+	{
+		.id = SBI_FWFT_DOUBLE_TRAP_ENABLE,
+		.set = kvm_sbi_fwft_set_double_trap,
+		.get = kvm_sbi_fwft_get_double_trap,
 	}
 };
 
-- 
2.43.0