Allow platform to handle load/store faults

Signed-off-by Bo Gan <[email protected]>

---
?lib/sbi/objects.mk???????????????????????????????? | 2 +- ?lib/sbi/{sbi_misaligned_ldst.c => sbi_trap_ldst.c} | 0 ?2 files changed, 1 insertion(+), 1 deletion(-)
?rename lib/sbi/{sbi_misaligned_ldst.c => sbi_trap_ldst.c} (100%)

lib/sbi/objects.mk +0 -0

index 0a50e95..204dd6b 100644
@@ -70,7 +70,6 @@ libsbi-objs-y += sbi_illegal_insn.o

?libsbi-objs-y += sbi_init.o
?libsbi-objs-y += sbi_ipi.o
?libsbi-objs-y += sbi_irqchip.o
-libsbi-objs-y += sbi_misaligned_ldst.o
?libsbi-objs-y += sbi_platform.o
?libsbi-objs-y += sbi_pmu.o
?libsbi-objs-y += sbi_dbtr.o

lib/sbi/objects.mk +0 -0

@@ -80,6 +79,7 @@ libsbi-objs-y += sbi_system.o

?libsbi-objs-y += sbi_timer.o
?libsbi-objs-y += sbi_tlb.o
?libsbi-objs-y += sbi_trap.o
+libsbi-objs-y += sbi_trap_ldst.o
?libsbi-objs-y += sbi_unpriv.o
?libsbi-objs-y += sbi_expected_trap.o
?libsbi-objs-y += sbi_cppc.o

lib/sbi/sbi_trap_ldst.c renamed from lib/sbi/sbi_misaligned_ldst.c +0 -0

similarity index 100%
rename from lib/sbi/sbi_misaligned_ldst.c
rename to lib/sbi/sbi_trap_ldst.c

Signed-off-by Bo Gan <[email protected]>

---

?include/sbi/{sbi_misaligned_ldst.h => sbi_trap_ldst.h} | 0
?lib/sbi/sbi_trap.c???????????????????????????????????? | 2 +-
?lib/sbi/sbi_trap_ldst.c??????????????????????????????? | 2 +-

?3 files changed, 2 insertions(+), 2 deletions(-)
?rename include/sbi/{sbi_misaligned_ldst.h => sbi_trap_ldst.h} (100%)

include/sbi/sbi_trap_ldst.h renamed from include/sbi/sbi_misaligned_ldst.h +0 -0

similarity index 100%
rename from include/sbi/sbi_misaligned_ldst.h
rename to include/sbi/sbi_trap_ldst.h

lib/sbi/sbi_trap.c +0 -0

index dbf307c..145db4b 100644
@@ -17,7 +17,7 @@

?#include <sbi/sbi_illegal_insn.h>
?#include <sbi/sbi_ipi.h>
?#include <sbi/sbi_irqchip.h>

lib/sbi/sbi_trap.c +1 -1

-#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_trap_ldst.h>

?#include <sbi/sbi_pmu.h>
?#include <sbi/sbi_scratch.h>
?#include <sbi/sbi_timer.h>

lib/sbi/sbi_trap_ldst.c +0 -0

index aa512de..be9a394 100644
@@ -11,7 +11,7 @@

?#include <sbi/riscv_encoding.h>
?#include <sbi/riscv_fp.h>
?#include <sbi/sbi_error.h>

lib/sbi/sbi_trap_ldst.c +1 -1

-#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_trap_ldst.h>

?#include <sbi/sbi_pmu.h>
?#include <sbi/sbi_trap.h>
?#include <sbi/sbi_unpriv.h>

This patch allows platforms to define load/store emulators to handle load/store faults. Each platform can then use it to trap-and-emulate special devices, or filter access to physical devices.

Two more functions are added to sbi_platform_operations.
emulate_load: called when trap.mcause == CAUSE_LOAD_ACCESS
emulate_store: called when trap.mcause == CAUSE_STORE_ACCESS If not defined, sbi_trap_handler redirects the fault, same as before.

The code to decode load/store instruction is reused from sbi_misaligned_load/store_handler, with additional lb/lbu/sb decoding.

Signed-off-by Bo Gan <[email protected]>

