[PATCH v2 3/4] lib: sbi: Add variable-length unprivilege access functions

[Bo Gan <ganboing@gmail.com>]

sbi_load/store_loop read/write variable-length buffer unprivileged.
Both function use the widest aligned 8/4/2/1 byte load/stores in each
loop to reduce the total number of iterations.

Also switch the scalar/vector misaligned handlers to make use of such
functions to simplify code.

Miscellaneous: remove the unnecessary [taddr] in inline assembly

Signed-off-by: Bo Gan <ganboing@gmail.com>
---
 include/sbi/sbi_unpriv.h  | 16 +++++++
 lib/sbi/sbi_trap_ldst.c   | 24 ++++-------
 lib/sbi/sbi_trap_v_ldst.c | 38 ++++++++---------
 lib/sbi/sbi_unpriv.c      | 88 ++++++++++++++++++++++++++++++++++++---
 4 files changed, 123 insertions(+), 43 deletions(-)

diff --git a/include/sbi/sbi_unpriv.h b/include/sbi/sbi_unpriv.h
index 8cbd3de0..226cf175 100644
--- a/include/sbi/sbi_unpriv.h
+++ b/include/sbi/sbi_unpriv.h
@@ -15,6 +15,16 @@
 struct sbi_scratch;
 struct sbi_trap_info;
 
+union sbi_unpriv_data {
+	u8 b;
+	u16 h;
+	u32 w;
+#if __riscv_xlen == 64
+	u64 d;
+#endif
+	u8 bytes[__riscv_xlen / 8];
+};
+
 #define DECLARE_UNPRIVILEGED_LOAD_FUNCTION(type)           \
 	type sbi_load_##type(const type *addr,             \
 			     struct sbi_trap_info *trap);
@@ -36,6 +46,12 @@ DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u64)
 DECLARE_UNPRIVILEGED_STORE_FUNCTION(u64)
 DECLARE_UNPRIVILEGED_LOAD_FUNCTION(ulong)
 
+void sbi_load_loop(u8 *buffer, ulong addr, ulong len,
+		   struct sbi_trap_info *trap);
+
+void sbi_store_loop(u8 *buffer, ulong addr, ulong len,
+		    struct sbi_trap_info *trap);
+
 ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap);
 
 #endif
diff --git a/lib/sbi/sbi_trap_ldst.c b/lib/sbi/sbi_trap_ldst.c
index 5f7de662..e726520f 100644
--- a/lib/sbi/sbi_trap_ldst.c
+++ b/lib/sbi/sbi_trap_ldst.c
@@ -471,7 +471,6 @@ static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr,
 	const struct sbi_trap_info *orig_trap = &tcntx->trap;
 	struct sbi_trap_regs *regs = &tcntx->regs;
 	struct sbi_trap_info uptrap;
