diff --git a/include/sbi/sbi_unpriv.h b/include/sbi/sbi_unpriv.h index 8cbd3de0..be219626 100644 --- a/include/sbi/sbi_unpriv.h +++ b/include/sbi/sbi_unpriv.h @@ -36,6 +36,12 @@ DECLARE_UNPRIVILEGED_LOAD_FUNCTION(u64) DECLARE_UNPRIVILEGED_STORE_FUNCTION(u64) DECLARE_UNPRIVILEGED_LOAD_FUNCTION(ulong) +void sbi_load_loop(u8 *buffer, ulong addr, ulong len, + struct sbi_trap_info *trap); + +void sbi_store_loop(u8 *buffer, ulong addr, ulong len, + struct sbi_trap_info *trap); + ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap); #endif diff --git a/lib/sbi/sbi_trap_ldst.c b/lib/sbi/sbi_trap_ldst.c index 5f7de662..e726520f 100644 --- a/lib/sbi/sbi_trap_ldst.c +++ b/lib/sbi/sbi_trap_ldst.c @@ -471,7 +471,6 @@ static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr, const struct sbi_trap_info *orig_trap = &tcntx->trap; struct sbi_trap_regs *regs = &tcntx->regs; struct sbi_trap_info uptrap; - int i; if (!rlen) { if (IS_VECTOR_LOAD_STORE(insn)) @@ -484,13 +483,10 @@ static int sbi_misaligned_ld_emulator(ulong insn, int rlen, ulong addr, if (addr != orig_trap->tval) return SBI_EFAIL; - for (i = 0; i < rlen; i++) { - out_val->data_bytes[i] = - sbi_load_u8((void *)(addr + i), &uptrap); - if (uptrap.cause) { - sbi_misaligned_tinst_fixup(orig_trap, &uptrap); - return sbi_trap_redirect(regs, &uptrap); - } + sbi_load_loop(out_val->data_bytes, addr, rlen, &uptrap); + if (uptrap.cause) { + sbi_misaligned_tinst_fixup(orig_trap, &uptrap); + return sbi_trap_redirect(regs, &uptrap); } return rlen; } @@ -507,7 +503,6 @@ static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr, const struct sbi_trap_info *orig_trap = &tcntx->trap; struct sbi_trap_regs *regs = &tcntx->regs; struct sbi_trap_info uptrap; - int i; if (!wlen) { if (IS_VECTOR_LOAD_STORE(insn)) @@ -520,13 +515,10 @@ static int sbi_misaligned_st_emulator(ulong insn, int wlen, ulong addr, if (addr != orig_trap->tval) return SBI_EFAIL; - for (i = 0; i < wlen; i++) { - sbi_store_u8((void *)(addr + i), - in_val.data_bytes[i], &uptrap); - if (uptrap.cause) { - sbi_misaligned_tinst_fixup(orig_trap, &uptrap); - return sbi_trap_redirect(regs, &uptrap); - } + sbi_store_loop(in_val.data_bytes, addr, wlen, &uptrap); + if (uptrap.cause) { + sbi_misaligned_tinst_fixup(orig_trap, &uptrap); + return sbi_trap_redirect(regs, &uptrap); } return wlen; } diff --git a/lib/sbi/sbi_trap_v_ldst.c b/lib/sbi/sbi_trap_v_ldst.c index 6952bc72..38c28caf 100644 --- a/lib/sbi/sbi_trap_v_ldst.c +++ b/lib/sbi/sbi_trap_v_ldst.c @@ -229,20 +229,17 @@ int sbi_misaligned_v_ld_emulator(ulong insn, struct sbi_trap_context *tcntx) /* obtain load data from memory */ for (ulong seg = 0; seg < nf; seg++) { - for (ulong i = 0; i < len; i++) { - bytes[seg * len + i] = - sbi_load_u8((void *)(addr + seg * len + i), - &uptrap); + sbi_load_loop(bytes + seg * len, + addr + seg * len, len, &uptrap); - if (uptrap.cause) { - if (IS_FAULT_ONLY_FIRST_LOAD(insn) && vstart != 0) { - vl = vstart; - break; - } - vsetvl(vl, vtype); - sbi_misaligned_v_tinst_fixup(&uptrap); - return sbi_trap_redirect(regs, &uptrap); + if (uptrap.cause) { + if (IS_FAULT_ONLY_FIRST_LOAD(insn) && vstart != 0) { + vl = vstart; + break; } + vsetvl(vl, vtype); + sbi_misaligned_v_tinst_fixup(&uptrap); + return sbi_trap_redirect(regs, &uptrap); } } @@ -332,14 +329,13 @@ int sbi_misaligned_v_st_emulator(ulong insn, struct sbi_trap_context *tcntx) /* write store data to memory */ for (ulong seg = 0; seg < nf; seg++) { - for (ulong i = 0; i < len; i++) { - sbi_store_u8((void *)(addr + seg * len + i), - bytes[seg * len + i], &uptrap); - if (uptrap.cause) { - vsetvl(vl, vtype); - sbi_misaligned_v_tinst_fixup(&uptrap); - return sbi_trap_redirect(regs, &uptrap); - } + sbi_store_loop(bytes + seg * len, + addr + seg * len, len, &uptrap); + + if (uptrap.cause) { + vsetvl(vl, vtype); + sbi_misaligned_v_tinst_fixup(&uptrap); + return sbi_trap_redirect(regs, &uptrap); } } } while (++vstart < vl); diff --git a/lib/sbi/sbi_unpriv.c b/lib/sbi/sbi_unpriv.c index f9bbec59..60becedc 100644 --- a/lib/sbi/sbi_unpriv.c +++ b/lib/sbi/sbi_unpriv.c @@ -11,9 +11,20 @@ #include #include #include +#include #include #include +union sbi_unpriv_data { + u8 b; + u16 h; + u32 w; +#if __riscv_xlen == 64 + u64 d; +#endif + u8 bytes[__riscv_xlen / 8]; +}; + /** * a3 must a pointer to the sbi_trap_info and a4 is used as a temporary * register in the trap handler. Make sure that compiler doesn't use a3 & a4. @@ -22,13 +33,12 @@ type sbi_load_##type(const type *addr, \ struct sbi_trap_info *trap) \ { \ - register ulong tinfo asm("a3"); \ + register ulong tinfo asm("a3") = (ulong)trap; \ register ulong mstatus = 0; \ register ulong mtvec = (ulong)sbi_hart_expected_trap; \ type ret = 0; \ trap->cause = 0; \ asm volatile( \ - "add %[tinfo], %[taddr], zero\n" \ "csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n" \ "csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n" \ ".option push\n" \ @@ -39,8 +49,7 @@ "csrw " STR(CSR_MTVEC) ", %[mtvec]" \ : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec), \ [tinfo] "+&r"(tinfo), [ret] "=&r"(ret) \ - : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV), \ - [taddr] "r"((ulong)trap) \ + : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV) \ : "a4", "memory"); \ return ret; \ } @@ -54,7 +63,6 @@ register ulong mtvec = (ulong)sbi_hart_expected_trap; \ trap->cause = 0; \ asm volatile( \ - "add %[tinfo], %[taddr], zero\n" \ "csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n" \ "csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n" \ ".option push\n" \ @@ -66,7 +74,7 @@ : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec), \ [tinfo] "+&r"(tinfo) \ : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV), \ - [val] "r"(val), [taddr] "r"((ulong)trap) \ + [val] "r"(val) \ : "a4", "memory"); \ } @@ -116,6 +124,84 @@ void sbi_store_u64(u64 *addr, u64 val, # error "Unexpected __riscv_xlen" #endif +void sbi_load_loop(u8 *buffer, ulong addr, ulong len, + struct sbi_trap_info *trap) +{ + union sbi_unpriv_data data; + + trap->cause = 0; + while (len) { + unsigned int width = __riscv_xlen / 8; + void *ptr = (void*)addr; + + while (len < width || (addr & (width - 1))) + width /= 2; + + switch (width) { + case 1: + data.b = sbi_load_u8(ptr, trap); + break; + case 2: + data.h = sbi_load_u16(ptr, trap); + break; + case 4: + data.w = sbi_load_u32(ptr, trap); + break; +#if __riscv_xlen == 64 + case 8: + data.d = sbi_load_u64(ptr, trap); + break; +#endif + } + if (trap->cause) + return; + + sbi_memcpy(buffer, data.bytes, width); + len -= width; + addr += width; + buffer += width; + } +} + +void sbi_store_loop(u8 *buffer, ulong addr, ulong len, + struct sbi_trap_info *trap) +{ + union sbi_unpriv_data data; + + trap->cause = 0; + while (len) { + unsigned int width = __riscv_xlen / 8; + void *ptr = (void*)addr; + + while (len < width || (addr & (width - 1))) + width /= 2; + + sbi_memcpy(data.bytes, buffer, width); + switch (width) { + case 1: + sbi_store_u8(ptr, data.b, trap); + break; + case 2: + sbi_store_u16(ptr, data.h, trap); + break; + case 4: + sbi_store_u32(ptr, data.w, trap); + break; +#if __riscv_xlen == 64 + case 8: + sbi_store_u64(ptr, data.d, trap); + break; +#endif + } + if (trap->cause) + return; + + len -= width; + addr += width; + buffer += width; + } +} + ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap) { register ulong tinfo asm("a3");