From 1a5614e971cf44e08342c2b1639fa3be544b0202 Mon Sep 17 00:00:00 2001
From: Anup Patel <anup.patel@wdc.com>
Date: Fri, 5 Apr 2019 14:08:57 +0530
Subject: lib: Extend sbi_hart_switch_mode() to support hypervisor extension
This patch extends sbi_hart_switch_mode() to support entering
VS/VU modes when hypervisor extension is available.
Signed-off-by: Anup Patel <anup.patel@wdc.com>
---
include/sbi/riscv_encoding.h | 14 ++++++++++++++
include/sbi/sbi_hart.h | 3 ++-
lib/sbi/sbi_hart.c | 27 +++++++++++++++++++++++++--
lib/sbi/sbi_init.c | 5 +++--
4 files changed, 44 insertions(+), 5 deletions(-)
diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 5a5086a..dace9e6 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -38,8 +38,21 @@
#define MSTATUS_TW 0x00200000
#define MSTATUS_TSR 0x00400000
#define MSTATUS32_SD 0x80000000
+#if __riscv_xlen == 64
#define MSTATUS_UXL 0x0000000300000000
#define MSTATUS_SXL 0x0000000C00000000
+#define MSTATUS_MTL 0x0000004000000000
+#define MSTATUS_MTL_SHIFT 38
+#define MSTATUS_MPV 0x0000008000000000
+#define MSTATUS_MPV_HIFT 39
+#else
+#define MSTATUSH_UXL 0x00000003
+#define MSTATUSH_SXL 0x0000000C
+#define MSTATUSH_MTL 0x00000040
+#define MSTATUSH_MTL_SHIFT 6
+#define MSTATUSH_MPV 0x00000080
+#define MSTATUSH_MPV_HIFT 7
+#endif
#define MSTATUS64_SD 0x8000000000000000
#define SSTATUS_UIE 0x00000001
@@ -256,6 +269,7 @@
#define CSR_MIE 0x304
#define CSR_MTVEC 0x305
#define CSR_MCOUNTEREN 0x306
+#define CSR_MSTATUSH 0x310
#define CSR_MSCRATCH 0x340
#define CSR_MEPC 0x341
#define CSR_MCAUSE 0x342
diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h
index 4aa4a5c..0762f95 100644
--- a/include/sbi/sbi_hart.h
+++ b/include/sbi/sbi_hart.h
@@ -26,7 +26,8 @@ void __attribute__((noreturn)) sbi_hart_hang(void);
void __attribute__((noreturn))
sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
- unsigned long next_addr, unsigned long next_mode);
+ unsigned long next_addr, unsigned long next_mode,
+ bool next_virt);
void sbi_hart_mark_available(u32 hartid);
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 6dac84b..b0c087a 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -241,9 +241,14 @@ void __attribute__((noreturn)) sbi_hart_hang(void)
void __attribute__((noreturn))
sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
- unsigned long next_addr, unsigned long next_mode)
+ unsigned long next_addr, unsigned long next_mode,
+ bool next_virt)
{
+#if __riscv_xlen == 32
+ unsigned long val, valH;
+#else
unsigned long val;
+#endif
switch (next_mode) {
case PRV_M:
@@ -263,7 +268,25 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
val = csr_read(CSR_MSTATUS);
val = INSERT_FIELD(val, MSTATUS_MPP, next_mode);
val = INSERT_FIELD(val, MSTATUS_MPIE, 0);
-
+#if __riscv_xlen == 32
+ if (misa_extension('H')) {
+ valH = csr_read(CSR_MSTATUSH);
+ valH = INSERT_FIELD(valH, MSTATUSH_MTL, 0);
+ if (next_virt)
+ valH = INSERT_FIELD(valH, MSTATUSH_MPV, 1);
+ else
+ valH = INSERT_FIELD(valH, MSTATUSH_MPV, 0);
+ csr_write(CSR_MSTATUSH, valH);
+ }
+#else
+ if (misa_extension('H')) {
+ val = INSERT_FIELD(val, MSTATUS_MTL, 0);
+ if (next_virt)
+ val = INSERT_FIELD(val, MSTATUS_MPV, 1);
+ else
+ val = INSERT_FIELD(val, MSTATUS_MPV, 0);
+ }
+#endif
csr_write(CSR_MSTATUS, val);
csr_write(CSR_MEPC, next_addr);
diff --git a/lib/sbi/sbi_init.c b/lib/sbi/sbi_init.c
index 4663a30..e1acb57 100644
--- a/lib/sbi/sbi_init.c
+++ b/lib/sbi/sbi_init.c
@@ -102,7 +102,7 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
sbi_hart_wake_coldboot_harts(scratch, hartid);
sbi_hart_mark_available(hartid);
sbi_hart_switch_mode(hartid, scratch->next_arg1, scratch->next_addr,
- scratch->next_mode);
+ scratch->next_mode, FALSE);
}
static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
@@ -147,7 +147,8 @@ static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
sbi_hart_hang();
else
sbi_hart_switch_mode(hartid, scratch->next_arg1,
- scratch->next_addr, scratch->next_mode);
+ scratch->next_addr,
+ scratch->next_mode, FALSE);
}
static atomic_t coldboot_lottery = ATOMIC_INITIALIZER(0);
--
cgit v1.2.3