[PATCH v3 02/17] riscv: sbi: add FWFT extension interface

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

This SBI extensions enables supervisor mode to control feature that are
under M-mode control (For instance, Svadu menvcfg ADUE bit, Ssdbltrp
DTE, etc).

Signed-off-by: Clément Léger <cleger@rivosinc.com>
---
 arch/riscv/include/asm/sbi.h |  5 ++
 arch/riscv/kernel/sbi.c      | 97 ++++++++++++++++++++++++++++++++++++
 2 files changed, 102 insertions(+)

diff --git a/arch/riscv/include/asm/sbi.h b/arch/riscv/include/asm/sbi.h
index bb077d0c912f..fc87c609c11a 100644
--- a/arch/riscv/include/asm/sbi.h
+++ b/arch/riscv/include/asm/sbi.h
@@ -503,6 +503,11 @@ int sbi_remote_hfence_vvma_asid(const struct cpumask *cpu_mask,
 				unsigned long asid);
 long sbi_probe_extension(int ext);
 
+int sbi_fwft_all_cpus_set(u32 feature, unsigned long value, unsigned long flags,
+			  bool revert_on_failure);
+int sbi_fwft_get(u32 feature, unsigned long *value);
+int sbi_fwft_set(u32 feature, unsigned long value, unsigned long flags);
+
 /* Check if current SBI specification version is 0.1 or not */
 static inline int sbi_spec_is_0_1(void)
 {
diff --git a/arch/riscv/kernel/sbi.c b/arch/riscv/kernel/sbi.c
index 1989b8cade1b..256910db1307 100644
--- a/arch/riscv/kernel/sbi.c
+++ b/arch/riscv/kernel/sbi.c
@@ -299,6 +299,103 @@ static int __sbi_rfence_v02(int fid, const struct cpumask *cpu_mask,
 	return 0;
 }
 
+int sbi_fwft_get(u32 feature, unsigned long *value)
+{
+	return -EOPNOTSUPP;
+}
+
+/**
+ * sbi_fwft_set() - Set a feature on all online cpus
+ * @feature: The feature to be set
+ * @value: The feature value to be set
+ * @flags: FWFT feature set flags
+ *
+ * Return: 0 on success, appropriate linux error code otherwise.
+ */
+int sbi_fwft_set(u32 feature, unsigned long value, unsigned long flags)
+{
+	return -EOPNOTSUPP;
+}
+
+struct fwft_set_req {
+	u32 feature;
+	unsigned long value;
+	unsigned long flags;
+	cpumask_t mask;
+};
+
+static void cpu_sbi_fwft_set(void *arg)
+{
+	struct fwft_set_req *req = arg;
+
+	if (sbi_fwft_set(req->feature, req->value, req->flags))
+		cpumask_clear_cpu(smp_processor_id(), &req->mask);
+}
+
+static int sbi_fwft_feature_local_set(u32 feature, unsigned long value,
+				      unsigned long flags,
+				      bool revert_on_fail)
+{
+	int ret;
+	unsigned long prev_value;
+	cpumask_t tmp;
+	struct fwft_set_req req = {
+		.feature = feature,
+		.value = value,
+		.flags = flags,
+	};
+
+	cpumask_copy(&req.mask, cpu_online_mask);
+
+	/* We can not revert if features are locked */
+	if (revert_on_fail && flags & SBI_FWFT_SET_FLAG_LOCK)
+		return -EINVAL;
+
+	/* Reset value is the same for all cpus, read it once. */
+	ret = sbi_fwft_get(feature, &prev_value);
+	if (ret)
+		return ret;
+
+	/* Feature might already be set to the value we want */
+	if (prev_value == value)
+		return 0;
+
+	on_each_cpu_mask(&req.mask, cpu_sbi_fwft_set, &req, 1);
+	if (cpumask_equal(&req.mask, cpu_online_mask))
+		return 0;
+
+	pr_err("Failed to set feature %x for all online cpus, reverting\n",
+	       feature);
+
+	req.value = prev_value;
+	cpumask_copy(&tmp, &req.mask);
+	on_each_cpu_mask(&req.mask, cpu_sbi_fwft_set, &req, 1);
+	if (cpumask_equal(&req.mask, &tmp))
+		return 0;
+
+	return -EINVAL;
+}
+
+/**
+ * sbi_fwft_all_cpus_set() - Set a feature on all online cpus
+ * @feature: The feature to be set
+ * @value: The feature value to be set
+ * @flags: FWFT feature set flags
+ * @revert_on_fail: true if feature value should be restored to it's orignal
+ * 		    value on failure.
+ *
+ * Return: 0 on success, appropriate linux error code otherwise.
+ */
+int sbi_fwft_all_cpus_set(u32 feature, unsigned long value, unsigned long flags,
+			  bool revert_on_fail)
+{
+	if (feature & SBI_FWFT_GLOBAL_FEATURE_BIT)
+		return sbi_fwft_set(feature, value, flags);
+
+	return sbi_fwft_feature_local_set(feature, value, flags,
+					  revert_on_fail);
+}
+
 /**
  * sbi_set_timer() - Program the timer for next timer event.
  * @stime_value: The value after which next timer event should fire.
-- 
2.47.2