From 24c920ff548e889e207587fdfa4e536afdfb55dc Mon Sep 17 00:00:00 2001
From: Heiko Stuebner <heiko@sntech.de>
Date: Sun, 5 Dec 2021 12:01:49 +0100
Subject: [PATCH 2/4] riscv: add cpufeature handling via alternatives

Some cpufeatures should be handled via the alternatives mechanism
to not incur penalties on unsupporting variants.

So add a mechanism to handle these similar to cpu erratas.

Signed-off-by: Heiko Stuebner <heiko@sntech.de>
---
 arch/riscv/errata/Makefile           |  1 +
 arch/riscv/errata/alternative.c      |  3 ++
 arch/riscv/errata/cpufeatures.c      | 55 ++++++++++++++++++++++++++++
 arch/riscv/include/asm/alternative.h |  1 +
 arch/riscv/include/asm/errata_list.h |  2 +
 5 files changed, 62 insertions(+)
 create mode 100644 arch/riscv/errata/cpufeatures.c

diff --git a/arch/riscv/errata/Makefile b/arch/riscv/errata/Makefile
index b8f8740a3e44..c9ba91052643 100644
--- a/arch/riscv/errata/Makefile
+++ b/arch/riscv/errata/Makefile
@@ -1,2 +1,3 @@
 obj-y	+= alternative.o
+obj-y	+= cpufeatures.o
 obj-$(CONFIG_ERRATA_SIFIVE) += sifive/
diff --git a/arch/riscv/errata/alternative.c b/arch/riscv/errata/alternative.c
index 3b15885db70b..4b45731efe64 100644
--- a/arch/riscv/errata/alternative.c
+++ b/arch/riscv/errata/alternative.c
@@ -64,6 +64,9 @@ void __init apply_boot_alternatives(void)
 
 	init_alternative();
 
+	riscv_apply_cpufeatures((struct alt_entry *)__alt_start,
+				(struct alt_entry *)__alt_end);
+
 	if (!vendor_patch_func)
 		return;
 
diff --git a/arch/riscv/errata/cpufeatures.c b/arch/riscv/errata/cpufeatures.c
new file mode 100644
index 000000000000..99b7c5fe8dd4
--- /dev/null
+++ b/arch/riscv/errata/cpufeatures.c
@@ -0,0 +1,55 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * cpufeatures runtime patching
+ *
+ * Copyright (C) 2021 ?????
+ */
+
+#include <linux/init.h>
+#include <linux/of.h>
+#include <asm/alternative.h>
+#include <asm/errata_list.h>
+#include <asm/patch.h>
+
+struct cpufeature_info_t {
+	char name[ERRATA_STRING_LENGTH_MAX];
+	bool (*check_func)(void);
+};
+
+static struct cpufeature_info_t cpufeature_list[CPUFEATURE_NUMBER] = {
+};
+
+static u32 __init cpufeature_probe(void)
+{
+	u32 cpu_req_feature = 0;
+	int idx;
+
+	for (idx = 0; idx < CPUFEATURE_NUMBER; idx++)
+		if (cpufeature_list[idx].check_func())
+			cpu_req_feature |= (1U << idx);
+
+	return cpu_req_feature;
+}
+
+void riscv_apply_cpufeatures(struct alt_entry *begin, struct alt_entry *end)
+{
+	u32 cpu_req_feature = cpufeature_probe();
+	u32 cpu_apply_feature = 0;
+	struct alt_entry *alt;
+	u32 tmp;
+
+	for (alt = begin; alt < end; alt++) {
+		if (alt->vendor_id != 0)
+			continue;
+		if (alt->errata_id >= CPUFEATURE_NUMBER) {
+			WARN(1, "This feature id:%d is not in kernel cpufeature list", alt->errata_id);
+			continue;
+		}
+
+		tmp = (1U << alt->errata_id);
+		if (cpu_req_feature & tmp) {
+			patch_text_nosync(alt->old_ptr, alt->alt_ptr, alt->alt_len);
+			cpu_apply_feature |= tmp;
+		}
+	}
+}
diff --git a/arch/riscv/include/asm/alternative.h b/arch/riscv/include/asm/alternative.h
index e625d3cafbed..fa0280cd8340 100644
--- a/arch/riscv/include/asm/alternative.h
+++ b/arch/riscv/include/asm/alternative.h
@@ -35,5 +35,6 @@ struct errata_checkfunc_id {
 void sifive_errata_patch_func(struct alt_entry *begin, struct alt_entry *end,
 			      unsigned long archid, unsigned long impid);
 
+void riscv_apply_cpufeatures(struct alt_entry *begin, struct alt_entry *end);
 #endif
 #endif
diff --git a/arch/riscv/include/asm/errata_list.h b/arch/riscv/include/asm/errata_list.h
index 5f1046e82d9f..6b95bd9aee82 100644
--- a/arch/riscv/include/asm/errata_list.h
+++ b/arch/riscv/include/asm/errata_list.h
@@ -14,6 +14,8 @@
 #define	ERRATA_SIFIVE_NUMBER 2
 #endif
 
+#define	CPUFEATURE_NUMBER 0
+
 #ifdef __ASSEMBLY__
 
 #define ALT_INSN_FAULT(x)						\
-- 
2.30.2