lib: Emulate HTIMEDELTA CSR for platforms not having TIME CSR

For platforms not having TIME CSR, we trap-n-emulate TIME CSR
read/write in OpenSBI. Same rationale applies to HTIMEDELTA CSR
as well so we trap-n-emulate HTIMEDELTA CSR for platforms not
having TIME CSR.

Signed-off-by: Anup Patel <anup.patel@wdc.com>

6 files changed, 122 insertions, 17 deletions

diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 5aab9d2..902c81a 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -276,7 +276,9 @@
 #define CSR_HSTATUS			0x600
 #define CSR_HEDELEG			0x602
 #define CSR_HIDELEG			0x603
-#define CSR_HCOUNTERNEN		0x606
+#define CSR_HTIMEDELTA			0x605
+#define CSR_HTIMEDELTAH			0x615
+#define CSR_HCOUNTERNEN			0x606
 #define CSR_HGATP			0x680
 
 #define CSR_VSSTATUS			0x200
diff --git a/include/sbi/sbi_emulate_csr.h b/include/sbi/sbi_emulate_csr.h
index 5d1755f..fe357e5 100644
--- a/include/sbi/sbi_emulate_csr.h
+++ b/include/sbi/sbi_emulate_csr.h
@@ -12,12 +12,13 @@
 
 #include <sbi/sbi_types.h>
 
+struct sbi_trap_regs;
 struct sbi_scratch;
 
-int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
+int sbi_emulate_csr_read(int csr_num, u32 hartid, struct sbi_trap_regs *regs,
 			 struct sbi_scratch *scratch, ulong *csr_val);
 
