diff options
Diffstat (limited to 'lib/sbi')
-rw-r--r-- | lib/sbi/sbi_misaligned_ldst.c | 42 |
1 files changed, 34 insertions, 8 deletions
diff --git a/lib/sbi/sbi_misaligned_ldst.c b/lib/sbi/sbi_misaligned_ldst.c index 314b2f3..400372c 100644 --- a/lib/sbi/sbi_misaligned_ldst.c +++ b/lib/sbi/sbi_misaligned_ldst.c @@ -26,14 +26,27 @@ int sbi_misaligned_load_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs, struct sbi_scratch *scratch) { + ulong insn; union reg_data val; struct sbi_trap_info uptrap; int i, fp = 0, shift = 0, len = 0; - ulong insn = sbi_get_insn(regs->mepc, scratch, &uptrap); - if (uptrap.cause) { - uptrap.epc = regs->mepc; - return sbi_trap_redirect(regs, &uptrap, scratch); + if (tinst & 0x1) { + /* + * Bit[0] == 1 implies trapped instruction value is + * transformed instruction or custom instruction. + */ + insn = tinst | INSN_16BIT_MASK; + } else { + /* + * Bit[0] == 0 implies trapped instruction value is + * zero or special value. + */ + insn = sbi_get_insn(regs->mepc, scratch, &uptrap); + if (uptrap.cause) { + uptrap.epc = regs->mepc; + return sbi_trap_redirect(regs, &uptrap, scratch); + } } if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) { @@ -135,14 +148,27 @@ int sbi_misaligned_store_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs, struct sbi_scratch *scratch) { + ulong insn; union reg_data val; struct sbi_trap_info uptrap; int i, len = 0; - ulong insn = sbi_get_insn(regs->mepc, scratch, &uptrap); - if (uptrap.cause) { - uptrap.epc = regs->mepc; - return sbi_trap_redirect(regs, &uptrap, scratch); + if (tinst & 0x1) { + /* + * Bit[0] == 1 implies trapped instruction value is + * transformed instruction or custom instruction. + */ + insn = tinst | INSN_16BIT_MASK; + } else { + /* + * Bit[0] == 0 implies trapped instruction value is + * zero or special value. + */ + insn = sbi_get_insn(regs->mepc, scratch, &uptrap); + if (uptrap.cause) { + uptrap.epc = regs->mepc; + return sbi_trap_redirect(regs, &uptrap, scratch); + } } val.data_ulong = GET_RS2(insn, regs); |