Fixes for load/store misaligned and access faults

- Add MXL encoding for calculating XLEN.
- Add instruction encoding for c.lbu/c.sb,
and imm encoding for multiple RVC insn.

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

---
 include/sbi/riscv_encoding.h | 20 ++++++++++++++++++--
 1 file changed, 18 insertions(+), 2 deletions(-)

include/sbi/riscv_encoding.h +18 -3

index b5a4ce81..8ab59abe 100644
@@ -36,8 +36,10 @@
 #define MSTATUS_SDT                    _UL(0x01000000)
 #define MSTATUS32_SD                   _UL(0x80000000)
 #if __riscv_xlen == 64
-#define MSTATUS_UXL                    _ULL(0x0000000300000000)
-#define MSTATUS_SXL                    _ULL(0x0000000C00000000)
+#define MSTATUS_UXL_SHIFT              32
+#define MSTATUS_UXL                    (_ULL(3) << MSTATUS_UXL_SHIFT)
+#define MSTATUS_SXL_SHIFT              34
+#define MSTATUS_SXL                    (_ULL(3) << MSTATUS_SXL_SHIFT)
 #define MSTATUS_SBE                    _ULL(0x0000001000000000)
 #define MSTATUS_MBE                    _ULL(0x0000002000000000)
 #define MSTATUS_GVA                    _ULL(0x0000004000000000)

@@ -56,6 +58,9 @@
 #endif
 #define MSTATUS32_SD                   _UL(0x80000000)
 #define MSTATUS64_SD                   _ULL(0x8000000000000000)
+#define MXL_XLEN_32                    1
+#define MXL_XLEN_64                    2
+#define MXL_TO_XLEN(x)                 (1U << (x + 4))
 
 #define SSTATUS_SIE                    MSTATUS_SIE
 #define SSTATUS_SPIE_SHIFT             MSTATUS_SPIE_SHIFT

@@ -939,6 +944,10 @@
 #define INSN_MATCH_C_FSWSP             0xe002
 #define INSN_MASK_C_FSWSP              0xe003
 
+#define INSN_MATCH_C_LBU               0x8000
+#define INSN_MASK_C_LBU                        0xfc03
+#define INSN_MATCH_C_SB                        0x8800
+#define INSN_MASK_C_SB                 0xfc03
 #define INSN_MATCH_C_LHU               0x8400
 #define INSN_MASK_C_LHU                        0xfc43
 #define INSN_MATCH_C_LH                        0x8440

@@ -1368,6 +1377,9 @@
 #define SH_RS2C                                2
 
 #define RV_X(x, s, n)                  (((x) >> (s)) & ((1 << (n)) - 1))
+#define RVC_LB_IMM(x)                  ((RV_X(x, 6, 1) << 0) | \
+                                        (RV_X(x, 5, 1) << 1))
+#define RVC_LH_IMM(x)                   (RV_X(x, 5, 1) << 1)
 #define RVC_LW_IMM(x)                  ((RV_X(x, 6, 1) << 2) | \
                                         (RV_X(x, 10, 3) << 3) | \
                                         (RV_X(x, 5, 1) << 6))

@@ -1379,6 +1391,10 @@
 #define RVC_LDSP_IMM(x)                        ((RV_X(x, 5, 2) << 3) | \
                                         (RV_X(x, 12, 1) << 5) | \
                                         (RV_X(x, 2, 3) << 6))
+#define RVC_SB_IMM(x)                  RVC_LB_IMM(x)
+#define RVC_SH_IMM(x)                  RVC_LH_IMM(x)
+#define RVC_SW_IMM(x)                  RVC_LW_IMM(x)
+#define RVC_SD_IMM(x)                  RVC_LD_IMM(x)
 #define RVC_SWSP_IMM(x)                        ((RV_X(x, 9, 4) << 2) | \
                                         (RV_X(x, 7, 2) << 6))
 #define RVC_SDSP_IMM(x)                        ((RV_X(x, 10, 3) << 3) | \
--

2.34.1

sbi_regs_prev_xlen reports the xlen of previous mode by decoding from multiple CSRs including mstatus/hstatus/vsstatus

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

---
 include/sbi/sbi_trap.h | 58 ++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 58 insertions(+)

include/sbi/sbi_trap.h +58 -1

index 731a0c98..4ef672f4 100644
@@ -273,6 +273,64 @@ static inline int sbi_mstatus_prev_mode(unsigned long mstatus)
        return (mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
 }
 
+#if __riscv_xlen == 32
+static inline int sbi_regs_prev_xlen(const struct sbi_trap_regs *regs)
+{
+       return __riscv_xlen;
+}
+#else
+static inline int sbi_mstatus_sxl(unsigned long mstatus)
+{
+       return (mstatus & MSTATUS_SXL) >> MSTATUS_SXL_SHIFT;
+}
+
+static inline int sbi_mstatus_uxl(unsigned long mstatus)
+{
+       return (mstatus & MSTATUS_UXL) >> MSTATUS_UXL_SHIFT;
+}
+
+static inline int sbi_hstatus_vsxl(unsigned long hstatus)
+{
+       return (hstatus & HSTATUS_VSXL) >> HSTATUS_VSXL_SHIFT;
+}
+
+static inline int sbi_regs_prev_xlen(const struct sbi_trap_regs *regs)
+{
+       unsigned long hstatus, vsstatus;
+
+       if (!sbi_regs_from_virt(regs)) {
+               switch (sbi_mstatus_prev_mode(regs->mstatus)) {
+               case PRV_M:
+                       return __riscv_xlen;
+               case PRV_S:
+                       return MXL_TO_XLEN(sbi_mstatus_sxl(regs->mstatus));
+               case PRV_U:
+                       return MXL_TO_XLEN(sbi_mstatus_uxl(regs->mstatus));
+               default:
+                       __builtin_unreachable();
+               }
+       }
+
+       /* V=1, Check HSXLEN first */
+       if (sbi_mstatus_sxl(regs->mstatus) < MXL_XLEN_64)
+               return 32;
+
+       hstatus = csr_read(CSR_HSTATUS);
+       /* Check VSXLEN */
+       if (sbi_hstatus_vsxl(hstatus) < MXL_XLEN_64)
+               return 32;
+
+       vsstatus = csr_read(CSR_VSSTATUS);
+       switch (sbi_mstatus_prev_mode(regs->mstatus)) {
+               case PRV_S:
+                       return MXL_TO_XLEN(sbi_hstatus_vsxl(hstatus));
+               case PRV_U:
+                       return MXL_TO_XLEN(sbi_mstatus_uxl(vsstatus));
+       }
+       __builtin_unreachable();
+}
+#endif
+
 int sbi_trap_redirect(struct sbi_trap_regs *regs,
                      const struct sbi_trap_info *trap);
 
--

2.34.1

These macros can be used to decode rd' and set rd' in RVC instructions

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

---
 include/sbi/riscv_encoding.h |  1 +
 include/sbi/riscv_fp.h       | 24 ++++++++++++++++++------
 include/sbi/sbi_trap.h       |  1 +
 3 files changed, 20 insertions(+), 6 deletions(-)

include/sbi/riscv_encoding.h +1 -0

index 8ab59abe..064ba9d1 100644
@@ -1414,6 +1414,7 @@
 #define GET_RS2S_NUM(insn)             RVC_RS2S(insn)
 #define GET_RS2C_NUM(insn)             RVC_RS2(insn)
 #define GET_RD_NUM(insn)               ((insn & MASK_RD) >> SH_RD)
+#define GET_RDS_NUM(insn)              RVC_RS2S(insn)
 #define GET_CSR_NUM(insn)              ((insn & MASK_CSR) >> SHIFT_CSR)
 #define GET_AQRL(insn)                 ((insn & MASK_AQRL) >> SHIFT_AQRL)
 

include/sbi/riscv_fp.h +18 -6

index f523c56e..03011af9 100644
@@ -91,15 +91,27 @@
 #define GET_F64_RS1(insn, regs) (GET_F64_REG(insn, 15, regs))
 #define GET_F64_RS2(insn, regs) (GET_F64_REG(insn, 20, regs))
 #define GET_F64_RS3(insn, regs) (GET_F64_REG(insn, 27, regs))
-#define SET_F32_RD(insn, regs, val) \
-       (SET_F32_REG(insn, 7, regs, val), SET_FS_DIRTY(regs))
-#define SET_F64_RD(insn, regs, val) \
-       (SET_F64_REG(insn, 7, regs, val), SET_FS_DIRTY(regs))
+#define SET_F32_RD(insn, regs, val) do {       \
+       SET_F32_REG(insn, 7, regs, val);        \
+       SET_FS_DIRTY(regs);                     \
+} while(0)
+#define SET_F64_RD(insn, regs, val) do {       \
+       SET_F64_REG(insn, 7, regs, val);        \
+       SET_FS_DIRTY(regs);                     \
+} while(0)
 
 #define GET_F32_RS2C(insn, regs) (GET_F32_REG(insn, 2, regs))
-#define GET_F32_RS2S(insn, regs) (GET_F32_REG(RVC_RS2S(insn), 0, regs))
+#define GET_F32_RS2S(insn, regs) (GET_F32_REG(GET_RS2S_NUM(insn), 0, regs))
 #define GET_F64_RS2C(insn, regs) (GET_F64_REG(insn, 2, regs))
