diff --git a/sys/arm64/arm64/support.S b/sys/arm64/arm64/support.S --- a/sys/arm64/arm64/support.S +++ b/sys/arm64/arm64/support.S @@ -55,6 +55,48 @@ ret END(fsu_fault) +#ifdef COMPAT_FREEBSD32 +ENTRY(swp_emulate_atomic) + adr x6, fsu_fault /* Load the fault handler */ + mov w5, #1 + SET_FAULT_HANDLER(x6, x4) /* And set it */ + ENTER_USER_ACCESS(w6, x4) + + /* Save off &val, we'll store the old value to it later. */ + mov x7, x1 + ldr w1, [x7] + + cmp x2, #0 /* is_swpb */ + b.ne 2f + + /* swp */ +1: ldxr w2, [x0] /* Stash the old value in w2 */ + stxr w3, w1, [x0] /* Store new value */ + cbnz w3, 1b /* Success? */ + + mov w0, w2 /* Return the old value */ + b 3f + + /* swpb */ +2: ldxrb w2, [x0] + stxrb w3, w1, [x0] + cbnz w3, 2b + + mov w0, w2 + +3: EXIT_USER_ACCESS(w6) + SET_FAULT_HANDLER(xzr, x6) + + /* + * w0 has the value to be returned, stash it in + * *val and return success. + */ + str x0, [x7] + mov x0, #0 + ret +END(swp_emulate_atomic) +#endif + /* * int casueword32_llsc(volatile uint32_t *, uint32_t, uint32_t *, uint32_t) */ diff --git a/sys/arm64/arm64/undefined.c b/sys/arm64/arm64/undefined.c --- a/sys/arm64/arm64/undefined.c +++ b/sys/arm64/arm64/undefined.c @@ -41,10 +41,15 @@ #include #include +#include #include +#include #include #include +#include +#include + MALLOC_DEFINE(M_UNDEF, "undefhandler", "Undefined instruction handler data"); struct undef_handler { @@ -113,6 +118,63 @@ } return 0; } + +extern int swp_emulate_atomic(vm_offset_t addr, uint32_t *val, bool is_swpb); + +static int +swp_emulate(vm_offset_t va, uint32_t insn, struct trapframe *frame, + uint32_t esr) +{ + ksiginfo_t ksi; + struct thread *td; + vm_offset_t vaddr; + uint64_t *regs; + uint32_t val; + int Rn, Rd, Rm; + bool is_swpb; + + td = curthread; + + /* swp, swpb only */ + if (!SV_PROC_FLAG(td->td_proc, SV_ILP32)) + return (0); + else if ((insn & 0x0fb00ff0) != 0x01000090) + return (0); + + Rn = (insn & 0xf0000) >> 16; + Rd = (insn & 0xf000) >> 12; + Rm = (insn & 0xf); + + regs = frame->tf_x; + vaddr = regs[Rn] & 0xffffffff; + val = regs[Rm]; + + /* Enforce alignment for swp. */ + is_swpb = (insn & 0x00400000) != 0; + if (!is_swpb && (vaddr & 3) != 0) + goto fault; + + /* + * vaddr is a 32-bit quantity, let it slide. + */ + if (swp_emulate_atomic(vaddr, &val, is_swpb) != 0) + goto fault; + + regs[Rd] = val; + + /* No thumb SWP/SWPB */ + frame->tf_elr += 4; //INSN_SIZE; + + return (1); +fault: + ksiginfo_init_trap(&ksi); + ksi.ksi_signo = SIGSEGV; + ksi.ksi_code = SEGV_MAPERR; + ksi.ksi_addr = (void *)va; + trapsignal(td, &ksi); + + return (1); +} #endif void @@ -125,6 +187,7 @@ install_undef_handler(false, id_aa64mmfr2_handler); #ifdef COMPAT_FREEBSD32 install_undef_handler(true, gdb_trapper); + install_undef_handler(true, swp_emulate); #endif }