[PATCH -fixes 2/2] riscv: Save/restore envcfg CSR during CPU suspend

[Samuel Holland <samuel.holland@sifive.com>]

The value of the [ms]envcfg CSR is lost when entering a nonretentive
idle state, so the CSR must be rewritten when resuming the CPU.

Because the [ms]envcfg CSR is part of the base RISC-V privileged ISA
specification, it cannot be detected from the ISA string. However, most
existing hardware is too old to implement this CSR. As a result, it must
be probed at runtime.

Extend the logic for the Zicsr ISA extension to probe for the presence
of specific CSRs. Since the CSR number is encoded as an immediate value
within the csrr instruction, a switch case is necessary for any CSR that
must be probed this way. Use the exception table to handle the illegal
instruction exception raised when the CSR is not implemented.

Cc: stable@kernel.org
Fixes: 43c16d51a19b ("RISC-V: Enable cbo.zero in usermode")
Signed-off-by: Samuel Holland <samuel.holland@sifive.com>
---

 arch/riscv/include/asm/csr.h     | 23 +++++++++++++++++++++++
 arch/riscv/include/asm/suspend.h |  1 +
 arch/riscv/kernel/cpufeature.c   | 23 +++++++++++++++++++++++
 arch/riscv/kernel/suspend.c      |  2 ++
 4 files changed, 49 insertions(+)

diff --git a/arch/riscv/include/asm/csr.h b/arch/riscv/include/asm/csr.h
index 2468c55933cd..daff95feb817 100644
--- a/arch/riscv/include/asm/csr.h
+++ b/arch/riscv/include/asm/csr.h
@@ -542,6 +542,29 @@
 			      : "memory");			\
 })
 
+#define ALT_CSR_READ(csr)					\
+({								\
+	unsigned long __v;					\
+	__asm__ __volatile__ (					\
+		ALTERNATIVE("li %[v], 0", "csrr %[v], %[r]", 0,	\
+			    csr << 16 | RISCV_ISA_EXT_ZICSR, 1)	\
+		: [v] "=r" (__v)				\
+		: [r] "i" (csr)					\
+		: "memory");					\
+	__v;							\
+})
+
+#define ALT_CSR_WRITE(csr, val)					\
+({								\
+	unsigned long __v = (unsigned long)(val);		\
+	__asm__ __volatile__ (					\
+		ALTERNATIVE("nop", "csrw %[r], %[v]", 0,	\
+			    csr << 16 | RISCV_ISA_EXT_ZICSR, 1)	\
+		: : [r] "i" (csr), [v] "rK" (__v)		\
+		: "memory");					\
+	__v;							\
+})
+
 #endif /* __ASSEMBLY__ */
 
 #endif /* _ASM_RISCV_CSR_H */
diff --git a/arch/riscv/include/asm/suspend.h b/arch/riscv/include/asm/suspend.h
index 02f87867389a..491296a335d0 100644
--- a/arch/riscv/include/asm/suspend.h
+++ b/arch/riscv/include/asm/suspend.h
@@ -14,6 +14,7 @@ struct suspend_context {
 	struct pt_regs regs;
 	/* Saved and restored by high-level functions */
 	unsigned long scratch;
+	unsigned long envcfg;
 	unsigned long tvec;
 	unsigned long ie;
 #ifdef CONFIG_MMU
diff --git a/arch/riscv/kernel/cpufeature.c b/arch/riscv/kernel/cpufeature.c
index c5b13f7dd482..934090270ae5 100644
--- a/arch/riscv/kernel/cpufeature.c
+++ b/arch/riscv/kernel/cpufeature.c
@@ -954,6 +954,27 @@ void riscv_user_isa_enable(void)
 }
 
 #ifdef CONFIG_RISCV_ALTERNATIVE
+static bool riscv_cpufeature_probe_csr(u16 csr)
+{
+	bool ret = false;
+
+	switch (csr) {
+#define PROBE_CSR_CASE(_csr)			\
+	case _csr:				\
+		asm("1:	csrr zero, %[csr]\n"	\
+		    "	li %[r], 1\n"		\
+		    "2:\n"			\
+		    _ASM_EXTABLE(1b, 2b)	\
+			: [r] "+r" (ret)	\
+			: [csr] "i" (_csr));	\
+		break
+	PROBE_CSR_CASE(CSR_ENVCFG);
+#undef PROBE_CSR_CASE
+	}
+
+	return ret;
+}
+
 /*
  * Alternative patch sites consider 48 bits when determining when to patch
  * the old instruction sequence with the new. These bits are broken into a
@@ -974,6 +995,8 @@ static bool riscv_cpufeature_patch_check(u16 id, u16 value)
 		return true;
 
 	switch (id) {
+	case RISCV_ISA_EXT_ZICSR:
+		return riscv_cpufeature_probe_csr(value);
 	case RISCV_ISA_EXT_ZICBOZ:
 		/*
 		 * Zicboz alternative applications provide the maximum
diff --git a/arch/riscv/kernel/suspend.c b/arch/riscv/kernel/suspend.c
index 239509367e42..fe544f12a5c5 100644
--- a/arch/riscv/kernel/suspend.c
+++ b/arch/riscv/kernel/suspend.c
@@ -15,6 +15,7 @@
 void suspend_save_csrs(struct suspend_context *context)
 {
 	context->scratch = csr_read(CSR_SCRATCH);
+	context->envcfg = ALT_CSR_READ(CSR_ENVCFG);
 	context->tvec = csr_read(CSR_TVEC);
 	context->ie = csr_read(CSR_IE);
 
@@ -36,6 +37,7 @@ void suspend_save_csrs(struct suspend_context *context)
 void suspend_restore_csrs(struct suspend_context *context)
 {
 	csr_write(CSR_SCRATCH, context->scratch);
+	ALT_CSR_WRITE(CSR_ENVCFG, context->envcfg);
 	csr_write(CSR_TVEC, context->tvec);
 	csr_write(CSR_IE, context->ie);
 
-- 
2.43.0