[PATCH 2/2] riscv: andes: Add Smstateen extension support

[Leo Yu-Chi Liang]

Use the CSR detection mechanism to probe for the Smstateen extension
and initialize mstateen0/sstateen0 CSRs when present.

The mstateen0 register is configured to allow S-mode access to fcsr,
AIA, IMSIC, indirect CSR access, environment configuration, and
sstateen0. The sstateen0 register enables U-mode access to compressed
extension state and fcsr.

Supports both RV32 (using MSTATEEN0/MSTATEEN0H split) and RV64.

Signed-off-by: Leo Yu-Chi Liang <ycliang at andestech.com>
---
 arch/riscv/cpu/andes/cpu.c   | 29 +++++++++++++++++++++++++++++
 arch/riscv/include/asm/csr.h | 15 +++++++++++++++
 2 files changed, 44 insertions(+)

diff --git a/arch/riscv/cpu/andes/cpu.c b/arch/riscv/cpu/andes/cpu.c
index d25ecba0e88..feb755a4f0d 100644
--- a/arch/riscv/cpu/andes/cpu.c
+++ b/arch/riscv/cpu/andes/cpu.c
@@ -9,6 +9,7 @@
 #include <irq_func.h>
 #include <asm/cache.h>
 #include <asm/csr.h>
+#include <asm/csr_detect.h>
 #include <asm/arch-andes/csr.h>
 
 /*
@@ -60,5 +61,33 @@ void harts_early_init(void)
 		mmisc_ctl_val |= MMISC_CTL_NON_BLOCKING_EN;
 
 		csr_write(CSR_MMISC_CTL, mmisc_ctl_val);
+
+		/* Initialize Smstateen if the extension is present */
+		{
+			int trap = 0;
+			unsigned long long mstateen0;
+
+			mstateen0 = csr_read_allowed(CSR_MSTATEEN0, trap);
+			if (!trap) {
+#if __riscv_xlen == 32
+				mstateen0 |= (unsigned long long)
+					csr_read(CSR_MSTATEEN0H) << 32;
+#endif
+				mstateen0 |= SMSTATEEN0_CS | SMSTATEEN0_FCSR |
+					     SMSTATEEN0_CONTEXT |
+					     SMSTATEEN0_IMSIC |
+					     SMSTATEEN0_AIA |
+					     SMSTATEEN0_CSRIND |
+					     SMSTATEEN0_ENVCFG |
+					     SMSTATEEN0_SE0;
+				csr_write(CSR_MSTATEEN0, mstateen0);
+#if __riscv_xlen == 32
+				csr_write(CSR_MSTATEEN0H, mstateen0 >> 32);
+#endif
+				csr_write(CSR_SSTATEEN0,
+					  csr_read(CSR_SSTATEEN0) |
+					  SMSTATEEN0_CS | SMSTATEEN0_FCSR);
+			}
+		}
 	}
 }
diff --git a/arch/riscv/include/asm/csr.h b/arch/riscv/include/asm/csr.h
index 986f951c31a..cef0fd906da 100644
--- a/arch/riscv/include/asm/csr.h
+++ b/arch/riscv/include/asm/csr.h
@@ -97,6 +97,18 @@
 #define SIE_STIE		(_AC(0x1, UL) << IRQ_S_TIMER)
 #define SIE_SEIE		(_AC(0x1, UL) << IRQ_S_EXT)
 
+/* Smstateen: mstateen0 / sstateen0 bit definitions */
+#define SMSTATEEN0_CS		(_AC(0x1, ULL) << 0)
+#define SMSTATEEN0_FCSR		(_AC(0x1, ULL) << 1)
+#define SMSTATEEN0_JVT		(_AC(0x1, ULL) << 2)
+#define SMSTATEEN0_CONTEXT	(_AC(0x1, ULL) << 57)
+#define SMSTATEEN0_IMSIC	(_AC(0x1, ULL) << 58)
+#define SMSTATEEN0_AIA		(_AC(0x1, ULL) << 59)
+#define SMSTATEEN0_CSRIND	(_AC(0x1, ULL) << 60)
+#define SMSTATEEN0_P1P13	(_AC(0x1, ULL) << 61)
+#define SMSTATEEN0_ENVCFG	(_AC(0x1, ULL) << 62)
+#define SMSTATEEN0_SE0		(_AC(0x1, ULL) << 63)
+
 #define CSR_FCSR		0x003
 #define CSR_CYCLE		0xc00
 #define CSR_TIME		0xc01
@@ -105,6 +117,7 @@
 #define CSR_SIE			0x104
 #define CSR_STVEC		0x105
 #define CSR_SCOUNTEREN		0x106
+#define CSR_SSTATEEN0		0x10c
 #define CSR_SSCRATCH		0x140
 #define CSR_SEPC		0x141
 #define CSR_SCAUSE		0x142
@@ -126,6 +139,8 @@
 #else
 #define CSR_MCOUNTEREN		0x306
 #endif
+#define CSR_MSTATEEN0		0x30c
+#define CSR_MSTATEEN0H		0x31c
 #define CSR_MSCRATCH		0x340
 #define CSR_MEPC		0x341
 #define CSR_MCAUSE		0x342
-- 
2.34.1