-#define GET_F64_RS2S(insn, regs) (GET_F64_REG(RVC_RS2S(insn), 0, regs))
+#define GET_F64_RS2S(insn, regs) (GET_F64_REG(GET_RS2S_NUM(insn), 0, regs))
+#define SET_F32_RDS(insn, regs, val) do {              \
+       SET_F32_REG(GET_RDS_NUM(insn), 0, regs, val);   \
+       SET_FS_DIRTY(regs);                             \
+} while(0)
+#define SET_F64_RDS(insn, regs, val) do {              \
+       SET_F64_REG(GET_RDS_NUM(insn), 0, regs, val);   \
+       SET_FS_DIRTY(regs);                             \
+} while(0)
 
 #endif
 

include/sbi/sbi_trap.h +1 -1

index 4ef672f4..816e93c8 100644
@@ -218,6 +218,7 @@ _Static_assert(
 #define GET_RS2S(insn, regs)           REG_VAL(GET_RS2S_NUM(insn), regs)
 #define GET_RS2C(insn, regs)           REG_VAL(GET_RS2C_NUM(insn), regs)
 #define SET_RD(insn, regs, val)                (REG_VAL(GET_RD_NUM(insn), regs) = (val))
+#define SET_RDS(insn, regs, val)       (REG_VAL(GET_RDS_NUM(insn), regs) = (val))
 
 /** Representation of trap details */
 struct sbi_trap_info {
--

2.34.1

According to Privileged ISA 19.2.11:

Modifying the floating-point state when V=1 causes both fields (vsstatus.FS and the HS-level sstatus.FS) to be set to 3 (Dirty)

Fixes 130e65dd9d44 ("lib: sbi: Implement SET_FS_DIRTY() to make sure the mstatus FS dirty is set")

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

---
 include/sbi/riscv_fp.h | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

include/sbi/riscv_fp.h +5 -2

index 03011af9..ff1dbc87 100644
@@ -83,7 +83,11 @@
 #define GET_FFLAGS() csr_read(CSR_FFLAGS)
 #define SET_FFLAGS(value) csr_write(CSR_FFLAGS, (value))
 
-#define SET_FS_DIRTY(regs) (regs->mstatus |= MSTATUS_FS)
+#define SET_FS_DIRTY(regs) do {                                \
+       if (sbi_regs_from_virt(regs))                   \
+               csr_set(CSR_VSSTATUS, MSTATUS_FS);      \
+       regs->mstatus |= MSTATUS_FS;                    \
+} while(0)
 
 #define GET_F32_RS1(insn, regs) (GET_F32_REG(insn, 15, regs))
 #define GET_F32_RS2(insn, regs) (GET_F32_REG(insn, 20, regs))
--

2.34.1

It's wrong to override the emulator callback in sbi_trap_emulate_load/ store. The function must respect the callback function passed in the parameter. Hence, let the misaligned emulator callback decide when to use sbi_misaligned_v_ld/st_emulator. To clean up things, also make the following changes:

- Add the insn parameter to the callback. The trapping insn could have been fetched by the caller already, thus it saves some time in the callback. Also the tcntx is added, in case the callback needs richer information to properly handle the trap. Specifically, the callback can utilize the tinst if it knows how to decode custom values.

- Clarify that the read/write length (rlen/wlen) can be 0, in which case it could be a vector load/store or some customized instruction. The callback is responsible to handle it by parsing the insn (and fetch it if not available), and act accordingly.

Also fixes the following issue in the sbi_misaligned_v_ld/st_emulator

a. Not checking if sbi_get_insn has faulted
b. Need to redirect the trap when OPENSBI_CC_SUPPORT_VECTOR is not
available.

Fixes c2acc5e5b0d8 ("lib: sbi_misaligned_ldst: Add handling of vector load/store")

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

---
 include/sbi/sbi_platform.h  |  92 ++++++++++++++++++++----------
 include/sbi/sbi_trap_ldst.h |   4 +-
 lib/sbi/sbi_trap_ldst.c     | 111 ++++++++++++++++++++++++------------
 lib/sbi/sbi_trap_v_ldst.c   |  25 ++++----
 4 files changed, 150 insertions(+), 82 deletions(-)

include/sbi/sbi_platform.h +62 -30

index e65d9877..d3b2f9ad 100644
@@ -137,11 +137,14 @@ struct sbi_platform_operations {
                                   struct sbi_ecall_return *out);
 
        /** platform specific handler to fixup load fault */
-       int (*emulate_load)(int rlen, unsigned long addr,
-                           union sbi_ldst_data *out_val);
+       int (*emulate_load)(ulong insn, int rlen, ulong addr,
+                           union sbi_ldst_data *out_val,
+                           struct sbi_trap_context *tcntx);
+
        /** platform specific handler to fixup store fault */
-       int (*emulate_store)(int wlen, unsigned long addr,
-                            union sbi_ldst_data in_val);
+       int (*emulate_store)(ulong insn, int wlen, ulong addr,
+                            union sbi_ldst_data in_val,
+                            struct sbi_trap_context *tcntx);
 
        /** platform specific pmp setup on current HART */
        void (*pmp_set)(unsigned int n, unsigned long flags,

@@ -612,45 +615,74 @@ static inline int sbi_platform_vendor_ext_provider(
 }
 
 /**
- * Ask platform to emulate the trapped load
- *
- * @param plat pointer to struct sbi_platform
- * @param rlen length of the load: 1/2/4/8...
- * @param addr virtual address of the load. Platform needs to page-walk and
- *        find the physical address if necessary
- * @param out_val value loaded
- *
- * @return 0 on success and negative error code on failure
+ * Ask platform to emulate the trapped load:
+ *
+ * @param insn the instruction that caused the load fault (optional)
+ *             If 0, it's not fetched by caller, and the emulator can
+ *             fetch it on a need basis.
+ * @param rlen read length in [0, 1, 2, 4, 8]. If 0, it's a special load.
+ *             In that case, it could be a vector load or customized insn,
+ *             which may read/gather a block of memory. The emulator should
+ *             further parse the @insn (fetch if 0), and act accordingly.
+ * @param raddr read address. If @rlen is not 0, it's the base address of
+ *              the load. It doesn't necessarily match tcntx->trap->tval,
+ *              in case of unaligned load triggering access fault.
+ *              If @rlen is 0, @raddr should be ignored.
+ * @param out_val the buffer to hold data loaded by the emulator.
+ *                If @rlen == 0, @out_val is ignored by caller.
+ * @param tcntx trap context saved on load fault entry.
+ *
+ * @return >0 success: register will be updated by caller if @rlen != 0,
+ *            and mepc will be advanced by caller.
+ *         0  success: no register modification; no mepc advancement.
+ *         <0 failure
+ *
+ * It's expected that if @rlen != 0, and the emulator returns >0, the
+ * caller will set the corresponding registers with @out_val to simplify
+ * things. Otherwise, no register manipulation is done by the caller.
  */
 static inline int sbi_platform_emulate_load(const struct sbi_platform *plat,
-                                           int rlen, unsigned long addr,
-                                           union sbi_ldst_data *out_val)
+                                           ulong insn, int rlen, ulong raddr,
+                                           union sbi_ldst_data *out_val,
+                                           struct sbi_trap_context *tcntx)
 {
        if (plat && sbi_platform_ops(plat)->emulate_load) {
-               return sbi_platform_ops(plat)->emulate_load(rlen, addr,
-                                                           out_val);
+               return sbi_platform_ops(plat)->emulate_load(insn, rlen, raddr,
+                                                           out_val, tcntx);
        }
        return SBI_ENOTSUPP;
 }
 
 /**
- * Ask platform to emulate the trapped store
- *
- * @param plat pointer to struct sbi_platform
- * @param wlen length of the store: 1/2/4/8...
- * @param addr virtual address of the store. Platform needs to page-walk and
- *        find the physical address if necessary
- * @param in_val value to store
- *
- * @return 0 on success and negative error code on failure
+ * Ask platform to emulate the trapped store:
+ *
+ * @param insn the instruction that caused the store fault (optional).
+ *             If 0, it's not fetched by caller, and the emulator can
+ *             fetch it on a need basis.
+ * @param wlen write length in [0, 1, 2, 4, 8]. If 0, it's a special store.
+ *             In that case, it could be a vector store or customized insn,
+ *             which may write/scatter a block of memory. The emulator should
+ *             further parse the @insn (fetch if 0), and act accordingly.
+ * @param waddr write address. If @wlen is not 0, it's the base address of
+ *              the store. It doesn't necessarily match tcntx->trap->tval,
+ *              in case of unaligned store triggering access fault.
+ *              If @wlen is 0, @waddr should be ignored.
+ * @param in_val the buffer to hold data about to be stored by the emulator.
+ *               If @wlen == 0, @in_val should be ignored.
+ * @param tcntx trap context saved on store fault entry.
+ *
+ * @return >0 success: mepc will be advanced by caller.
+ *         0  success: no mepc advancement.
+ *         <0 failure
  */
 static inline int sbi_platform_emulate_store(const struct sbi_platform *plat,
-                                            int wlen, unsigned long addr,
-                                            union sbi_ldst_data in_val)
+                                            ulong insn, int wlen, ulong waddr,
+                                            union sbi_ldst_data in_val,
+                                            struct sbi_trap_context *tcntx)
 {
        if (plat && sbi_platform_ops(plat)->emulate_store) {
-               return sbi_platform_ops(plat)->emulate_store(wlen, addr,
-                                                            in_val);
+               return sbi_platform_ops(plat)->emulate_store(insn, wlen, waddr,
+                                                            in_val, tcntx);
        }
        return SBI_ENOTSUPP;
 }

