[PATCH v2 2/4] lib: sbi: Rework and split sbi_misaligned(_v)_tinst_fixup

[Bo Gan <ganboing@gmail.com>]

The load/store address offset between the uptrap and the orig_trap
can be derived by orig_trap->tval - uptrap->tval, thus refactor
the function prototype for simplicity.

For vector load, sbi_misaligned_v_tinst_fixup is introduced. There's
no transformed instruction for vector load/store, so null out tinst
if the fault is not a guest-page fault.

Signed-off-by: Bo Gan <ganboing@gmail.com>
---
 include/sbi/sbi_trap_ldst.h |  3 ---
 lib/sbi/sbi_trap_ldst.c     | 46 +++++++++++++++++++++++++++----------
 lib/sbi/sbi_trap_v_ldst.c   | 31 ++++++++++++++++++++-----
 3 files changed, 59 insertions(+), 21 deletions(-)

diff --git a/include/sbi/sbi_trap_ldst.h b/include/sbi/sbi_trap_ldst.h
index 33c348c5..30228d24 100644
--- a/include/sbi/sbi_trap_ldst.h
+++ b/include/sbi/sbi_trap_ldst.h
@@ -28,9 +28,6 @@ int sbi_load_access_handler(struct sbi_trap_context *tcntx);
 
 int sbi_store_access_handler(struct sbi_trap_context *tcntx);
 
-ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
-				 ulong addr_offset);
-
 int sbi_misaligned_v_ld_emulator(ulong insn,
 				 struct sbi_trap_context *tcntx);
 
diff --git a/lib/sbi/sbi_trap_ldst.c b/lib/sbi/sbi_trap_ldst.c
index c1392251..5f7de662 100644
--- a/lib/sbi/sbi_trap_ldst.c
+++ b/lib/sbi/sbi_trap_ldst.c
@@ -32,16 +32,40 @@ typedef int (*sbi_trap_st_emulator)(ulong insn, int wlen, ulong waddr,
 				    union sbi_ldst_data in_val,
 				    struct sbi_trap_context *tcntx);
 
-ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
-					ulong addr_offset)
+/**
+ * Handling of misaligned fault is done by a collection of smaller, but
+ * aligned load/store(s). Another fault (load/store, page fault...) can
+ * arise from any of them, then the handling gets aborted. We must fixup
+ * the tinst to pretend the fault was rised from the original insn.
+ * Specifically, fixup the offset field using the tval diff between the
+ * new trap and the original one (if required).
+ */
+static inline void sbi_misaligned_tinst_fixup(
+			const struct sbi_trap_info *orig_trap,
+			      struct sbi_trap_info *uptrap)
 {
-	if (new_tinst == INSN_PSEUDO_VS_LOAD ||
-	    new_tinst == INSN_PSEUDO_VS_STORE)
-		return new_tinst;
-	else if (orig_tinst == 0)
-		return 0UL;
+	ulong offset = uptrap->tval - orig_trap->tval;
+
+	/*
+	 * The function is called in code path for handling a scalar
+	 * load/store misaligned fault, thus the new uptrap can't have
+	 * custom value of tinst
+	 */
+	if (uptrap->tinst == INSN_PSEUDO_VS_LOAD ||
+	    uptrap->tinst == INSN_PSEUDO_VS_STORE)
+		/* Use uptrap as-is for guest-page faults */
+		return;
+	/*
+	 * Only fixup if orig tinst is valid. Otherwise, discard the
+	 * new tinst to be on the safe side. Never use new tinst as-is!
+	 * It's load/store width surely mismatches the original width.
+	 * For vector, discard it regardless. It doesn't make sense to
+	 * have a transformed tinst
+	 */
+	else if (orig_trap->tinst == 0)
+		uptrap->tinst = 0;
 	else
-		return orig_tinst | (addr_offset << SH_RS1);
+		uptrap->tinst = orig_trap->tinst | (offset << SH_RS1);
 }
 
 static inline bool sbi_trap_tinst_valid(ulong tinst)
@@ -464,8 +488,7 @@ static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr,
 		out_val->data_bytes[i] =
 			sbi_load_u8((void *)(addr + i), &uptrap);
 		if (uptrap.cause) {
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				orig_trap->tinst, uptrap.tinst, i);
+			sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
 			return sbi_trap_redirect(regs, &uptrap);
 		}
 	}
@@ -501,8 +524,7 @@ static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr,
 		sbi_store_u8((void *)(addr + i),
 			     in_val.data_bytes[i], &uptrap);
 		if (uptrap.cause) {
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				orig_trap->tinst, uptrap.tinst, i);
+			sbi_misaligned_tinst_fixup(orig_trap, &uptrap);
 			return sbi_trap_redirect(regs, &uptrap);
 		}
 	}
diff --git a/lib/sbi/sbi_trap_v_ldst.c b/lib/sbi/sbi_trap_v_ldst.c
index 7d2e1409..e16e3def 100644
--- a/lib/sbi/sbi_trap_v_ldst.c
+++ b/lib/sbi/sbi_trap_v_ldst.c
@@ -138,9 +138,31 @@ static inline void vsetvl(ulong vl, ulong vtype)
 			:: "r" (vl), "r" (vtype));
 }
 
+/**
+ * Handling of misaligned fault is done by a collection of smaller, but
+ * aligned load/store(s). Another fault (load/store, page fault...) can
+ * arise from any of them, then the handling gets aborted. We must fixup
+ * the tinst to pretend the fault was rised from the original insn. For
+ * vector insn, simply null out tinst if it's not a guest-page fault, as
+ * there's no transformed insn for vector load/store
+ */
+static inline void sbi_misaligned_v_tinst_fixup(struct sbi_trap_info *uptrap)
+{
+	/*
+	 * The function is called in code path for handling a vector
+	 * load/store misaligned fault, thus the new uptrap can't have
+	 * custom value of tinst
+	 */
+	if (uptrap->tinst == INSN_PSEUDO_VS_LOAD ||
+	    uptrap->tinst == INSN_PSEUDO_VS_STORE)
+		/* Use uptrap as-is for guest-page faults */
+		return;
+
+	uptrap->tinst = 0;
+}
+
 int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
 {
-	const struct sbi_trap_info *orig_trap = &tcntx->trap;
 	struct sbi_trap_regs *regs = &tcntx->regs;
 	struct sbi_trap_info uptrap;
 	ulong vl = csr_read(CSR_VL);
@@ -218,8 +240,7 @@ int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
 						break;
 					}
 					vsetvl(vl, vtype);
-					uptrap.tinst = sbi_misaligned_tinst_fixup(
-						orig_trap->tinst, uptrap.tinst, i);
+					sbi_misaligned_v_tinst_fixup(&uptrap);
 					return sbi_trap_redirect(regs, &uptrap);
 				}
 			}
@@ -240,7 +261,6 @@ int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
 
 int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx)
 {
-	const struct sbi_trap_info *orig_trap = &tcntx->trap;
 	struct sbi_trap_regs *regs = &tcntx->regs;
 	struct sbi_trap_info uptrap;
 	ulong vl = csr_read(CSR_VL);
@@ -317,8 +337,7 @@ int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx)
 					     bytes[seg * len + i], &uptrap);
 				if (uptrap.cause) {
 					vsetvl(vl, vtype);
-					uptrap.tinst = sbi_misaligned_tinst_fixup(
-						orig_trap->tinst, uptrap.tinst, i);
+					sbi_misaligned_v_tinst_fixup(&uptrap);
 					return sbi_trap_redirect(regs, &uptrap);
 				}
 			}
-- 
2.34.1


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