diff options
Diffstat (limited to 'machine/misaligned_ldst.c')
| -rw-r--r-- | machine/misaligned_ldst.c | 56 |
1 files changed, 54 insertions, 2 deletions
diff --git a/machine/misaligned_ldst.c b/machine/misaligned_ldst.c index 980aeba..2fccd50 100644 --- a/machine/misaligned_ldst.c +++ b/machine/misaligned_ldst.c @@ -2,6 +2,8 @@ #include "fp_emulation.h" #include "unprivileged_memory.h" #include "mtrap.h" +#include "config.h" +#include "pk.h" union byte_array { uint8_t bytes[8]; @@ -14,6 +16,7 @@ void misaligned_load_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) union byte_array val; uintptr_t mstatus; insn_t insn = get_insn(mepc, &mstatus); + uintptr_t npc = mepc + insn_len(insn); uintptr_t addr = read_csr(mbadaddr); int shift = 0, fp = 0, len; @@ -35,6 +38,30 @@ void misaligned_load_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) len = 2, shift = 8*(sizeof(uintptr_t) - len); else if ((insn & MASK_LHU) == MATCH_LHU) len = 2; +#ifdef __riscv_compressed +# if __riscv_xlen >= 64 + else if ((insn & MASK_C_LD) == MATCH_C_LD) + len = 8, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD; + else if ((insn & MASK_C_LDSP) == MATCH_C_LDSP && ((insn >> SH_RD) & 0x1f)) + len = 8, shift = 8*(sizeof(uintptr_t) - len); +# endif + else if ((insn & MASK_C_LW) == MATCH_C_LW) + len = 4, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD; + else if ((insn & MASK_C_LWSP) == MATCH_C_LWSP && ((insn >> SH_RD) & 0x1f)) + len = 4, shift = 8*(sizeof(uintptr_t) - len); +# ifdef PK_ENABLE_FP_EMULATION + else if ((insn & MASK_C_FLD) == MATCH_C_FLD) + fp = 1, len = 8, insn = RVC_RS2S(insn) << SH_RD; + else if ((insn & MASK_C_FLDSP) == MATCH_C_FLDSP) + fp = 1, len = 8; +# if __riscv_xlen == 32 + else if ((insn & MASK_C_FLW) == MATCH_C_FLW) + fp = 1, len = 4, insn = RVC_RS2S(insn) << SH_RD; + else if ((insn & MASK_C_FLWSP) == MATCH_C_FLWSP) + fp = 1, len = 4; +# endif +# endif +#endif else return truly_illegal_insn(regs, mcause, mepc, mstatus, insn); @@ -49,7 +76,7 @@ void misaligned_load_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) else SET_F32_RD(insn, regs, val.intx); - write_csr(mepc, mepc + 4); + write_csr(mepc, npc); } void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) @@ -57,6 +84,7 @@ void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) union byte_array val; uintptr_t mstatus; insn_t insn = get_insn(mepc, &mstatus); + uintptr_t npc = mepc + insn_len(insn); int len; val.intx = GET_RS2(insn, regs); @@ -74,6 +102,30 @@ void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) #endif else if ((insn & MASK_SH) == MATCH_SH) len = 2; +#ifdef __riscv_compressed +# if __riscv_xlen >= 64 + else if ((insn & MASK_C_SD) == MATCH_C_SD) + len = 8, val.intx = GET_RS2S(insn, regs); + else if ((insn & MASK_C_SDSP) == MATCH_C_SDSP && ((insn >> SH_RD) & 0x1f)) + len = 8, val.intx = GET_RS2C(insn, regs); +# endif + else if ((insn & MASK_C_SW) == MATCH_C_SW) + len = 4, val.intx = GET_RS2S(insn, regs); + else if ((insn & MASK_C_SWSP) == MATCH_C_SWSP && ((insn >> SH_RD) & 0x1f)) + len = 4, val.intx = GET_RS2C(insn, regs); +# ifdef PK_ENABLE_FP_EMULATION + else if ((insn & MASK_C_FSD) == MATCH_C_FSD) + len = 8, val.int64 = GET_F64_RS2S(insn, regs); + else if ((insn & MASK_C_FSDSP) == MATCH_C_FSDSP) + len = 8, val.int64 = GET_F64_RS2C(insn, regs); +# if __riscv_xlen == 32 + else if ((insn & MASK_C_FSW) == MATCH_C_FSW) + len = 4, val.intx = GET_F32_RS2S(insn, regs); + else if ((insn & MASK_C_FSWSP) == MATCH_C_FSWSP) + len = 4, val.intx = GET_F32_RS2C(insn, regs); +# endif +# endif +#endif else return truly_illegal_insn(regs, mcause, mepc, mstatus, insn); @@ -81,5 +133,5 @@ void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc) for (int i = 0; i < len; i++) store_uint8_t((void *)(addr + i), val.bytes[i], mepc); - write_csr(mepc, mepc + 4); + write_csr(mepc, npc); } |