-	int i;
 
 	if (!rlen) {
 		if (IS_VECTOR_LOAD_STORE(insn))
@@ -484,13 +483,10 @@ static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr,
 	if (addr != orig_trap->tval)
 		return SBI_EFAIL;
 
-	for (i = 0; i < rlen; i++) {
-		out_val->data_bytes[i] =
-			sbi_load_u8((void *)(addr + i), &uptrap);
-		if (uptrap.cause) {
-			sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
+	sbi_load_loop(out_val->data_bytes, addr, rlen, &uptrap);
+	if (uptrap.cause) {
+		sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
+		return sbi_trap_redirect(regs, &uptrap);
 	}
 	return rlen;
 }
@@ -507,7 +503,6 @@ static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr,
 	const struct sbi_trap_info *orig_trap = &tcntx->trap;
 	struct sbi_trap_regs *regs = &tcntx->regs;
 	struct sbi_trap_info uptrap;
-	int i;
 
 	if (!wlen) {
 		if (IS_VECTOR_LOAD_STORE(insn))
@@ -520,13 +515,10 @@ static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr,
 	if (addr != orig_trap->tval)
 		return SBI_EFAIL;
 
-	for (i = 0; i < wlen; i++) {
-		sbi_store_u8((void *)(addr + i),
-			     in_val.data_bytes[i], &uptrap);
-		if (uptrap.cause) {
-			sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
+	sbi_store_loop(in_val.data_bytes, addr, wlen, &uptrap);
+	if (uptrap.cause) {
+		sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
+		return sbi_trap_redirect(regs, &uptrap);
 	}
 	return wlen;
 }
diff --git a/lib/sbi/sbi_trap_v_ldst.c b/lib/sbi/sbi_trap_v_ldst.c
index e16e3def..02f7d6cc 100644
--- a/lib/sbi/sbi_trap_v_ldst.c
+++ b/lib/sbi/sbi_trap_v_ldst.c
@@ -229,20 +229,17 @@ int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
 
 		/* obtain load data from memory */
 		for (ulong seg = 0; seg < nf; seg++) {
-			for (ulong i = 0; i < len; i++) {
-				bytes[seg * len + i] =
-					sbi_load_u8((void *)(addr + seg * len + i),
-						    &uptrap);
-
-				if (uptrap.cause) {
-					if (IS_FAULT_ONLY_FIRST_LOAD(insn) && vstart != 0) {
-						vl = vstart;
-						break;
-					}
-					vsetvl(vl, vtype);
-					sbi_misaligned_v_tinst_fixup(&uptrap);
-					return sbi_trap_redirect(regs, &uptrap);
+			sbi_load_loop(bytes + seg * len,
+				       addr + seg * len, len, &uptrap);
+
+			if (uptrap.cause) {
+				if (IS_FAULT_ONLY_FIRST_LOAD(insn) && vstart != 0) {
+					vl = vstart;
+					break;
 				}
+				vsetvl(vl, vtype);
+				sbi_misaligned_v_tinst_fixup(&uptrap);
+				return sbi_trap_redirect(regs, &uptrap);
 			}
 		}
 
@@ -332,14 +329,13 @@ int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx)
 
 		/* write store data to memory */
 		for (ulong seg = 0; seg < nf; seg++) {
-			for (ulong i = 0; i < len; i++) {
-				sbi_store_u8((void *)(addr + seg * len + i),
-					     bytes[seg * len + i], &uptrap);
-				if (uptrap.cause) {
-					vsetvl(vl, vtype);
-					sbi_misaligned_v_tinst_fixup(&uptrap);
-					return sbi_trap_redirect(regs, &uptrap);
-				}
+			sbi_store_loop(bytes + seg * len,
+					addr + seg * len, len, &uptrap);
+
+			if (uptrap.cause) {
+				vsetvl(vl, vtype);
+				sbi_misaligned_v_tinst_fixup(&uptrap);
+				return sbi_trap_redirect(regs, &uptrap);
 			}
 		}
 	} while (++vstart < vl);
diff --git a/lib/sbi/sbi_unpriv.c b/lib/sbi/sbi_unpriv.c
index f9bbec59..14b8f3cb 100644
--- a/lib/sbi/sbi_unpriv.c
+++ b/lib/sbi/sbi_unpriv.c
@@ -11,6 +11,7 @@
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
 #include <sbi/sbi_trap.h>
 #include <sbi/sbi_unpriv.h>
 
@@ -22,13 +23,12 @@
 	type sbi_load_##type(const type *addr,                                \
 			     struct sbi_trap_info *trap)                      \
 	{                                                                     \
-		register ulong tinfo asm("a3");                               \
+		register ulong tinfo asm("a3") = (ulong)trap;                 \
 		register ulong mstatus = 0;                                   \
 		register ulong mtvec = (ulong)sbi_hart_expected_trap;         \
 		type ret = 0;                                                 \
 		trap->cause = 0;                                              \
 		asm volatile(                                                 \
-			"add %[tinfo], %[taddr], zero\n"                      \
 			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
 			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
 			".option push\n"                                      \
@@ -39,8 +39,7 @@
 			"csrw " STR(CSR_MTVEC) ", %[mtvec]"                   \
 		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
 		      [tinfo] "+&r"(tinfo), [ret] "=&r"(ret)                  \
-		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
-		      [taddr] "r"((ulong)trap)                                \
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV)             \
 		    : "a4", "memory");                                        \
 		return ret;                                                   \
 	}
@@ -54,7 +53,6 @@
 		register ulong mtvec = (ulong)sbi_hart_expected_trap;         \
 		trap->cause = 0;                                              \
 		asm volatile(                                                 \
-			"add %[tinfo], %[taddr], zero\n"                      \
 			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"      \
 			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"   \
 			".option push\n"                                      \
@@ -66,7 +64,7 @@
 		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),         \
 		      [tinfo] "+&r"(tinfo)                                    \
 		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),            \
-		      [val] "r"(val), [taddr] "r"((ulong)trap)                \
+		      [val] "r"(val)                                          \
 		    : "a4", "memory");                                        \
 	}
 
@@ -116,6 +114,84 @@ void sbi_store_u64(u64 *addr, u64 val,
 # error "Unexpected __riscv_xlen"
 #endif
 
+void sbi_load_loop(u8 *buffer, ulong addr, ulong len,
+		   struct sbi_trap_info *trap)
+{
+	union sbi_unpriv_data data;
+
+	trap->cause = 0;
+	while (len) {
+		unsigned int width = __riscv_xlen / 8;
+		void *ptr = (void*)addr;
+
+		while (len < width || (addr & (width - 1)))
+			width /= 2;
+
+		switch (width) {
+		case 1:
+			data.b = sbi_load_u8(ptr, trap);
+			break;
+		case 2:
+			data.h = sbi_load_u16(ptr, trap);
+			break;
+		case 4:
+			data.w = sbi_load_u32(ptr, trap);
+			break;
+#if __riscv_xlen == 64
+		case 8:
+			data.d = sbi_load_u64(ptr, trap);
+			break;
+#endif
+		}
+		if (trap->cause)
+			return;
+
+		sbi_memcpy(buffer, data.bytes, width);
+		len -= width;
+		addr += width;
+		buffer += width;
+	}
+}
+
+void sbi_store_loop(u8 *buffer, ulong addr, ulong len,
+		    struct sbi_trap_info *trap)
+{
+	union sbi_unpriv_data data;
+
+	trap->cause = 0;
+	while (len) {
+		unsigned int width = __riscv_xlen / 8;
+		void *ptr = (void*)addr;
+
+		while (len < width || (addr & (width - 1)))
+			width /= 2;
+
+		sbi_memcpy(data.bytes, buffer, width);
+		switch (width) {
+		case 1:
+			sbi_store_u8(ptr, data.b, trap);
+			break;
+		case 2:
+			sbi_store_u16(ptr, data.h, trap);
+			break;
+		case 4:
+			sbi_store_u32(ptr, data.w, trap);
+			break;
+#if __riscv_xlen == 64
+		case 8:
+			sbi_store_u64(ptr, data.d, trap);
+			break;
+#endif
+		}
+		if (trap->cause)
+			return;
+
+		len -= width;
+		addr += width;
+		buffer += width;
+	}
+}
+
 ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap)
 {
 	register ulong tinfo asm("a3");
-- 
2.34.1


-- 
opensbi mailing list
opensbi@lists.infradead.org
http://lists.infradead.org/mailman/listinfo/opensbi