---

?include/sbi/sbi_platform.h? |? 13 +++
?include/sbi/sbi_trap.h????? |?? 2 +-
?include/sbi/sbi_trap_ldst.h |? 24 +++++-
?lib/sbi/sbi_trap.c????????? |? 34 +++++---
?lib/sbi/sbi_trap_ldst.c???? | 195 ++++++++++++++++++++++++++++----------------

?5 files changed, 178 insertions(+), 90 deletions(-)

include/sbi/sbi_platform.h +0 -0

index 2fb33e1..f8e4514 100644
@@ -48,6 +48,7 @@

?#include <sbi/sbi_error.h>
?#include <sbi/sbi_scratch.h>
?#include <sbi/sbi_version.h>
+#include <sbi/sbi_trap_ldst.h>
?
?struct sbi_domain_memregion;
?struct sbi_ecall_return;

include/sbi/sbi_platform.h +12 -0

@@ -139,6 +140,18 @@ struct sbi_platform_operations {
 ?	int (*vendor_ext_provider)(long funcid,
 ?				?? struct sbi_trap_regs *regs,
 ?				?? struct sbi_ecall_return *out);
+
+	/**
+	 * platform specific handler to "fixup" load fault
+	 * If NULL, `sbi_trap_handler` will redirect the load fault
+	 */
+	sbi_trap_ld_emulator emulate_load;
+
+	/**
+	 * platform specific handler to "fixup" store fault
+	 * If NULL, `sbi_trap_handler` will redirect the store fault
+	 */
+	sbi_trap_st_emulator emulate_store;
 ?};

?
?/** Platform default per-HART stack size for exception/interrupt handling */

include/sbi/sbi_trap.h +0 -0

index a562b95..2727bdb 100644
@@ -225,7 +225,7 @@ static inline unsigned long sbi_regs_gva(const struct sbi_trap_regs *regs)
 ?}

?
?int sbi_trap_redirect(struct sbi_trap_regs *regs,

include/sbi/sbi_trap.h +1 -1

-		????? struct sbi_trap_info *trap);
+		????? const struct sbi_trap_info *trap);

?
?struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs); ?

include/sbi/sbi_trap_ldst.h +0 -0

index ab27eb4..65ceb80 100644
@@ -13,11 +13,27 @@

?#include <sbi/sbi_types.h>
?
?struct sbi_trap_regs;
+struct sbi_trap_info;
?

include/sbi/sbi_trap_ldst.h +6 -2

-int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
-				struct sbi_trap_regs *regs);
+union reg_data {
+	u64 data_u64;
+	u32 data_u32;
+	u8 data_bytes[8];
+	ulong data_ulong;
+};

?

include/sbi/sbi_trap_ldst.h +3 -2

-int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
-				 struct sbi_trap_regs *regs);
+typedef int (*sbi_trap_ld_emulator)(ulong addr, int len, union reg_data *out_val,
+		struct sbi_trap_regs *regs, const struct sbi_trap_info *orig_trap);
+

?
?#endif

lib/sbi/sbi_trap.c +0 -0

index 145db4b..3800587 100644
@@ -84,7 +84,7 @@ static void __noreturn sbi_trap_error(const char *msg, int rc,

? * @return 0 on success and negative error code on failure ? */
?int sbi_trap_redirect(struct sbi_trap_regs *regs,

lib/sbi/sbi_trap.c +1 -1

-		????? struct sbi_trap_info *trap)
+		????? const struct sbi_trap_info *trap)

?{
? ulong hstatus, vsstatus, prev_mode;
?#if __riscv_xlen == 32

lib/sbi/sbi_trap.c +7 -0

@@ -285,6 +285,13 @@ struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
 ?		}
 ?		return regs;
 ?	}
+	/* Original trap_info */
+	trap.epc = regs->mepc;
+	trap.cause = mcause;
+	trap.tval = mtval;
+	trap.tval2 = mtval2;
+	trap.tinst = mtinst;
+	trap.gva?? = sbi_regs_gva(regs);