include/sbi/sbi_trap_ldst.h +2 -2

index a6a6c75b..33c348c5 100644
@@ -31,10 +31,10 @@ 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(int rlen, union sbi_ldst_data *out_val,
+int sbi_misaligned_v_ld_emulator(ulong insn,
                                 struct sbi_trap_context *tcntx);
 
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
+int sbi_misaligned_v_st_emulator(ulong insn,
                                 struct sbi_trap_context *tcntx);
 
 #endif

lib/sbi/sbi_trap_ldst.c +9 -7

index 448406b1..22c4d5a7 100644
@@ -18,18 +18,18 @@
 
 /**
  * Load emulator callback:
- *
- * @return rlen=success, 0=success w/o regs modification, or negative error
+ *   Refer to comments of `sbi_platform_emulate_load`.
  */
-typedef int (*sbi_trap_ld_emulator)(int rlen, union sbi_ldst_data *out_val,
+typedef int (*sbi_trap_ld_emulator)(ulong insn, int rlen, ulong raddr,
+                                   union sbi_ldst_data *out_val,
                                    struct sbi_trap_context *tcntx);
 
 /**
  * Store emulator callback:
- *
- * @return wlen=success, 0=success w/o regs modification, or negative error
+ *   Refer to comments of `sbi_platform_emulate_store`.
  */
-typedef int (*sbi_trap_st_emulator)(int wlen, union sbi_ldst_data in_val,
+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,

@@ -52,13 +52,15 @@ static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
        ulong insn, insn_len;
        union sbi_ldst_data val = { 0 };
        struct sbi_trap_info uptrap;
-       int rc, fp = 0, shift = 0, len = 0, vector = 0;
+       int rc, fp = 0, shift = 0, len = 0;
+       bool xform = false;
 
        if (orig_trap->tinst & 0x1) {
                /*
                 * Bit[0] == 1 implies trapped instruction value is
                 * transformed instruction or custom instruction.
                 */
+               xform    = true;
                insn     = orig_trap->tinst | INSN_16BIT_MASK;
                insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
        } else {

@@ -144,27 +146,20 @@ static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
                len = 2;
                shift = 8 * (sizeof(ulong) - len);

lib/sbi/sbi_trap_ldst.c +8 -8

        if (rc <= 0)
                return rc;
 
-       if (!vector) {
-               if (!fp)
-                       SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
+       if (!fp)
+               SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
 #ifdef __riscv_flen
-               else if (len == 8)
-                       SET_F64_RD(insn, regs, val.data_u64);
-               else
-                       SET_F32_RD(insn, regs, val.data_ulong);
+       else if (len == 8)
+               SET_F64_RD(insn, regs, val.data_u64);
+       else
+               SET_F32_RD(insn, regs, val.data_ulong);
 #endif
-       }
 
        regs->mepc += insn_len;
 

@@ -180,12 +175,14 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
        union sbi_ldst_data val;
        struct sbi_trap_info uptrap;
        int rc, len = 0;
+       bool xform = false;
 
        if (orig_trap->tinst & 0x1) {
                /*
                 * Bit[0] == 1 implies trapped instruction value is
                 * transformed instruction or custom instruction.
                 */
+               xform    = true;
                insn     = orig_trap->tinst | INSN_16BIT_MASK;
                insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
        } else {

@@ -253,13 +250,9 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
        } else if ((insn & INSN_MASK_C_SH) == INSN_MATCH_C_SH) {

lib/sbi/sbi_trap_ldst.c +2 -1

        if (rc <= 0)
                return rc;
 

@@ -268,7 +261,8 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
        return 0;
 }
 
-static int sbi_misaligned_ld_emulator(int rlen, union sbi_ldst_data *out_val,
+static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr,
+                                     union sbi_ldst_data *out_val,
                                      struct sbi_trap_context *tcntx)
 {
        const struct sbi_trap_info *orig_trap = &tcntx->trap;

lib/sbi/sbi_trap_ldst.c +13 -2

+               if (IS_VECTOR_LOAD_STORE(insn))
+                       return sbi_misaligned_v_ld_emulator(insn, tcntx);
+               else
+                       /* Unrecognized instruction. Can't emulate it. */
+                       return sbi_trap_redirect(regs, orig_trap);
+       }
+       /* For misaligned fault, addr must be the same as orig_trap->tval */
+       if (addr != orig_trap->tval)
+               return SBI_EFAIL;
+
        for (i = 0; i < rlen; i++) {
                out_val->data_bytes[i] =
-                       sbi_load_u8((void *)(orig_trap->tval + i), &uptrap);
+                       sbi_load_u8((void *)(addr + i), &uptrap);
                if (uptrap.cause) {
                        uptrap.tinst = sbi_misaligned_tinst_fixup(
                                orig_trap->tinst, uptrap.tinst, i);

@@ -293,7 +303,8 @@ int sbi_misaligned_load_handler(struct sbi_trap_context *tcntx)
        return sbi_trap_emulate_load(tcntx, sbi_misaligned_ld_emulator);
 }
 
-static int sbi_misaligned_st_emulator(int wlen, union sbi_ldst_data in_val,
+static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr,
+                                     union sbi_ldst_data in_val,
                                      struct sbi_trap_context *tcntx)
 {
        const struct sbi_trap_info *orig_trap = &tcntx->trap;

lib/sbi/sbi_trap_ldst.c +27 -9

+               if (IS_VECTOR_LOAD_STORE(insn))
+                       return sbi_misaligned_v_st_emulator(insn, tcntx);
+               else
+                       /* Unrecognized instruction. Can't emulate it. */
+                       return sbi_trap_redirect(regs, orig_trap);
+       }
+       /* For misaligned fault, addr must be the same as orig_trap->tval */
+       if (addr != orig_trap->tval)
+               return SBI_EFAIL;
+
        for (i = 0; i < wlen; i++) {
-               sbi_store_u8((void *)(orig_trap->tval + i),
+               sbi_store_u8((void *)(addr + i),
                             in_val.data_bytes[i], &uptrap);
                if (uptrap.cause) {
                        uptrap.tinst = sbi_misaligned_tinst_fixup(

@@ -318,22 +345,26 @@ int sbi_misaligned_store_handler(struct sbi_trap_context *tcntx)
        return sbi_trap_emulate_store(tcntx, sbi_misaligned_st_emulator);
 }
 
-static int sbi_ld_access_emulator(int rlen, union sbi_ldst_data *out_val,
+static int sbi_ld_access_emulator(ulong insn, int rlen, ulong addr,
+                                 union sbi_ldst_data *out_val,
                                  struct sbi_trap_context *tcntx)
 {
        const struct sbi_trap_info *orig_trap = &tcntx->trap;
        struct sbi_trap_regs *regs = &tcntx->regs;
+       int rc;
 
        /* If fault came from M mode, just fail */
        if (sbi_mstatus_prev_mode(regs->mstatus) == PRV_M)
                return SBI_EINVAL;
 
+       rc = sbi_platform_emulate_load(sbi_platform_thishart_ptr(),
+                                      insn, rlen, addr, out_val, tcntx);
+
        /* If platform emulator failed, we redirect instead of fail */
-       if (sbi_platform_emulate_load(sbi_platform_thishart_ptr(), rlen,
-                                     orig_trap->tval, out_val))
+       if (rc < 0)
                return sbi_trap_redirect(regs, orig_trap);
 
-       return rlen;
+       return rc;
 }
 
 int sbi_load_access_handler(struct sbi_trap_context *tcntx)

@@ -341,22 +372,26 @@ int sbi_load_access_handler(struct sbi_trap_context *tcntx)
        return sbi_trap_emulate_load(tcntx, sbi_ld_access_emulator);
 }
 
-static int sbi_st_access_emulator(int wlen, union sbi_ldst_data in_val,
+static int sbi_st_access_emulator(ulong insn, int wlen, ulong addr,
+                                 union sbi_ldst_data in_val,
                                  struct sbi_trap_context *tcntx)
 {
        const struct sbi_trap_info *orig_trap = &tcntx->trap;
        struct sbi_trap_regs *regs = &tcntx->regs;
+       int rc;
 
        /* If fault came from M mode, just fail */
        if (sbi_mstatus_prev_mode(regs->mstatus) == PRV_M)
                return SBI_EINVAL;
 
+       rc = sbi_platform_emulate_store(sbi_platform_thishart_ptr(),
+                                       insn, wlen, addr, in_val, tcntx);
+
        /* If platform emulator failed, we redirect instead of fail */
-       if (sbi_platform_emulate_store(sbi_platform_thishart_ptr(), wlen,
-                                      orig_trap->tval, in_val))
+       if (rc < 0)
                return sbi_trap_redirect(regs, orig_trap);
 
-       return wlen;
+       return rc;
 }
 
 int sbi_store_access_handler(struct sbi_trap_context *tcntx)

lib/sbi/sbi_trap_v_ldst.c +13 -13

index f4d469dc..f361ce04 100644
@@ -137,13 +137,11 @@ static inline void vsetvl(ulong vl, ulong vtype)
                        :: "r" (vl), "r" (vtype));
 }
 
-int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
-                                struct sbi_trap_context *tcntx)
+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 insn = sbi_get_insn(regs->mepc, &uptrap);
        ulong vl = csr_read(CSR_VL);
        ulong vtype = csr_read(CSR_VTYPE);
        ulong vlenb = csr_read(CSR_VLENB);

@@ -237,13 +235,11 @@ int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
        return vl;
 }
 
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
-                                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 insn = sbi_get_insn(regs->mepc, &uptrap);
        ulong vl = csr_read(CSR_VL);
        ulong vtype = csr_read(CSR_VTYPE);
        ulong vlenb = csr_read(CSR_VLENB);

@@ -331,14 +327,19 @@ int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
        return vl;
 }
 #else
-int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
-                                struct sbi_trap_context *tcntx)
+int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
 {
-       return 0;
+       const struct sbi_trap_info *orig_trap = &tcntx->trap;
+       struct sbi_trap_regs *regs = &tcntx->regs;
+
+       return sbi_trap_redirect(regs, orig_trap);
 }
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
-                                struct sbi_trap_context *tcntx)
+
+int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx)
 {
-       return 0;
+       const struct sbi_trap_info *orig_trap = &tcntx->trap;
+       struct sbi_trap_regs *regs = &tcntx->regs;
+
+       return sbi_trap_redirect(regs, orig_trap);
 }
 #endif /* OPENSBI_CC_SUPPORT_VECTOR */
--

2.34.1

Quoted 224 lines · Bo Gan

It's wrong to override the emulator callback in sbi_trap_emulate_load/ store. The function must respect the callback function passed in the parameter. Hence, let the misaligned emulator callback decide when to use sbi_misaligned_v_ld/st_emulator. To clean up things, also make the following changes:

- Add the insn parameter to the callback. The trapping insn could have been fetched by the caller already, thus it saves some time in the callback. Also the tcntx is added, in case the callback needs richer information to properly handle the trap. Specifically, the callback can utilize the tinst if it knows how to decode custom values.

- Clarify that the read/write length (rlen/wlen) can be 0, in which case it could be a vector load/store or some customized instruction. The callback is responsible to handle it by parsing the insn (and fetch it if not available), and act accordingly.

Also fixes the following issue in the sbi_misaligned_v_ld/st_emulator

a. Not checking if sbi_get_insn has faulted
b. Need to redirect the trap when OPENSBI_CC_SUPPORT_VECTOR is not
available.

Fixes c2acc5e5b0d8 ("lib: sbi_misaligned_ldst: Add handling of vector load/store")

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

---
  include/sbi/sbi_platform.h  |  92 ++++++++++++++++++++----------
  include/sbi/sbi_trap_ldst.h |   4 +-
  lib/sbi/sbi_trap_ldst.c     | 111 ++++++++++++++++++++++++------------
  lib/sbi/sbi_trap_v_ldst.c   |  25 ++++----
  4 files changed, 150 insertions(+), 82 deletions(-)

include/sbi/sbi_platform.h +62 -30

index e65d9877..d3b2f9ad 100644
@@ -137,11 +137,14 @@ struct sbi_platform_operations {
                                    struct sbi_ecall_return *out);
 
         /** platform specific handler to fixup load fault */
-       int (*emulate_load)(int rlen, unsigned long addr,
-                           union sbi_ldst_data *out_val);
+       int (*emulate_load)(ulong insn, int rlen, ulong addr,
+                           union sbi_ldst_data *out_val,
+                           struct sbi_trap_context *tcntx);
+
         /** platform specific handler to fixup store fault */
-       int (*emulate_store)(int wlen, unsigned long addr,
-                            union sbi_ldst_data in_val);
+       int (*emulate_store)(ulong insn, int wlen, ulong addr,
+                            union sbi_ldst_data in_val,
+                            struct sbi_trap_context *tcntx);
 
         /** platform specific pmp setup on current HART */
         void (*pmp_set)(unsigned int n, unsigned long flags,

@@ -612,45 +615,74 @@ static inline int sbi_platform_vendor_ext_provider(
  }
 
  /**
- * Ask platform to emulate the trapped load
- *
- * @param plat pointer to struct sbi_platform
- * @param rlen length of the load: 1/2/4/8...
- * @param addr virtual address of the load. Platform needs to page-walk and
- *        find the physical address if necessary
- * @param out_val value loaded
- *
- * @return 0 on success and negative error code on failure
+ * Ask platform to emulate the trapped load:
+ *
+ * @param insn the instruction that caused the load fault (optional)
+ *             If 0, it's not fetched by caller, and the emulator can
+ *             fetch it on a need basis.
+ * @param rlen read length in [0, 1, 2, 4, 8]. If 0, it's a special load.
+ *             In that case, it could be a vector load or customized insn,
+ *             which may read/gather a block of memory. The emulator should
+ *             further parse the @insn (fetch if 0), and act accordingly.
+ * @param raddr read address. If @rlen is not 0, it's the base address of
+ *              the load. It doesn't necessarily match tcntx->trap->tval,
+ *              in case of unaligned load triggering access fault.
+ *              If @rlen is 0, @raddr should be ignored.
+ * @param out_val the buffer to hold data loaded by the emulator.
+ *                If @rlen == 0, @out_val is ignored by caller.
+ * @param tcntx trap context saved on load fault entry.
+ *
+ * @return >0 success: register will be updated by caller if @rlen != 0,
+ *            and mepc will be advanced by caller.
+ *         0  success: no register modification; no mepc advancement.
+ *         <0 failure
+ *
+ * It's expected that if @rlen != 0, and the emulator returns >0, the
+ * caller will set the corresponding registers with @out_val to simplify
+ * things. Otherwise, no register manipulation is done by the caller.
   */
  static inline int sbi_platform_emulate_load(const struct sbi_platform *plat,
-                                           int rlen, unsigned long addr,
-                                           union sbi_ldst_data *out_val)
+                                           ulong insn, int rlen, ulong raddr,
+                                           union sbi_ldst_data *out_val,
+                                           struct sbi_trap_context *tcntx)
  {
         if (plat && sbi_platform_ops(plat)->emulate_load) {
-               return sbi_platform_ops(plat)->emulate_load(rlen, addr,
-                                                           out_val);
+               return sbi_platform_ops(plat)->emulate_load(insn, rlen, raddr,
+                                                           out_val, tcntx);
         }
         return SBI_ENOTSUPP;
  }
 
  /**
- * Ask platform to emulate the trapped store
- *
- * @param plat pointer to struct sbi_platform
- * @param wlen length of the store: 1/2/4/8...
- * @param addr virtual address of the store. Platform needs to page-walk and
- *        find the physical address if necessary
- * @param in_val value to store
- *
- * @return 0 on success and negative error code on failure
+ * Ask platform to emulate the trapped store:
+ *
+ * @param insn the instruction that caused the store fault (optional).
+ *             If 0, it's not fetched by caller, and the emulator can
+ *             fetch it on a need basis.
+ * @param wlen write length in [0, 1, 2, 4, 8]. If 0, it's a special store.
+ *             In that case, it could be a vector store or customized insn,
+ *             which may write/scatter a block of memory. The emulator should
+ *             further parse the @insn (fetch if 0), and act accordingly.
+ * @param waddr write address. If @wlen is not 0, it's the base address of
+ *              the store. It doesn't necessarily match tcntx->trap->tval,
+ *              in case of unaligned store triggering access fault.
+ *              If @wlen is 0, @waddr should be ignored.
+ * @param in_val the buffer to hold data about to be stored by the emulator.
+ *               If @wlen == 0, @in_val should be ignored.
+ * @param tcntx trap context saved on store fault entry.
+ *
+ * @return >0 success: mepc will be advanced by caller.
+ *         0  success: no mepc advancement.
+ *         <0 failure
   */
  static inline int sbi_platform_emulate_store(const struct sbi_platform *plat,
-                                            int wlen, unsigned long addr,
-                                            union sbi_ldst_data in_val)
+                                            ulong insn, int wlen, ulong waddr,
+                                            union sbi_ldst_data in_val,
+                                            struct sbi_trap_context *tcntx)
  {
         if (plat && sbi_platform_ops(plat)->emulate_store) {
-               return sbi_platform_ops(plat)->emulate_store(wlen, addr,
-                                                            in_val);
+               return sbi_platform_ops(plat)->emulate_store(insn, wlen, waddr,
+                                                            in_val, tcntx);
         }
         return SBI_ENOTSUPP;
  }