-int sbi_emulate_csr_write(int csr_num, u32 hartid, ulong mstatus,
+int sbi_emulate_csr_write(int csr_num, u32 hartid, struct sbi_trap_regs *regs,
 			  struct sbi_scratch *scratch, ulong csr_val);
 
 #endif
diff --git a/include/sbi/sbi_timer.h b/include/sbi/sbi_timer.h
index e40cce9..c14c8d0 100644
--- a/include/sbi/sbi_timer.h
+++ b/include/sbi/sbi_timer.h
@@ -16,6 +16,14 @@ struct sbi_scratch;
 
 u64 sbi_timer_value(struct sbi_scratch *scratch);
 
+u64 sbi_timer_virt_value(struct sbi_scratch *scratch);
+
+u64 sbi_timer_get_delta(struct sbi_scratch *scratch);
+
+void sbi_timer_set_delta(struct sbi_scratch *scratch, ulong delta);
+
+void sbi_timer_set_delta_upper(struct sbi_scratch *scratch, ulong delta_upper);
+
 void sbi_timer_event_stop(struct sbi_scratch *scratch);
 
 void sbi_timer_event_start(struct sbi_scratch *scratch, u64 next_event);
diff --git a/lib/sbi/sbi_emulate_csr.c b/lib/sbi/sbi_emulate_csr.c
index 791890f..0901ec1 100644
--- a/lib/sbi/sbi_emulate_csr.c
+++ b/lib/sbi/sbi_emulate_csr.c
@@ -14,16 +14,30 @@
 #include <sbi/sbi_emulate_csr.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_timer.h>
+#include <sbi/sbi_trap.h>
 
-int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
+int sbi_emulate_csr_read(int csr_num, u32 hartid, struct sbi_trap_regs *regs,
 			 struct sbi_scratch *scratch, ulong *csr_val)
 {
+	int ret = 0;
 	ulong cen = -1UL;
+	ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
 
-	if (EXTRACT_FIELD(mstatus, MSTATUS_MPP) == PRV_U)
+	if (prev_mode == PRV_U)
 		cen = csr_read(CSR_SCOUNTEREN);
 
 	switch (csr_num) {
+	case CSR_HTIMEDELTA:
+		if (prev_mode == PRV_S && !virt)
+			*csr_val = sbi_timer_get_delta(scratch);
+		else
+			ret = SBI_ENOTSUPP;
+		break;
 	case CSR_CYCLE:
 		if (!((cen >> (CSR_CYCLE - CSR_CYCLE)) & 1))
 			return -1;
@@ -32,7 +46,8 @@ int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
 	case CSR_TIME:
 		if (!((cen >> (CSR_TIME - CSR_CYCLE)) & 1))
 			return -1;
-		*csr_val = sbi_timer_value(scratch);
+		*csr_val = (virt) ? sbi_timer_virt_value(scratch):
+				    sbi_timer_value(scratch);
 		break;
 	case CSR_INSTRET:
 		if (!((cen >> (CSR_INSTRET - CSR_CYCLE)) & 1))
@@ -50,6 +65,12 @@ int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
 		*csr_val = csr_read(CSR_MHPMCOUNTER4);
 		break;
 #if __riscv_xlen == 32
+	case CSR_HTIMEDELTAH:
+		if (prev_mode == PRV_S && !virt)
+			*csr_val = sbi_timer_get_delta(scratch) >> 32;
+		else
+			ret = SBI_ENOTSUPP;
+		break;
 	case CSR_CYCLEH:
 		if (!((cen >> (CSR_CYCLE - CSR_CYCLE)) & 1))
 			return -1;
@@ -58,7 +79,8 @@ int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
 	case CSR_TIMEH:
 		if (!((cen >> (CSR_TIME - CSR_CYCLE)) & 1))
 			return -1;
-		*csr_val = sbi_timer_value(scratch) >> 32;
+		*csr_val = (virt) ? sbi_timer_virt_value(scratch) >> 32:
+				    sbi_timer_value(scratch) >> 32;
 		break;
 	case CSR_INSTRETH:
 		if (!((cen >> (CSR_INSTRET - CSR_CYCLE)) & 1))
@@ -83,18 +105,35 @@ int sbi_emulate_csr_read(int csr_num, u32 hartid, ulong mstatus,
 		*csr_val = csr_read(CSR_MHPMEVENT4);
 		break;
 	default:
+		ret = SBI_ENOTSUPP;
+		break;
+	};
+
+	if (ret)
 		sbi_dprintf(scratch, "%s: hartid%d: invalid csr_num=0x%x\n",
 			    __func__, hartid, csr_num);
-		return SBI_ENOTSUPP;
-	};
 
-	return 0;
+	return ret;
 }
 
-int sbi_emulate_csr_write(int csr_num, u32 hartid, ulong mstatus,
+int sbi_emulate_csr_write(int csr_num, u32 hartid, struct sbi_trap_regs *regs,
 			  struct sbi_scratch *scratch, ulong csr_val)
 {
+	int ret = 0;
+	ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
+#if __riscv_xlen == 32
+	bool virt = (regs->mstatusH & MSTATUSH_MPV) ? TRUE : FALSE;
+#else
+	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
+#endif
+
 	switch (csr_num) {
+	case CSR_HTIMEDELTA:
+		if (prev_mode == PRV_S && !virt)
+			sbi_timer_set_delta(scratch, csr_val);
+		else
+			ret = SBI_ENOTSUPP;
+		break;
 	case CSR_CYCLE:
 		csr_write(CSR_MCYCLE, csr_val);
 		break;
@@ -108,6 +147,12 @@ int sbi_emulate_csr_write(int csr_num, u32 hartid, ulong mstatus,
 		csr_write(CSR_MHPMCOUNTER4, csr_val);
 		break;
 #if __riscv_xlen == 32
+	case CSR_HTIMEDELTAH:
+		if (prev_mode == PRV_S && !virt)
+			sbi_timer_set_delta_upper(scratch, csr_val);
+		else
+			ret = SBI_ENOTSUPP;
+		break;
 	case CSR_CYCLEH:
 		csr_write(CSR_MCYCLEH, csr_val);
 		break;
@@ -128,10 +173,13 @@ int sbi_emulate_csr_write(int csr_num, u32 hartid, ulong mstatus,
 		csr_write(CSR_MHPMEVENT4, csr_val);
 		break;
 	default:
+		ret = SBI_ENOTSUPP;
+		break;
+	};
+
+	if (ret)
 		sbi_dprintf(scratch, "%s: hartid%d: invalid csr_num=0x%x\n",
 			    __func__, hartid, csr_num);
-		return SBI_ENOTSUPP;
-	};
 
-	return 0;
+	return ret;
 }
diff --git a/lib/sbi/sbi_illegal_insn.c b/lib/sbi/sbi_illegal_insn.c
index e75c13c..d15986a 100644
--- a/lib/sbi/sbi_illegal_insn.c
+++ b/lib/sbi/sbi_illegal_insn.c
@@ -49,8 +49,7 @@ static int system_opcode_insn(ulong insn, u32 hartid, ulong mcause,
 		return sbi_trap_redirect(regs, scratch,
 					 regs->mepc, mcause, insn);
 
-	if (sbi_emulate_csr_read(csr_num, hartid, regs->mstatus,
-				 scratch, &csr_val))
+	if (sbi_emulate_csr_read(csr_num, hartid, regs, scratch, &csr_val))
 		return truly_illegal_insn(insn, hartid, mcause,
 					  regs, scratch);
 
@@ -80,7 +79,7 @@ static int system_opcode_insn(ulong insn, u32 hartid, ulong mcause,
 		return truly_illegal_insn(insn, hartid, mcause, regs, scratch);
 	};
 
-	if (do_write && sbi_emulate_csr_write(csr_num, hartid, regs->mstatus,
+	if (do_write && sbi_emulate_csr_write(csr_num, hartid, regs,
 					      scratch, new_csr_val))
 		return truly_illegal_insn(insn, hartid, mcause, regs, scratch);
 
diff --git a/lib/sbi/sbi_timer.c b/lib/sbi/sbi_timer.c
index c58441d..1ba386f 100644
--- a/lib/sbi/sbi_timer.c
+++ b/lib/sbi/sbi_timer.c
@@ -9,9 +9,12 @@
 
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
+#include <sbi/sbi_error.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_timer.h>
 
+static unsigned long time_delta_off;
+
 #if __riscv_xlen == 32
 u64 get_ticks(void)
 {
@@ -44,6 +47,35 @@ u64 sbi_timer_value(struct sbi_scratch *scratch)
 		return get_ticks();
 }
 
+u64 sbi_timer_virt_value(struct sbi_scratch *scratch)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+
+	return sbi_timer_value(scratch) + *time_delta;
+}
+
+u64 sbi_timer_get_delta(struct sbi_scratch *scratch)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+
+	return *time_delta;
+}
+
+void sbi_timer_set_delta(struct sbi_scratch *scratch, ulong delta)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+
+	*time_delta = (u64)delta;
+}
+
+void sbi_timer_set_delta_upper(struct sbi_scratch *scratch, ulong delta_upper)
+{
+	u64 *time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+
+	*time_delta &= 0xffffffffULL;
+	*time_delta |= ((u64)delta_upper << 32);
+}
+
 void sbi_timer_event_stop(struct sbi_scratch *scratch)
 {
 	sbi_platform_timer_event_stop(sbi_platform_ptr(scratch));
@@ -64,5 +96,20 @@ void sbi_timer_process(struct sbi_scratch *scratch)
 
 int sbi_timer_init(struct sbi_scratch *scratch, bool cold_boot)
 {
+	u64 *time_delta;
+
+	if (cold_boot) {
+		time_delta_off = sbi_scratch_alloc_offset(sizeof(*time_delta),
+							  "TIME_DELTA");
+		if (!time_delta_off)
+			return SBI_ENOMEM;
+	} else {
+		if (!time_delta_off)
+			return SBI_ENOMEM;
+	}
+
+	time_delta = sbi_scratch_offset_ptr(scratch, time_delta_off);
+	*time_delta = 0;
+
 	return sbi_platform_timer_init(sbi_platform_ptr(scratch), cold_boot);
 }