?
? switch (mcause) {
? case CAUSE_ILLEGAL_INSTRUCTION:

lib/sbi/sbi_trap.c +0 -0

@@ -292,11 +299,13 @@ struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
 ?		msg = "illegal instruction handler failed";
 ?		break;

? case CAUSE_MISALIGNED_LOAD:

lib/sbi/sbi_trap.c +2 -1

-		rc = sbi_misaligned_load_handler(mtval, mtval2, mtinst, regs);
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_LOAD);
+		rc = sbi_misaligned_load_handler(regs, &trap);

? msg = "misaligned load handler failed";
? break;
? case CAUSE_MISALIGNED_STORE:

lib/sbi/sbi_trap.c +2 -1

-		rc? = sbi_misaligned_store_handler(mtval, mtval2, mtinst, regs);
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_STORE);
+		rc? = sbi_misaligned_store_handler(regs, &trap);

? msg = "misaligned store handler failed";
? break;
? case CAUSE_SUPERVISOR_ECALL:

lib/sbi/sbi_trap.c +0 -0

@@ -305,19 +314,18 @@ struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
 ?		msg = "ecall handler failed";
 ?		break;

? case CAUSE_LOAD_ACCESS:

lib/sbi/sbi_trap.c +4 -0

+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ACCESS_LOAD);
+		rc = sbi_load_fault_handler(regs, &trap);
+		msg = "load fault handler failed";
+		break;

? case CAUSE_STORE_ACCESS:

lib/sbi/sbi_trap.c +4 -3

-		sbi_pmu_ctr_incr_fw(mcause == CAUSE_LOAD_ACCESS ?
-			SBI_PMU_FW_ACCESS_LOAD : SBI_PMU_FW_ACCESS_STORE);
-		/* fallthrough */
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ACCESS_STORE);
+		rc = sbi_store_fault_handler(regs, &trap);
+		msg = "store fault handler failed";
+		break;

? default:
? /* If the trap came from S or U mode, redirect it there */

lib/sbi/sbi_trap.c +1 -7

-		trap.epc = regs->mepc;
-		trap.cause = mcause;
-		trap.tval = mtval;
-		trap.tval2 = mtval2;
-		trap.tinst = mtinst;
-		trap.gva?? = sbi_regs_gva(regs);
-
+		msg = "trap redirect failed";

? rc = sbi_trap_redirect(regs, &trap);
? break;
? }

lib/sbi/sbi_trap_ldst.c +0 -0

index be9a394..768ea20 100644
@@ -15,42 +15,24 @@

?#include <sbi/sbi_pmu.h>
?#include <sbi/sbi_trap.h>
?#include <sbi/sbi_unpriv.h>
+#include <sbi/sbi_platform.h>
?

lib/sbi/sbi_trap_ldst.c +3 -20

-union reg_data {
-	u8 data_bytes[8];
-	ulong data_ulong;
-	u64 data_u64;
-};
-
-static ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
-					ulong addr_offset)
-{
-	if (new_tinst == INSN_PSEUDO_VS_LOAD ||
-	??? new_tinst == INSN_PSEUDO_VS_STORE)
-		return new_tinst;
-	else if (orig_tinst == 0)
-		return 0UL;
-	else
-		return orig_tinst | (addr_offset << SH_RS1);
-}
-
-int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
-				struct sbi_trap_regs *regs)
+static int sbi_trap_emulate_load(struct sbi_trap_regs *regs,
+				const struct sbi_trap_info *orig_trap,
+				sbi_trap_ld_emulator emu)

