1 files changed, 58 insertions, 14 deletions

diff --git a/arch/riscv/include/asm/system.h b/arch/riscv/include/asm/system.h
index adf856f9e9..f0b6bf2945 100644
--- a/arch/riscv/include/asm/system.h
+++ b/arch/riscv/include/asm/system.h
@@ -5,6 +5,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include <asm/sbi.h>
+
 #define RISCV_MODE_MASK 0x3
 enum riscv_mode {
     RISCV_U_MODE	= 0,
@@ -13,7 +15,39 @@ enum riscv_mode {
     RISCV_M_MODE	= 3,
 };
 
-static inline enum riscv_mode __riscv_mode(u32 flags)
+static inline void riscv_set_flags(unsigned flags)
+{
+	switch (flags & RISCV_MODE_MASK) {
+	case RISCV_S_MODE:
+		__asm__ volatile("csrw sscratch, %0" : : "r"(flags));
+		break;
+	case RISCV_M_MODE:
+		__asm__ volatile("csrw mscratch, %0" : : "r"(flags));
+		break;
+	default:
+		/* Other modes are not implemented yet */
+		break;
+	}
+}
+
+static inline u32 riscv_get_flags(void)
+{
+	u32 flags = 0;
+
+	if (IS_ENABLED(CONFIG_RISCV_S_MODE))
+		__asm__ volatile("csrr %0, sscratch" : "=r"(flags));
+
+	/*
+	 * Since we always set the scratch register on the very beginning, a
+	 * empty flags indicates that we are running in M-mode.
+	 */
+	if (!flags)
+		__asm__ volatile("csrr %0, mscratch" : "=r"(flags));
+
+	return flags;
+}
+
+static inline enum riscv_mode riscv_mode(void)
 {
 	/* allow non-LTO builds to discard code for unused modes */
 	if (!IS_ENABLED(CONFIG_RISCV_MULTI_MODE)) {
@@ -23,14 +57,14 @@ static inline enum riscv_mode __riscv_mode(u32 flags)
 			return RISCV_S_MODE;
 	}
 
-	return flags & RISCV_MODE_MASK;
+	return riscv_get_flags() & RISCV_MODE_MASK;
 }
 
-static inline long __riscv_hartid(u32 flags)
+static inline long riscv_hartid(void)
 {
 	long hartid = -1;
 
-	switch (__riscv_mode(flags)) {
+	switch (riscv_mode()) {
 	case RISCV_S_MODE:
 		__asm__ volatile("mv %0, tp\n" : "=r"(hartid) :);
 		break;
@@ -42,19 +76,29 @@ static inline long __riscv_hartid(u32 flags)
 	return hartid;
 }
 
-#ifndef __PBL__
-extern unsigned barebox_riscv_pbl_flags;
-
-static inline enum riscv_mode riscv_mode(void)
+static inline long riscv_vendor_id(void)
 {
-	return __riscv_mode(barebox_riscv_pbl_flags);
-}
+	struct sbiret ret;
+	long id;
 
-static inline long riscv_hartid(void)
-{
-	return __riscv_hartid(barebox_riscv_pbl_flags);
+	switch (riscv_mode()) {
+	case RISCV_M_MODE:
+		__asm__ volatile("csrr %0, mvendorid\n" : "=r"(id));
+		return id;
+	case RISCV_S_MODE:
+		/*
+		 * We need to use the sbi_ecall() since it can be that we got
+		 * called without a working stack
+		 */
+		ret = sbi_ecall(SBI_EXT_BASE, SBI_EXT_BASE_GET_MVENDORID,
+				0, 0, 0, 0, 0, 0);
+		if (!ret.error)
+			return ret.value;
+		return -1;
+	default:
+		return -1;
+	}
 }
-#endif
 
 #endif