include/sbi/sbi_trap_ldst.h +2 -2

index a6a6c75b..33c348c5 100644
@@ -31,10 +31,10 @@ 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(int rlen, union sbi_ldst_data *out_val,
+int sbi_misaligned_v_ld_emulator(ulong insn,
                                  struct sbi_trap_context *tcntx);
 
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
+int sbi_misaligned_v_st_emulator(ulong insn,
                                  struct sbi_trap_context *tcntx);
 
  #endif

lib/sbi/sbi_trap_ldst.c +9 -7

index 448406b1..22c4d5a7 100644
@@ -18,18 +18,18 @@
 
  /**
   * Load emulator callback:
- *
- * @return rlen=success, 0=success w/o regs modification, or negative error
+ *   Refer to comments of `sbi_platform_emulate_load`.
   */
-typedef int (*sbi_trap_ld_emulator)(int rlen, union sbi_ldst_data *out_val,
+typedef int (*sbi_trap_ld_emulator)(ulong insn, int rlen, ulong raddr,
+                                   union sbi_ldst_data *out_val,
                                     struct sbi_trap_context *tcntx);
 
  /**
   * Store emulator callback:
- *
- * @return wlen=success, 0=success w/o regs modification, or negative error
+ *   Refer to comments of `sbi_platform_emulate_store`.
   */
-typedef int (*sbi_trap_st_emulator)(int wlen, union sbi_ldst_data in_val,
+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,

@@ -52,13 +52,15 @@ static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
         ulong insn, insn_len;
         union sbi_ldst_data val = { 0 };
         struct sbi_trap_info uptrap;
-       int rc, fp = 0, shift = 0, len = 0, vector = 0;
+       int rc, fp = 0, shift = 0, len = 0;
+       bool xform = false;
 
         if (orig_trap->tinst & 0x1) {
                 /*
                  * Bit[0] == 1 implies trapped instruction value is
                  * transformed instruction or custom instruction.
                  */
+               xform    = true;
                 insn     = orig_trap->tinst | INSN_16BIT_MASK;
                 insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
         } else {

@@ -144,27 +146,20 @@ static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
                 len = 2;
                 shift = 8 * (sizeof(ulong) - len);

lib/sbi/sbi_trap_ldst.c static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx, · line 151

Quoted 10 lines

lib/sbi/sbi_trap_ldst.c +1 -6

                 insn = RVC_RS2S(insn) << SH_RD;
-       } else if (IS_VECTOR_LOAD_STORE(insn)) {
-               vector = 1;
-               emu = sbi_misaligned_v_ld_emulator;
-       } else {
-               return sbi_trap_redirect(regs, orig_trap);
         }
 
-       rc = emu(len, &val, tcntx);
+       rc = emu(xform ? 0 : insn, len, orig_trap->tval, &val, tcntx);

Drop the "xform" and always pass insn

Quoted 39 lines

lib/sbi/sbi_trap_ldst.c +8 -8

         if (rc <= 0)
                 return rc;
 
-       if (!vector) {
-               if (!fp)
-                       SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
+       if (!fp)
+               SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
  #ifdef __riscv_flen
-               else if (len == 8)
-                       SET_F64_RD(insn, regs, val.data_u64);
-               else
-                       SET_F32_RD(insn, regs, val.data_ulong);
+       else if (len == 8)
+               SET_F64_RD(insn, regs, val.data_u64);
+       else
+               SET_F32_RD(insn, regs, val.data_ulong);
  #endif
-       }
 
         regs->mepc += insn_len;
 

@@ -180,12 +175,14 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
         union sbi_ldst_data val;
         struct sbi_trap_info uptrap;
         int rc, len = 0;
+       bool xform = false;
 
         if (orig_trap->tinst & 0x1) {
                 /*
                  * Bit[0] == 1 implies trapped instruction value is
                  * transformed instruction or custom instruction.
                  */
+               xform    = true;
                 insn     = orig_trap->tinst | INSN_16BIT_MASK;
                 insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
         } else {

@@ -253,13 +250,9 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
         } else if ((insn & INSN_MASK_C_SH) == INSN_MATCH_C_SH) {

lib/sbi/sbi_trap_ldst.c static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx, · line 255

Quoted 10 lines

lib/sbi/sbi_trap_ldst.c +1 -5

                 len             = 2;
                 val.data_ulong = GET_RS2S(insn, regs);
-       } else if (IS_VECTOR_LOAD_STORE(insn)) {
-               emu = sbi_misaligned_v_st_emulator;
-       } else {
-               return sbi_trap_redirect(regs, orig_trap);
         }
 
-       rc = emu(len, val, tcntx);
+       rc = emu(xform ? 0 : insn, len, orig_trap->tval, val, tcntx);

Same as above.

Quoted 14 lines

lib/sbi/sbi_trap_ldst.c +2 -1

         if (rc <= 0)
                 return rc;
 

@@ -268,7 +261,8 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
         return 0;
  }
 
-static int sbi_misaligned_ld_emulator(int rlen, union sbi_ldst_data *out_val,
+static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr,
+                                     union sbi_ldst_data *out_val,
                                       struct sbi_trap_context *tcntx)
  {
         const struct sbi_trap_info *orig_trap = &tcntx->trap;

lib/sbi/sbi_trap_ldst.c static int sbi_misaligned_ld_emulator(int rlen, union sbi_ldst_data *out_val, · line 278

Quoted 9 lines

lib/sbi/sbi_trap_ldst.c +6 -0

@@ -276,9 +270,25 @@ static int sbi_misaligned_ld_emulator(int rlen, union sbi_ldst_data *out_val,
         struct sbi_trap_info uptrap;
         int i;
 
+       if (!rlen) {
+               if (!insn) {
+                       insn = sbi_get_insn(regs->mepc, &uptrap);
+                       if (uptrap.cause)
+                               return sbi_trap_redirect(regs, &uptrap);
+               }

The above is not needed because sbi_trap_emulate_load() and sbi_trap_emulate_store() already does sbi_get_insn().

Quoted 28 lines

lib/sbi/sbi_trap_ldst.c +13 -2

+               if (IS_VECTOR_LOAD_STORE(insn))
+                       return sbi_misaligned_v_ld_emulator(insn, tcntx);
+               else
+                       /* Unrecognized instruction. Can't emulate it. */
+                       return sbi_trap_redirect(regs, orig_trap);
+       }
+       /* For misaligned fault, addr must be the same as orig_trap->tval */
+       if (addr != orig_trap->tval)
+               return SBI_EFAIL;
+
         for (i = 0; i < rlen; i++) {
                 out_val->data_bytes[i] =
-                       sbi_load_u8((void *)(orig_trap->tval + i), &uptrap);
+                       sbi_load_u8((void *)(addr + i), &uptrap);
                 if (uptrap.cause) {
                         uptrap.tinst = sbi_misaligned_tinst_fixup(
                                 orig_trap->tinst, uptrap.tinst, i);

@@ -293,7 +303,8 @@ int sbi_misaligned_load_handler(struct sbi_trap_context *tcntx)
         return sbi_trap_emulate_load(tcntx, sbi_misaligned_ld_emulator);
  }
 
-static int sbi_misaligned_st_emulator(int wlen, union sbi_ldst_data in_val,
+static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr,
+                                     union sbi_ldst_data in_val,
                                       struct sbi_trap_context *tcntx)
  {
         const struct sbi_trap_info *orig_trap = &tcntx->trap;

lib/sbi/sbi_trap_ldst.c static int sbi_misaligned_st_emulator(int wlen, union sbi_ldst_data in_val, · line 320

Quoted 9 lines

lib/sbi/sbi_trap_ldst.c +6 -0

@@ -301,8 +312,24 @@ static int sbi_misaligned_st_emulator(int wlen, union sbi_ldst_data in_val,
         struct sbi_trap_info uptrap;
         int i;
 
+       if (!wlen) {
+               if (!insn) {
+                       insn = sbi_get_insn(regs->mepc, &uptrap);
+                       if (uptrap.cause)
+                               return sbi_trap_redirect(regs, &uptrap);
+               }

same comment as above.

Quoted 139 lines

lib/sbi/sbi_trap_ldst.c +27 -9

+               if (IS_VECTOR_LOAD_STORE(insn))
+                       return sbi_misaligned_v_st_emulator(insn, tcntx);
+               else
+                       /* Unrecognized instruction. Can't emulate it. */
+                       return sbi_trap_redirect(regs, orig_trap);
+       }
+       /* For misaligned fault, addr must be the same as orig_trap->tval */
+       if (addr != orig_trap->tval)
+               return SBI_EFAIL;
+
         for (i = 0; i < wlen; i++) {
-               sbi_store_u8((void *)(orig_trap->tval + i),
+               sbi_store_u8((void *)(addr + i),
                              in_val.data_bytes[i], &uptrap);
                 if (uptrap.cause) {
                         uptrap.tinst = sbi_misaligned_tinst_fixup(

@@ -318,22 +345,26 @@ int sbi_misaligned_store_handler(struct sbi_trap_context *tcntx)
         return sbi_trap_emulate_store(tcntx, sbi_misaligned_st_emulator);
  }
 
-static int sbi_ld_access_emulator(int rlen, union sbi_ldst_data *out_val,
+static int sbi_ld_access_emulator(ulong insn, int rlen, ulong addr,
+                                 union sbi_ldst_data *out_val,
                                   struct sbi_trap_context *tcntx)
  {
         const struct sbi_trap_info *orig_trap = &tcntx->trap;
         struct sbi_trap_regs *regs = &tcntx->regs;
+       int rc;
 
         /* If fault came from M mode, just fail */
         if (sbi_mstatus_prev_mode(regs->mstatus) == PRV_M)
                 return SBI_EINVAL;
 
+       rc = sbi_platform_emulate_load(sbi_platform_thishart_ptr(),
+                                      insn, rlen, addr, out_val, tcntx);
+
         /* If platform emulator failed, we redirect instead of fail */
-       if (sbi_platform_emulate_load(sbi_platform_thishart_ptr(), rlen,
-                                     orig_trap->tval, out_val))
+       if (rc < 0)
                 return sbi_trap_redirect(regs, orig_trap);
 
-       return rlen;
+       return rc;
  }
 
  int sbi_load_access_handler(struct sbi_trap_context *tcntx)

@@ -341,22 +372,26 @@ int sbi_load_access_handler(struct sbi_trap_context *tcntx)
         return sbi_trap_emulate_load(tcntx, sbi_ld_access_emulator);
  }
 
-static int sbi_st_access_emulator(int wlen, union sbi_ldst_data in_val,
+static int sbi_st_access_emulator(ulong insn, int wlen, ulong addr,
+                                 union sbi_ldst_data in_val,
                                   struct sbi_trap_context *tcntx)
  {
         const struct sbi_trap_info *orig_trap = &tcntx->trap;
         struct sbi_trap_regs *regs = &tcntx->regs;
+       int rc;
 
         /* If fault came from M mode, just fail */
         if (sbi_mstatus_prev_mode(regs->mstatus) == PRV_M)
                 return SBI_EINVAL;
 
+       rc = sbi_platform_emulate_store(sbi_platform_thishart_ptr(),
+                                       insn, wlen, addr, in_val, tcntx);
+
         /* If platform emulator failed, we redirect instead of fail */
-       if (sbi_platform_emulate_store(sbi_platform_thishart_ptr(), wlen,
-                                      orig_trap->tval, in_val))
+       if (rc < 0)
                 return sbi_trap_redirect(regs, orig_trap);
 
-       return wlen;
+       return rc;
  }
 
  int sbi_store_access_handler(struct sbi_trap_context *tcntx)

lib/sbi/sbi_trap_v_ldst.c +13 -13

index f4d469dc..f361ce04 100644
@@ -137,13 +137,11 @@ static inline void vsetvl(ulong vl, ulong vtype)
                         :: "r" (vl), "r" (vtype));
  }
 
-int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
-                                struct sbi_trap_context *tcntx)
+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 insn = sbi_get_insn(regs->mepc, &uptrap);
         ulong vl = csr_read(CSR_VL);
         ulong vtype = csr_read(CSR_VTYPE);
         ulong vlenb = csr_read(CSR_VLENB);

@@ -237,13 +235,11 @@ int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
         return vl;
  }
 
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
-                                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 insn = sbi_get_insn(regs->mepc, &uptrap);
         ulong vl = csr_read(CSR_VL);
         ulong vtype = csr_read(CSR_VTYPE);
         ulong vlenb = csr_read(CSR_VLENB);

@@ -331,14 +327,19 @@ int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
         return vl;
  }
  #else
-int sbi_misaligned_v_ld_emulator(int rlen, union sbi_ldst_data *out_val,
-                                struct sbi_trap_context *tcntx)
+int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx)
  {
-       return 0;
+       const struct sbi_trap_info *orig_trap = &tcntx->trap;
+       struct sbi_trap_regs *regs = &tcntx->regs;
+
+       return sbi_trap_redirect(regs, orig_trap);
  }
-int sbi_misaligned_v_st_emulator(int wlen, union sbi_ldst_data in_val,
-                                struct sbi_trap_context *tcntx)
+
+int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx)
  {
-       return 0;
+       const struct sbi_trap_info *orig_trap = &tcntx->trap;
+       struct sbi_trap_regs *regs = &tcntx->regs;
+
+       return sbi_trap_redirect(regs, orig_trap);
  }
  #endif /* OPENSBI_CC_SUPPORT_VECTOR */
--

Quoted 2 lines

2.34.1

Regards,
Anup

Validation:
The patch is validated to have fixed the issue with test cases running on a modified version of QEMU that exposes misaligned faults [1], and a further modified version that removes tinst transformation [2]. The S-mode OS is a local build of Debian Trixie 6.12 kernel that enables COMPAT (RV32), and the U-mode test application exercises all integer and floating-point load/store (RVIFD64/32+RVC64/32) instructions with all possible imm values. The patch is also tested on real HW (Sifive P550/ESWIN EIC7700), which only supports RV64. On P550, the same test was validated both in U mode and VU mode, where the host runs a 6.12 ESWIN vendor kernel that has some ESWIN SoC device driver patches [3] applied, and the guest runs the exact same Debian Trixie 6.12 kernel mentioned above.

[1] https://github.com/ganboing/qemu/tree/ganboing-misalign
[2] https://github.com/ganboing/qemu/tree/ganboing-misalign-no-tinst
[3] https://github.com/sifiveinc/riscv-linux/tree/rel/kernel-6.12/hifive-premier-p550

On Tue, Feb 10, 2026 at 01:40:43AM -0800, Bo Gan wrote:
...

Quoted 14 lines

Validation:
The patch is validated to have fixed the issue with test cases running on a modified version of QEMU that exposes misaligned faults [1], and a further modified version that removes tinst transformation [2]. The S-mode OS is a local build of Debian Trixie 6.12 kernel that enables COMPAT (RV32), and the U-mode test application exercises all integer and floating-point load/store (RVIFD64/32+RVC64/32) instructions with all possible imm values. The patch is also tested on real HW (Sifive P550/ESWIN EIC7700), which only supports RV64. On P550, the same test was validated both in U mode and VU mode, where the host runs a 6.12 ESWIN vendor kernel that has some ESWIN SoC device driver patches [3] applied, and the guest runs the exact same Debian Trixie 6.12 kernel mentioned above.

Quoted 4 lines

[1] https://github.com/ganboing/qemu/tree/ganboing-misalign
[2] https://github.com/ganboing/qemu/tree/ganboing-misalign-no-tinst
[3] https://github.com/sifiveinc/riscv-linux/tree/rel/kernel-6.12/hifive-premier-p550

Hi Bo,

It'd be nice if we could integrate emulation tests into a test suite that gets run frequently in order to catch regressions. We have already started using the kvm-unit-tests[4] framework for SBI testing so adding emulation

tests there would make sense. If special QEMU behavior is needed then we should get cpu properties that enable those behaviors upstreamed so we can turn them on when running the tests.

[4] https://gitlab.com/kvm-unit-tests/kvm-unit-tests

Thanks,
drew

Hi Andrew,

Quoted 21 lines · Andrew Jones

On Tue, Feb 10, 2026 at 01:40:43AM -0800, Bo Gan wrote:
...

Quoted 14 lines

Validation:
The patch is validated to have fixed the issue with test cases running on a modified version of QEMU that exposes misaligned faults [1], and a further modified version that removes tinst transformation [2]. The S-mode OS is a local build of Debian Trixie 6.12 kernel that enables COMPAT (RV32), and the U-mode test application exercises all integer and floating-point load/store (RVIFD64/32+RVC64/32) instructions with all possible imm values. The patch is also tested on real HW (Sifive P550/ESWIN EIC7700), which only supports RV64. On P550, the same test was validated both in U mode and VU mode, where the host runs a 6.12 ESWIN vendor kernel that has some ESWIN SoC device driver patches [3] applied, and the guest runs the exact same Debian Trixie 6.12 kernel mentioned above.

Quoted 4 lines

[1] https://github.com/ganboing/qemu/tree/ganboing-misalign
[2] https://github.com/ganboing/qemu/tree/ganboing-misalign-no-tinst
[3] https://github.com/sifiveinc/riscv-linux/tree/rel/kernel-6.12/hifive-premier-p550

Quoted 5 lines

Hi Bo,

It'd be nice if we could integrate emulation tests into a test suite that gets run frequently in order to catch regressions. We have already started using the kvm-unit-tests[4] framework for SBI testing so adding emulation

Good to know. I wasn't aware of this project. I'll definitely take a look and see if I can upstream the test cases there. I guess it can be utilized to test misaligned handler in both M mode OpenSBI and the KVM SBI layer, depending on whether host Linux/KVM enables misaligned exc delegation, correct?

Quoted 4 lines

tests there would make sense. If special QEMU behavior is needed then we should get cpu properties that enable those behaviors upstreamed so we can turn them on when running the tests.

I think this is done by others for sure. I was actually inspired by this
https://lore.kernel.org/all/[email protected]/
series by Frederic to hack QEMU. Similar patches have been done by others such as Clément mentioned in this series:
https://lore.kernel.org/all/[email protected]/
I'd like to hear from folks to understand if there're any roadblocks exposing the misaligned fault in QEMU (gated by a config flag), before attempting.

Spoiler alert: Linux's misaligned handler seems to be affected by the same insn decoding issue where there's no checking of previous XLEN. I'm yet to check if Linux is affected by other issues in this patchset, too. Once this change is merged in OpenSBI, I'll also be looking into fixing Linux and adding my test cases into Linux/selftests as well.

Quoted 6 lines

[4] https://gitlab.com/kvm-unit-tests/kvm-unit-tests

Thanks,
drew

Bo

Rehaul instruction decoding to fix the following issues:

- We assume the XLEN of previous mode is the same as MXLEN. However, RVC instructions decodes differently in RV32 and RV64, so shouldn't have assumed that.
- We assume it's a misaligned fault and the load/store offset is 0, i.e., base address == fault address, but access faults can have non-0 offset (on HW supporting misaligned accesses), so platform specific load/store fault handler gets the wrong base address.
- No checking of [63:32] of tinst in RV64, which is explicitly required by Privileged ISA 19.6.3. Must reject tinst with non-0 high 32 bits.

Thus, fix all the above. For misaligned load/store fault, the address offset is always 0, thus we kill the use of base address, and use trap address instead (same as before), which lets the compiler optimize out imm parsing and other calculations.

I also analyzed the behavior of misaligned fault handler before fix. With the following conditions met, it can trigger data corruption:

- HW doesn't transform instruction into tinst.
- HW doesn't support misaligned load/store, and OS doesn't enable misaligned delegation, thus OpenSBI handler is in effect
- HW supports mixed XLEN, and M mode is running RV64, and the trapping mode (U/VS/VU) is running RV32.
- The trapping instruction is c.f{l|s}w(sp).

Due to the incorrect insn decoding, the trapping instruction would mistakenly be decoded as c.{l|s}d(sp). With this fix, c.f{l|s}w(sp) in RV32 is now emulated correctly.

Validation:
The patch is validated to have fixed the issue with test cases running on a modified version of QEMU that exposes misaligned faults [1], and a further modified version that removes tinst transformation [2]. The S-mode OS is a local build of Debian Trixie 6.12 kernel that enables COMPAT (RV32), and the U-mode test application exercises all integer and floating-point load/store (RVIFD64/32+RVC64/32) instructions with all possible imm values. The patch is also tested on real HW (Sifive P550/ESWIN EIC7700), which only supports RV64. On P550, the same test was validated both in U mode and VU mode, where the host runs a 6.12 ESWIN vendor kernel that has some ESWIN SoC device driver patches [3] applied, and the guest runs the exact same Debian Trixie 6.12 kernel mentioned above.

[1] https://github.com/ganboing/qemu/tree/ganboing-misalign
[2] https://github.com/ganboing/qemu/tree/ganboing-misalign-no-tinst
[3] https://github.com/sifiveinc/riscv-linux/tree/rel/kernel-6.12/hifive-premier-p550

Fixes 7219477f7b40 ("lib: Use MTINST CSR in misaligned load/store emulation")

Fixes b5ae8e8a650d ("lib: Add misaligned load/store trap handling")

Fixes 4c112650bbb0 ("lib: sbi: abstract out insn decoding to unify mem fault handlers")

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

---
  lib/sbi/sbi_trap_ldst.c | 427 +++++++++++++++++++++++++++-------------
  1 file changed, 295 insertions(+), 132 deletions(-)

lib/sbi/sbi_trap_ldst.c +295 -132

index 22c4d5a7..2371abca 100644
@@ -44,30 +44,34 @@ ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
  		return orig_tinst | (addr_offset << SH_RS1);
  }
 
+static inline bool sbi_trap_tinst_valid(ulong tinst)
+{
+	/*
+	 * Bit[0] == 1 implies trapped instruction value is
+	 * transformed instruction or custom instruction.
+	 * Also do proper checking per Privileged ISA 19.6.3,
+	 * and make sure high 32 bits of tinst is 0
+	 */
+	return tinst == (uint32_t)tinst && (tinst & 0x1);
+}
+
  static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
  				 sbi_trap_ld_emulator emu)
  {
  	const struct sbi_trap_info *orig_trap = &tcntx->trap;
  	struct sbi_trap_regs *regs = &tcntx->regs;
-	ulong insn, insn_len;
+	ulong insn, insn_len, imm = 0, shift = 0, off = 0;
  	union sbi_ldst_data val = { 0 };
  	struct sbi_trap_info uptrap;
-	int rc, fp = 0, shift = 0, len = 0;
-	bool xform = false;
-
-	if (orig_trap->tinst & 0x1) {
-		/*
-		 * Bit[0] == 1 implies trapped instruction value is
-		 * transformed instruction or custom instruction.
-		 */
+	bool xform = false, fp = false, c_load = false, c_ldsp = false;
+	int rc, len = 0, prev_xlen = 0;
+
+	if (sbi_trap_tinst_valid(orig_trap->tinst)) {
  		xform	 = true;
  		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
-		 * zero or special value.
-		 */
+		/* trapped instruction value is zero or special value */
  		insn = sbi_get_insn(regs->mepc, &uptrap);
  		if (uptrap.cause) {
  			return sbi_trap_redirect(regs, &uptrap);

@@ -75,92 +79,170 @@ static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
  		insn_len = INSN_LEN(insn);
  	}
 
+	/**
+	 * Common for RV32/RV64:
+	 *    lb, lbu, lh, lhu, lw, flw, flw
+	 *    c.lbu, c.lh, c.lhu, c.lw, c.lwsp, c.fld, c.fldsp
+	 */
  	if ((insn & INSN_MASK_LB) == INSN_MATCH_LB) {
-		len   = 1;
-		shift = 8 * (sizeof(ulong) - len);
+		len = -1;
  	} 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
-	} else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
-		len   = 8;
-		shift = 8 * (sizeof(ulong) - len);
-	} else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
-		len = 4;
-#endif
-#ifdef __riscv_flen
-	} else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
-		fp  = 1;
-		len = 8;
-	} else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
-		fp  = 1;
-		len = 4;
-#endif
+	} else if ((insn & INSN_MASK_C_LBU) == INSN_MATCH_C_LBU) {
+		/* Zcb */
+		len = 1;
+		imm = RVC_LB_IMM(insn);
+		c_load = true;
  	} else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
-		len   = 2;
-		shift = 8 * (sizeof(ulong) - len);
+		len = -2;
+	} else if ((insn & INSN_MASK_C_LH) == INSN_MATCH_C_LH) {
+		/* Zcb */
+		len = -2;
+		imm = RVC_LH_IMM(insn);
+		c_load = true;
  	} else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
  		len = 2;
-#if __riscv_xlen >= 64
-	} else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
-		len   = 8;
-		shift = 8 * (sizeof(ulong) - len);
-		insn  = RVC_RS2S(insn) << SH_RD;
-	} else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
-		   ((insn >> SH_RD) & 0x1f)) {
-		len   = 8;
-		shift = 8 * (sizeof(ulong) - len);
-#endif
+	} else if ((insn & INSN_MASK_C_LHU) == INSN_MATCH_C_LHU) {
+		/* Zcb */
+		len = 2;
+		imm = RVC_LH_IMM(insn);
+		c_load = true;
+	} else if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
+		len = -4;
  	} else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