?{
? ulong insn, insn_len;

lib/sbi/sbi_trap_ldst.c +1 -1

-	union reg_data val;
+	union reg_data val = {0};

? struct sbi_trap_info uptrap;

lib/sbi/sbi_trap_ldst.c +1 -1

-	int i, fp = 0, shift = 0, len = 0;
+	int rc, fp = 0, shift = 0, len = 0;

?

lib/sbi/sbi_trap_ldst.c +1 -3

-	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_LOAD);
-
-	if (tinst & 0x1) {
+	if (orig_trap->tinst & 0x1) {

? /*
? * Bit[0] == 1 implies trapped instruction value is
? * transformed instruction or custom instruction.
? */

lib/sbi/sbi_trap_ldst.c +2 -2

-		insn = tinst | INSN_16BIT_MASK;
-		insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+		insn = orig_trap->tinst | INSN_16BIT_MASK;
+		insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;

? } else {
? /*
? * Bit[0] == 0 implies trapped instruction value is

lib/sbi/sbi_trap_ldst.c +0 -0

@@ -64,7 +46,12 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
 ?		insn_len = INSN_LEN(insn);
 ?	}

?

lib/sbi/sbi_trap_ldst.c +6 -1

-	if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
+	if ((insn & INSN_MASK_LB) == INSN_MATCH_LB) {
+		len	? = 1;
+		shift = 8 * (sizeof(ulong) - len);
+	} else if ((insn & INSN_MASK_LBU) == INSN_MATCH_LBU) {
+		len	? = 1;
+	} else if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {

? len?? = 4;
? shift = 8 * (sizeof(ulong) - len);
?#if __riscv_xlen == 64

lib/sbi/sbi_trap_ldst.c +0 -0

@@ -124,26 +111,12 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,

?#endif
?#endif
? } else {

lib/sbi/sbi_trap_ldst.c +1 -7

-		uptrap.epc = regs->mepc;
-		uptrap.cause = CAUSE_MISALIGNED_LOAD;
-		uptrap.tval = addr;
-		uptrap.tval2 = tval2;
-		uptrap.tinst = tinst;
-		uptrap.gva?? = sbi_regs_gva(regs);
-		return sbi_trap_redirect(regs, &uptrap);
+		return sbi_trap_redirect(regs, orig_trap);

? }
?

lib/sbi/sbi_trap_ldst.c +3 -11

-	val.data_u64 = 0;
-	for (i = 0; i < len; i++) {
-		val.data_bytes[i] = sbi_load_u8((void *)(addr + i),
-						&uptrap);
-		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				tinst, uptrap.tinst, i);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
-	}
+	rc = emu(orig_trap->tval, len, &val, regs, orig_trap);
+	if (rc)
+		return rc;

?
? if (!fp)
? SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);

lib/sbi/sbi_trap_ldst.c +0 -0

@@ -159,23 +132,22 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
 ?	return 0;
 ?}

?

lib/sbi/sbi_trap_ldst.c +3 -2

-int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
-				 struct sbi_trap_regs *regs)
+static int sbi_trap_emulate_store(struct sbi_trap_regs *regs,
+				const struct sbi_trap_info *orig_trap,
+				sbi_trap_st_emulator emu)

?{
? ulong insn, insn_len;
? union reg_data val;
? struct sbi_trap_info uptrap;

lib/sbi/sbi_trap_ldst.c +1 -1

-	int i, len = 0;
+	int rc, len = 0;

?

lib/sbi/sbi_trap_ldst.c +1 -3

-	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_STORE);
-
-	if (tinst & 0x1) {
+	if (orig_trap->tinst & 0x1) {

? /*
? * Bit[0] == 1 implies trapped instruction value is
? * transformed instruction or custom instruction.
? */

lib/sbi/sbi_trap_ldst.c +2 -2

-		insn = tinst | INSN_16BIT_MASK;
-		insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+		insn = orig_trap->tinst | INSN_16BIT_MASK;
+		insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;

? } else {
? /*
? * Bit[0] == 0 implies trapped instruction value is

lib/sbi/sbi_trap_ldst.c +0 -0

@@ -191,7 +163,9 @@ int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,

?
? val.data_ulong = GET_RS2(insn, regs);
?

lib/sbi/sbi_trap_ldst.c +3 -1

-	if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
+	if ((insn & INSN_MASK_SB) == INSN_MATCH_SB) {
+		len = 1;
+	} else if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {

? len = 4;
?#if __riscv_xlen == 64
? } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {

lib/sbi/sbi_trap_ldst.c +0 -0

@@ -238,27 +212,104 @@ int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,

?#endif
?#endif
? } else {

lib/sbi/sbi_trap_ldst.c +1 -7

-		uptrap.epc = regs->mepc;
-		uptrap.cause = CAUSE_MISALIGNED_STORE;
-		uptrap.tval = addr;
-		uptrap.tval2 = tval2;
-		uptrap.tinst = tinst;
-		uptrap.gva?? = sbi_regs_gva(regs);
-		return sbi_trap_redirect(regs, &uptrap);
+		return sbi_trap_redirect(regs, orig_trap);

? }
?

lib/sbi/sbi_trap_ldst.c +3 -10

-	for (i = 0; i < len; i++) {
-		sbi_store_u8((void *)(addr + i), val.data_bytes[i],
-			???? &uptrap);
-		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				tinst, uptrap.tinst, i);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
-	}
+	rc = emu(orig_trap->tval, len, val, regs, orig_trap);
+	if (rc)
+		return rc;

?
? regs->mepc += insn_len;
?
? return 0;
?}

lib/sbi/sbi_trap_ldst.c +90 -0

+
+static ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
+					ulong addr_offset)
+{
+	if (new_tinst == INSN_PSEUDO_VS_LOAD ||
+	??? new_tinst == INSN_PSEUDO_VS_STORE)
+		return new_tinst;
+	else if (orig_tinst == 0)
+		return 0UL;
+	else
+		return orig_tinst | (addr_offset << SH_RS1);
+}
+
+static int sbi_misaligned_ld_emulator(ulong addr, int len, union reg_data *out_val,
+		struct sbi_trap_regs *regs, const struct sbi_trap_info *orig_trap)
+{
+	struct sbi_trap_info uptrap;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		out_val->data_bytes[i] = sbi_load_u8((void *)(addr + i), &uptrap);
+		if (uptrap.cause) {
+			uptrap.epc = regs->mepc;
+			uptrap.tinst = sbi_misaligned_tinst_fixup(
+				orig_trap->tinst, uptrap.tinst, i);
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+	return 0;
+}
+
+static int sbi_misaligned_st_emulator(ulong addr, int len, union reg_data in_val,
+		struct sbi_trap_regs *regs, const struct sbi_trap_info *orig_trap)
+{
+	struct sbi_trap_info uptrap;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		sbi_store_u8((void *)(addr + i), in_val.data_bytes[i], &uptrap);
+		if (uptrap.cause) {
+			uptrap.epc = regs->mepc;
+			uptrap.tinst = sbi_misaligned_tinst_fixup(
+				orig_trap->tinst, uptrap.tinst, i);
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+	return 0;
+}
+
+int sbi_misaligned_load_handler(struct sbi_trap_regs *regs, struct sbi_trap_info *orig_trap)
+{
+	return sbi_trap_emulate_load(regs, orig_trap, sbi_misaligned_ld_emulator);
+}
+
+int sbi_misaligned_store_handler(struct sbi_trap_regs *regs, struct sbi_trap_info *orig_trap)
+{
+	return sbi_trap_emulate_store(regs, orig_trap, sbi_misaligned_st_emulator);
+}
+
+int sbi_load_fault_handler(struct sbi_trap_regs *regs, struct sbi_trap_info *orig_trap)
+{
+	const struct sbi_platform *plat;
+
+	/* If fault came from M mode, just fail */
+	if (((regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT) == PRV_M) {
+		return SBI_EINVAL;
+	}
+
+	plat = sbi_platform_ptr(sbi_scratch_thishart_ptr());
+	if (sbi_platform_ops(plat)->emulate_load) {
+		return sbi_trap_emulate_load(regs, orig_trap, sbi_platform_ops(plat)->emulate_load);
+	}
+	return sbi_trap_redirect(regs, orig_trap);
+}
+
+int sbi_store_fault_handler(struct sbi_trap_regs *regs, struct sbi_trap_info *orig_trap)
+{
+	const struct sbi_platform *plat;
+
+	/* If fault came from M mode, just fail */
+	if (((regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT) == PRV_M) {
+		return SBI_EINVAL;
+	}
+
+	plat = sbi_platform_ptr(sbi_scratch_thishart_ptr());
+	if (sbi_platform_ops(plat)->emulate_store) {
+		return sbi_trap_emulate_store(regs, orig_trap, sbi_platform_ops(plat)->emulate_store);
+	}
+	return sbi_trap_redirect(regs, orig_trap);
+}