Index: sys/arm/arm/exception.S =================================================================== --- sys/arm/arm/exception.S +++ sys/arm/arm/exception.S @@ -364,7 +364,6 @@ * on exit (without transitioning back through the undefined mode stack). */ ASENTRY_NP(undefined_entry) - sub lr, lr, #4 /* Adjust the lr. Transition to scv32 */ PUSHFRAMEINSVC /* mode stack, build trapframe there. */ adr lr, exception_exit /* Return from handler via standard */ mov r0, sp /* exception exit routine. Pass the */ Index: sys/arm/arm/machdep.c =================================================================== --- sys/arm/arm/machdep.c +++ sys/arm/arm/machdep.c @@ -87,6 +87,7 @@ #include #include +#include #include #include #include @@ -287,6 +288,13 @@ tf->tf_pc = (register_t)catcher; tf->tf_usr_sp = (register_t)fp; tf->tf_usr_lr = (register_t)(PS_STRINGS - *(p->p_sysent->sv_szsigcode)); + /* Set the mode to enter in the signal handler */ +#if __ARM_ARCH >= 7 + if ((register_t)catcher & 1) + tf->tf_spsr |= PSR_T; + else + tf->tf_spsr &= ~PSR_T; +#endif CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td, tf->tf_usr_lr, tf->tf_usr_sp); @@ -576,6 +584,10 @@ struct proc *p; int error; + /* TODO: This needs to be updated for Thumb-2 */ + if ((td->td_frame->tf_spsr & PSR_T) != 0) + return (EINVAL); + KASSERT(td->td_md.md_ptrace_instr == 0, ("Didn't clear single step")); p = td->td_proc; @@ -599,6 +611,10 @@ { struct proc *p; + /* TODO: This needs to be updated for Thumb-2 */ + if ((td->td_frame->tf_spsr & PSR_T) != 0) + return (EINVAL); + if (td->td_md.md_ptrace_instr) { p = td->td_proc; PROC_UNLOCK(p); Index: sys/arm/arm/syscall.c =================================================================== --- sys/arm/arm/syscall.c +++ sys/arm/arm/syscall.c @@ -171,16 +171,7 @@ td->td_frame = frame; td->td_pticks = 0; - /* - * Make sure the program counter is correctly aligned so we - * don't take an alignment fault trying to read the opcode. - * XXX: Fix for Thumb mode - */ - if (__predict_false(((frame->tf_pc - INSN_SIZE) & 3) != 0)) { - call_trapsignal(td, SIGILL, 0); - userret(td, frame); - return; - } + /* * Enable interrupts if they were enabled before the exception. * Since all syscalls *should* come from user mode it will always Index: sys/arm/arm/undefined.c =================================================================== --- sys/arm/arm/undefined.c +++ sys/arm/arm/undefined.c @@ -179,6 +179,7 @@ int fault_code; int coprocessor; struct undefined_handler *uh; + int error; #ifdef VERBOSE_ARM32 int s; #endif @@ -192,6 +193,12 @@ PCPU_INC(cnt.v_trap); +#if __ARM_ARCH >= 7 + if ((frame->tf_spsr & PSR_T) != 0) + frame->tf_pc -= THUMB_INSN_SIZE; + else +#endif + frame->tf_pc -= INSN_SIZE; fault_pc = frame->tf_pc; /* @@ -200,11 +207,76 @@ */ td = curthread == NULL ? &thread0 : curthread; - /* - * Make sure the program counter is correctly aligned so we - * don't take an alignment fault trying to read the opcode. - */ - if (__predict_false((fault_pc & 3) != 0)) { + coprocessor = 0; + if ((frame->tf_spsr & PSR_T) == 0) { + /* + * Make sure the program counter is correctly aligned so we + * don't take an alignment fault trying to read the opcode. + */ + if (__predict_false((fault_pc & 3) != 0)) { + ksiginfo_init_trap(&ksi); + ksi.ksi_signo = SIGILL; + ksi.ksi_code = ILL_ILLADR; + ksi.ksi_addr = (u_int32_t *)(intptr_t) fault_pc; + trapsignal(td, &ksi); + userret(td, frame); + return; + } + + /* + * Should use fuword() here .. but in the interests of + * squeezing every bit of speed we will just use ReadWord(). + * We know the instruction can be read as was just executed + * so this will never fail unless the kernel is screwed up + * in which case it does not really matter does it ? + */ + + fault_instruction = *(u_int32_t *)fault_pc; + + /* Check for coprocessor instruction */ + + /* + * According to the datasheets you only need to look at bit + * 27 of the instruction to tell the difference between and + * undefined instruction and a coprocessor instruction + * following an undefined instruction trap. + */ + + if ((fault_instruction & (1 << 27)) != 0) + coprocessor = (fault_instruction >> 8) & 0x0f; +#ifdef VFP + else { /* check for special instructions */ + if (((fault_instruction & 0xfe000000) == 0xf2000000) || + ((fault_instruction & 0xff100000) == 0xf4000000)) + coprocessor = 10; /* vfp / simd */ + } +#endif /* VFP */ + } else { +#if __ARM_ARCH >= 7 + fault_instruction = *(uint16_t *)fault_pc; + if (fault_instruction >= 0xe800) { + fault_instruction <<= 16; + fault_instruction |= *(uint16_t *)(fault_pc + 2); + + /* + * Is it a Coprocessor, Advanced SIMD, or + * Floating-point instruction. + */ + if ((fault_instruction & (3 << 26)) == (3 << 26)) { + if ((fault_instruction & (0x3e << 20)) == 0) { + /* undefined */ + } else if ( + (fault_instruction & (3 << 24)) == (3 << 24)) + coprocessor = 10; /* vfp / simd */ + else + coprocessor = + (fault_instruction >> 8) & 0xf; + } + } +#else + /* + * No support for Thumb-2 on this cpu + */ ksiginfo_init_trap(&ksi); ksi.ksi_signo = SIGILL; ksi.ksi_code = ILL_ILLADR; @@ -212,41 +284,8 @@ trapsignal(td, &ksi); userret(td, frame); return; - } - - /* - * Should use fuword() here .. but in the interests of squeezing every - * bit of speed we will just use ReadWord(). We know the instruction - * can be read as was just executed so this will never fail unless the - * kernel is screwed up in which case it does not really matter does - * it ? - */ - - fault_instruction = *(u_int32_t *)fault_pc; - - /* Update vmmeter statistics */ -#if 0 - uvmexp.traps++; #endif - /* Check for coprocessor instruction */ - - /* - * According to the datasheets you only need to look at bit 27 of the - * instruction to tell the difference between and undefined - * instruction and a coprocessor instruction following an undefined - * instruction trap. - */ - - coprocessor = 0; - if ((fault_instruction & (1 << 27)) != 0) - coprocessor = (fault_instruction >> 8) & 0x0f; -#ifdef VFP - else { /* check for special instructions */ - if (((fault_instruction & 0xfe000000) == 0xf2000000) || - ((fault_instruction & 0xff100000) == 0xf4000000)) - coprocessor = 10; /* vfp / simd */ } -#endif /* VFP */ if ((frame->tf_spsr & PSR_MODE) == PSR_USR32_MODE) { /* @@ -264,13 +303,24 @@ fault_code) == 0) break; - if (fault_code & FAULT_USER && fault_instruction == PTRACE_BREAKPOINT) { - PROC_LOCK(td->td_proc); - _PHOLD(td->td_proc); - ptrace_clear_single_step(td); - _PRELE(td->td_proc); - PROC_UNLOCK(td->td_proc); - return; + if (fault_code & FAULT_USER) { + /* TODO: No support for ptrace from Thumb-2 */ + if ((frame->tf_spsr & PSR_T) == 0 && + fault_instruction == PTRACE_BREAKPOINT) { + PROC_LOCK(td->td_proc); + _PHOLD(td->td_proc); + error = ptrace_clear_single_step(td); + _PRELE(td->td_proc); + PROC_UNLOCK(td->td_proc); + if (error != 0) { + ksiginfo_init_trap(&ksi); + ksi.ksi_signo = SIGILL; + ksi.ksi_code = ILL_ILLOPC; + ksi.ksi_addr = (u_int32_t *)(intptr_t) fault_pc; + trapsignal(td, &ksi); + } + return; + } } if (uh == NULL && (fault_code & FAULT_USER)) { Index: sys/arm/arm/vm_machdep.c =================================================================== --- sys/arm/arm/vm_machdep.c +++ sys/arm/arm/vm_machdep.c @@ -72,6 +72,7 @@ #include #include +#include #include #include @@ -204,7 +205,12 @@ /* * Reconstruct the pc to point at the swi. */ - frame->tf_pc -= INSN_SIZE; +#if __ARM_ARCH >= 7 + if ((frame->tf_spsr & PSR_T) != 0) + frame->tf_pc -= THUMB_INSN_SIZE; + else +#endif + frame->tf_pc -= INSN_SIZE; break; case EJUSTRETURN: /* nothing to do */