-		len   = 4;
-		shift = 8 * (sizeof(ulong) - len);
-		insn  = RVC_RS2S(insn) << SH_RD;
+		/* Zca */
+		len = -4;
+		imm = RVC_LW_IMM(insn);
+		c_load = true;
  	} else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
-		   ((insn >> SH_RD) & 0x1f)) {
-		len   = 4;
-		shift = 8 * (sizeof(ulong) - len);
+		GET_RD_NUM(insn)) {
+		/* Zca */
+		len = -4;
+		imm = RVC_LWSP_IMM(insn);
+		c_ldsp = true;
  #ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
+		len = 4;
+		fp = true;
+	} else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
+		len = 8;
+		fp = true;
  	} else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
-		fp   = 1;
-		len  = 8;
-		insn = RVC_RS2S(insn) << SH_RD;
+		/* Zcd */
+		len = 8;
+		imm = RVC_LD_IMM(insn);
+		c_load = true;
+		fp = true;
  	} else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
-		fp  = 1;
+		/* Zcd */
  		len = 8;
-#if __riscv_xlen == 32
-	} else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
-		fp   = 1;
-		len  = 4;
-		insn = RVC_RS2S(insn) << SH_RD;
-	} else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
-		fp  = 1;
-		len = 4;
+		imm = RVC_LDSP_IMM(insn);
+		c_ldsp = true;
+		fp = true;
  #endif
