[PATCH] lib: sbi: Emulate debug CSR read/write

[Bin Meng]

From: Bin Meng <bin.meng at windriver.com>

Trap-n-emulate debug CSR read/write, which is needed by S-mode
software to implement the self-hosted debugger functionality.

Since these CSRs might be optional, use CSR detect read/write,
and redirect the illegal instruction exception if unavailable.

Signed-off-by: Bin Meng <bin.meng at windriver.com>
---

 lib/sbi/sbi_emulate_csr.c | 36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/lib/sbi/sbi_emulate_csr.c b/lib/sbi/sbi_emulate_csr.c
index dbb1755..b2893f2 100644
--- a/lib/sbi/sbi_emulate_csr.c
+++ b/lib/sbi/sbi_emulate_csr.c
@@ -11,6 +11,7 @@
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_console.h>
+#include <sbi/sbi_csr_detect.h>
 #include <sbi/sbi_emulate_csr.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
@@ -53,6 +54,7 @@ int sbi_emulate_csr_read(int csr_num, struct sbi_trap_regs *regs,
 #else
 	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
 #endif
+	struct sbi_trap_info trap = {0};
 
 	switch (csr_num) {
 	case CSR_HTIMEDELTA:
@@ -146,6 +148,22 @@ int sbi_emulate_csr_read(int csr_num, struct sbi_trap_regs *regs,
 #undef switchcase_hpm_2
 #undef switchcase_hpm
 
+#define switchcase_debug(__csr)						\
+	case __csr:							\
+		if (prev_mode == PRV_U)					\
+			return SBI_ENOTSUPP;				\
+		*csr_val = csr_read_allowed(__csr, (ulong)&trap);	\
+		if (trap.cause)						\
+			return SBI_ENOTSUPP;				\
+		break;
+
+	switchcase_debug(CSR_TSELECT)
+	switchcase_debug(CSR_TDATA1)
+	switchcase_debug(CSR_TDATA2)
+	switchcase_debug(CSR_TDATA3)
+
+#undef switchcase_debug
+
 	default:
 		ret = SBI_ENOTSUPP;
 		break;
@@ -168,6 +186,7 @@ int sbi_emulate_csr_write(int csr_num, struct sbi_trap_regs *regs,
 #else
 	bool virt = (regs->mstatus & MSTATUS_MPV) ? TRUE : FALSE;
 #endif
+	struct sbi_trap_info trap = {0};
 
 	switch (csr_num) {
 	case CSR_HTIMEDELTA:
@@ -184,6 +203,23 @@ int sbi_emulate_csr_write(int csr_num, struct sbi_trap_regs *regs,
 			ret = SBI_ENOTSUPP;
 		break;
 #endif
+
+#define switchcase_debug(__csr)						\
+	case __csr:							\
+		if (prev_mode == PRV_U)					\
+			return SBI_ENOTSUPP;				\
+		csr_write_allowed(__csr, (ulong)&trap, csr_val);	\
+		if (trap.cause)						\
+			return SBI_ENOTSUPP;				\
+		break;
+
+	switchcase_debug(CSR_TSELECT)
+	switchcase_debug(CSR_TDATA1)
+	switchcase_debug(CSR_TDATA2)
+	switchcase_debug(CSR_TDATA3)
+
+#undef switchcase_debug
+
 	default:
 		ret = SBI_ENOTSUPP;
 		break;
-- 
2.25.1