+	} else {
+		prev_xlen = sbi_regs_prev_xlen(regs);
+	}
+
+	/**
+	 * Must distinguish between rv64 and rv32, RVC instructions have
+	 * overlapping encoding:
+	 *     c.ld in rv64 == c.flw in rv32
+	 *     c.ldsp in rv64 == c.flwsp in rv32
+	 */
+	if (prev_xlen == 64) {
+		/* RV64 Only: lwu, ld, c.ld, c.ldsp  */
+		if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
+			len = 4;
+		} else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
+			len = 8;
+		} else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
+			/* Zca */
+			len = 8;
+			imm = RVC_LD_IMM(insn);
+			c_load = true;
+		} else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
+			GET_RD_NUM(insn)) {
+			/* Zca */
+			len = 8;
+			imm = RVC_LDSP_IMM(insn);
+			c_ldsp = true;
+		}
+#ifdef __riscv_flen
+	} else if (prev_xlen == 32) {
+		/* RV32 Only: c.flw, c.flwsp */
+		if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
+			/* Zcf */
+			len = 4;
+			imm = RVC_LW_IMM(insn);
+			c_load = true;
+			fp = true;
+		} else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
+			/* Zcf */
+			len = 4;
+			imm = RVC_LWSP_IMM(insn);
+			c_ldsp = true;
+			fp = true;
+		}
  #endif
-	} else if ((insn & INSN_MASK_C_LHU) == INSN_MATCH_C_LHU) {
-		len = 2;
-		insn = RVC_RS2S(insn) << SH_RD;
-	} else if ((insn & INSN_MASK_C_LH) == INSN_MATCH_C_LH) {
-		len = 2;
+	}
+
+	if (len < 0) {
+		len = -len;
  		shift = 8 * (sizeof(ulong) - len);
-		insn = RVC_RS2S(insn) << SH_RD;
  	}
 
-	rc = emu(xform ? 0 : insn, len, orig_trap->tval, &val, tcntx);
+	if (!len || orig_trap->cause == CAUSE_MISALIGNED_LOAD)
+		/* Unknown instruction or no need to calculate offset */
+		goto do_emu;
+
+	if (xform)
+		/* Transformed insn */
+		off = GET_RS1_NUM(insn);
+	else if (c_load)
+		/* non SP-based compressed load */
+		off = orig_trap->tval - GET_RS1S(insn, regs) - imm;
+	else if (c_ldsp)
+		/* SP-based compressed load */
+		off = orig_trap->tval - REG_VAL(2, regs) - imm;
+	else
+		/* I-type non-compressed load */
+		off = orig_trap->tval - GET_RS1(insn, regs) - (ulong)IMM_I(insn);
+	/**
+	 * Normalize offset, in case the XLEN of unpriv mode is smaller,
+	 * and/or pointer masking is in effect
+	 */
+	off &= (len - 1);
+
+do_emu:
+	rc = emu(xform ? 0 : insn, len, orig_trap->tval - off, &val, tcntx);
  	if (rc <= 0)
  		return rc;
+	if (!len)
+		goto epc_fixup;
+
+	if (!fp) {
+		ulong v = ((long)(val.data_ulong << shift)) >> shift;
 
-	if (!fp)
-		SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
+		if (c_load)
+			SET_RDS(insn, regs, v);
+		else
+			SET_RD(insn, regs, v);
  #ifdef __riscv_flen
-	else if (len == 8)
-		SET_F64_RD(insn, regs, val.data_u64);
-	else
-		SET_F32_RD(insn, regs, val.data_ulong);
+	} else if (len == 8) {
+		if (c_load)
+			SET_F64_RDS(insn, regs, val.data_u64);
+		else
+			SET_F64_RD(insn, regs, val.data_u64);
+	} else {
+		if (c_load)
+			SET_F32_RDS(insn, regs, val.data_ulong);
+		else
+			SET_F32_RD(insn, regs, val.data_ulong);
  #endif
+	}
 
+epc_fixup:
  	regs->mepc += insn_len;
 
  	return 0;

@@ -171,25 +253,18 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
  {
  	const struct sbi_trap_info *orig_trap = &tcntx->trap;
  	struct sbi_trap_regs *regs = &tcntx->regs;
-	ulong insn, insn_len;
+	ulong insn, insn_len, imm = 0, off = 0;
  	union sbi_ldst_data val;
  	struct sbi_trap_info uptrap;
-	int rc, len = 0;
-	bool xform = false;
-
-	if (orig_trap->tinst & 0x1) {
-		/*
-		 * Bit[0] == 1 implies trapped instruction value is
-		 * transformed instruction or custom instruction.
-		 */
+	bool xform = false, fp = false, c_store = false, c_stsp = false;
+	int rc, len = 0, prev_xlen = 0;
+
+	if (sbi_trap_tinst_valid(orig_trap->tinst)) {
  		xform	 = true;
  		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
-		 * zero or special value.
-		 */
+		/* trapped instruction value is zero or special value */
  		insn = sbi_get_insn(regs->mepc, &uptrap);
  		if (uptrap.cause) {
  			return sbi_trap_redirect(regs, &uptrap);

@@ -197,62 +272,150 @@ static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
  		insn_len = INSN_LEN(insn);
  	}
 
-	val.data_ulong = GET_RS2(insn, regs);
-
+	/**
+	 * Common for RV32/RV64:
+	 *    sb, sh, sw, fsw, fsd
+	 *    c.sb, c.sh, c.sw, c.swsp, c.fsd, c.fsdsp
+	 */
  	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) {
-		len = 8;
-#endif
-#ifdef __riscv_flen
-	} else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
-		len	     = 8;
-		val.data_u64 = GET_F64_RS2(insn, regs);
-	} else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
-		len	       = 4;
-		val.data_ulong = GET_F32_RS2(insn, regs);
-#endif
+	} else if ((insn & INSN_MASK_C_SB) == INSN_MATCH_C_SB) {
+		/* Zcb */
+		len = 1;
+		imm = RVC_SB_IMM(insn);
+		c_store = true;
  	} else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
  		len = 2;
-#if __riscv_xlen >= 64
-	} else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
-		len	       = 8;
-		val.data_ulong = GET_RS2S(insn, regs);
-	} else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP) {
-		len	       = 8;
-		val.data_ulong = GET_RS2C(insn, regs);
-#endif
+	} else if ((insn & INSN_MASK_C_SH) == INSN_MATCH_C_SH) {
+		/* Zcb */
+		len = 2;
+		imm = RVC_SH_IMM(insn);
+		c_store = true;
+	} else if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
+		len = 4;
  	} else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
-		len	       = 4;
-		val.data_ulong = GET_RS2S(insn, regs);
+		/* Zca */
+		len = 4;
+		imm = RVC_SW_IMM(insn);
+		c_store = true;
  	} else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP) {
-		len	       = 4;
-		val.data_ulong = GET_RS2C(insn, regs);
+		/* Zca */
+		len = 4;
+		imm = RVC_SWSP_IMM(insn);
+		c_stsp = true;
  #ifdef __riscv_flen
+	} else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
+		len = 4;
+		fp = true;
+	} else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
+		len = 8;
+		fp = true;
  	} else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) {
-		len	     = 8;
-		val.data_u64 = GET_F64_RS2S(insn, regs);
+		/* Zcd */
+		len = 8;
+		imm = RVC_SD_IMM(insn);
+		c_store = true;
+		fp = true;
  	} else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) {
-		len	     = 8;
-		val.data_u64 = GET_F64_RS2C(insn, regs);
-#if __riscv_xlen == 32
-	} else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
-		len	       = 4;
-		val.data_ulong = GET_F32_RS2S(insn, regs);
-	} else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
-		len	       = 4;
-		val.data_ulong = GET_F32_RS2C(insn, regs);
+		/* Zcd */
+		len = 8;
+		imm = RVC_SDSP_IMM(insn);
+		c_stsp = true;
+		fp = true;
  #endif
+	} else {
+		prev_xlen = sbi_regs_prev_xlen(regs);
+	}
+
+	/**
+	 * Must distinguish between rv64 and rv32, RVC instructions have
+	 * overlapping encoding:
+	 *     c.sd in rv64 == c.fsw in rv32
+	 *     c.sdsp in rv64 == c.fswsp in rv32
+	 */
+	if (prev_xlen == 64) {
+		/* RV64 Only: sd, c.sd, c.sdsp */
+		if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
+			len = 8;
+		} else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
+			/* Zca */
+			len = 8;
+			imm = RVC_SD_IMM(insn);
+			c_store = true;
+		} else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP) {
+			/* Zca */
+			len = 8;
+			imm = RVC_SDSP_IMM(insn);
+			c_stsp = true;
+		}
+#ifdef __riscv_flen
+	} else if (prev_xlen == 32) {
+		/* RV32 Only: c.fsw, c.fswsp */
+		if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
+			/* Zcf */
+			len = 4;
+			imm = RVC_SW_IMM(insn);
+			c_store = true;
+			fp = true;
+		} else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
+			/* Zcf */
+			len = 4;
+			imm = RVC_SWSP_IMM(insn);
+			c_stsp = true;
+			fp = true;
+		}
  #endif
-	} else if ((insn & INSN_MASK_C_SH) == INSN_MATCH_C_SH) {
-		len		= 2;
-		val.data_ulong = GET_RS2S(insn, regs);
  	}
 
-	rc = emu(xform ? 0 : insn, len, orig_trap->tval, val, tcntx);
+	if (!fp) {
+		if (c_store)
+			val.data_ulong = GET_RS2S(insn, regs);
+		else if (c_stsp)
+			val.data_ulong = GET_RS2C(insn, regs);
+		else
+			val.data_ulong = GET_RS2(insn, regs);
+#ifdef __riscv_flen
+	} else if (len == 8) {
+		if (c_store)
+			val.data_u64 = GET_F64_RS2S(insn, regs);
+		else if (c_stsp)
+			val.data_u64 = GET_F64_RS2C(insn, regs);
+		else
+			val.data_u64 = GET_F64_RS2(insn, regs);
+	} else {
+		if (c_store)
+			val.data_ulong = GET_F32_RS2S(insn, regs);
+		else if (c_stsp)
+			val.data_ulong = GET_F32_RS2C(insn, regs);
+		else
+			val.data_ulong = GET_F32_RS2(insn, regs);
+#endif
+	}
+
+	if (!len || orig_trap->cause == CAUSE_MISALIGNED_STORE)
+		/* Unknown instruction or no need to calculate offset */
+		goto do_emu;
+
+	if (xform)
+		/* Transformed insn */
+		off = GET_RS1_NUM(insn);
+	else if (c_store)
+		/* non SP-based compressed store */
+		off = orig_trap->tval - GET_RS1S(insn, regs) - imm;
+	else if (c_stsp)
+		/* SP-based compressed store */
+		off = orig_trap->tval - REG_VAL(2, regs) - imm;
+	else
+		/* S-type non-compressed store */
+		off = orig_trap->tval - GET_RS1(insn, regs) - (ulong)IMM_S(insn);
+	/**
+	 * Normalize offset, in case the XLEN of unpriv mode is smaller,
+	 * and/or pointer masking is in effect
+	 */
+	off &= (len - 1);
+
+do_emu:
+	rc = emu(xform ? 0 : insn, len, orig_trap->tval - off, val, tcntx);
  	if (rc <= 0)
  		return rc;