 @@ -1,14 +1,14 @@
-/* $NetBSD: trap.c,v 1.17.4.4 2021/01/01 12:58:35 martin Exp $ */
+/* $NetBSD: trap.c,v 1.17.4.5 2021/01/01 13:14:29 martin Exp $ */
 /*-
  * Copyright (c) 2014 The NetBSD Foundation, Inc.
  * All rights reserved.
+ *
  * This code is derived from software contributed to The NetBSD Foundation
  * by Matt Thomas of 3am Software Foundry.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
 @@ -21,27 +21,27 @@
  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(1, "$NetBSD: trap.c,v 1.17.4.4 2021/01/01 12:58:35 martin Exp $");
+__KERNEL_RCSID(1, "$NetBSD: trap.c,v 1.17.4.5 2021/01/01 13:14:29 martin Exp $");
 #include "opt_arm_intr_impl.h"
 #include "opt_compat_netbsd32.h"
 #include "opt_dtrace.h"
 #include <sys/param.h>
 #include <sys/kauth.h>
 #include <sys/types.h>
 #include <sys/atomic.h>
 #include <sys/cpu.h>
 #ifdef KDB
 #include <sys/kdb.h>
 #endif
 @@ -78,26 +78,32 @@ __KERNEL_RCSID(1, "$NetBSD: trap.c,v 1.1
 #include <sys/dtrace_bsd.h>
 #endif
 #ifdef DDB
 int sigill_debug = 0;
 #endif
 #ifdef KDTRACE_HOOKS
 dtrace_doubletrap_func_t	dtrace_doubletrap_func = NULL;
 dtrace_trap_func_t		dtrace_trap_func = NULL;
 int (*dtrace_invop_jump_addr)(struct trapframe *);
 #endif
 enum emul_arm_result {
 	EMUL_ARM_SUCCESS = 0,
 	EMUL_ARM_UNKNOWN,
 	EMUL_ARM_FAULT,
 };
 const char * const trap_names[] = {
 	[ESR_EC_UNKNOWN]	= "Unknown Reason (Illegal Instruction)",
 	[ESR_EC_SERROR]		= "SError Interrupt",
 	[ESR_EC_WFX]		= "WFI or WFE instruction execution",
 	[ESR_EC_ILL_STATE]	= "Illegal Execution State",
 	[ESR_EC_SYS_REG]	= "MSR/MRS/SYS instruction",
 	[ESR_EC_SVC_A64]	= "SVC Instruction Execution",
 	[ESR_EC_HVC_A64]	= "HVC Instruction Execution",
 	[ESR_EC_SMC_A64]	= "SMC Instruction Execution",
 	[ESR_EC_INSN_ABT_EL0]	= "Instruction Abort (EL0)",
 	[ESR_EC_INSN_ABT_EL1]	= "Instruction Abort (EL1)",
 @@ -346,131 +352,177 @@ interrupt(struct trapframe *tf)
 	ci->ci_intr_depth--;
 	cpu_dosoftints();
+}
 #ifdef COMPAT_NETBSD32
 /*
  * 32-bit length Thumb instruction. See ARMv7 DDI0406A A6.3.
  */
 #define THUMB_32BIT(hi) (((hi) & 0xe000) == 0xe000 && ((hi) & 0x1800))
 static int
-fetch_arm_insn(struct trapframe *tf, uint32_t *insn)
+fetch_arm_insn(uint64_t pc, uint64_t spsr, uint32_t *insn)
+{
 	/* THUMB? */
-	if (tf->tf_spsr & SPSR_A32_T) {
+	if (spsr & SPSR_A32_T) {
-		uint16_t *pc = (uint16_t *)(tf->tf_pc & ~1UL); /* XXX */
+		uint16_t *p = (uint16_t *)(pc & ~1UL); /* XXX */
 		uint16_t hi, lo;
-		if (ufetch_16(pc, &hi))
+		if (ufetch_16(p, &hi))
 			return -1;
 		if (!THUMB_32BIT(hi)) {
 			/* 16-bit Thumb instruction */
 			*insn = hi;
 			return 2;
+		}
 		/* 32-bit Thumb instruction */
-		if (ufetch_16(pc + 1, &lo))
+		if (ufetch_16(p + 1, &lo))
 			return -1;
 		*insn = ((uint32_t)hi << 16) | lo;
 		return 4;
+	}
-	if (ufetch_32((uint32_t *)tf->tf_pc, insn))
+	if (ufetch_32((uint32_t *)pc, insn))
 		return -1;
 	return 4;
+}
-enum emul_arm_result {
+static bool
-	EMUL_ARM_SUCCESS = 0,
+arm_cond_match(uint32_t insn, uint64_t spsr)
 	EMUL_ARM_UNKNOWN,
-	EMUL_ARM_FAULT,
+	bool invert = (insn >> 28) & 1;
-};
+	bool match;
 	switch (insn >> 29) {
 	case 0:	/* EQ or NE */
 		match = spsr & SPSR_Z;
 		break;
 	case 1:	/* CS/HI or CC/LO */
 		match = spsr & SPSR_C;
 		break;
 	case 2:	/* MI or PL */
 		match = spsr & SPSR_N;
 		break;
 	case 3:	/* VS or VC */
 		match = spsr & SPSR_V;
 		break;
 	case 4:	/* HI or LS */
 		match = ((spsr & (SPSR_C | SPSR_Z)) == SPSR_C);
 		break;
 	case 5:	/* GE or LT */
 		match = (!(spsr & SPSR_N) == !(spsr & SPSR_V));
 		break;
 	case 6:	/* GT or LE */
 		match = !(spsr & SPSR_Z) &&
 		    (!(spsr & SPSR_N) == !(spsr & SPSR_V));
 		break;
 	case 7:	/* AL */
 		match = true;
 		break;
+	}
 	return (!match != !invert);
+}
 static enum emul_arm_result
 emul_thumb_insn(struct trapframe *tf, uint32_t insn, int insn_size)
+{
 	/* T32-16bit or 32bit instructions */
 	switch (insn_size) {
 	case 2:
 		/* Breakpoint used by GDB */
 		if (insn == 0xdefe) {
 			do_trapsignal(curlwp, SIGTRAP, TRAP_BRKPT,
 			    (void *)tf->tf_pc, 0);
 			return EMUL_ARM_SUCCESS;
+		}
 		/* XXX: some T32 IT instruction deprecated should be emulated */
 		break;
 	case 4:
 		break;
 	default:
 		return EMUL_ARM_FAULT;
+	}
 	return EMUL_ARM_UNKNOWN;
+}
 static enum emul_arm_result
 emul_arm_insn(struct trapframe *tf)
+{
 	struct lwp * const l = curlwp;
 	uint32_t insn;
 	int insn_size;
-	insn_size = fetch_arm_insn(tf, &insn);
+	insn_size = fetch_arm_insn(tf->tf_pc, tf->tf_spsr, &insn);
 	tf->tf_far = reg_far_el1_read();
-	switch (insn_size) {
+	if (tf->tf_spsr & SPSR_A32_T)
-	case 2:
+		return emul_thumb_insn(tf, insn, insn_size);
-		/* T32-16bit instruction */
+	if (insn_size != 4)
 		return EMUL_ARM_FAULT;
 		/*
 		 * Breakpoint used by GDB.
 		 */
 		if (insn == 0xdefe)
 			goto trap;
-		/* XXX: some T32 IT instruction deprecated should be emulated */
+	/* Breakpoint used by GDB */
-		break;
+	if (insn == 0xe6000011 || insn == 0xe7ffdefe) {
-	case 4:
+		do_trapsignal(curlwp, SIGTRAP, TRAP_BRKPT,
-		/* T32-32bit instruction, or A32 instruction */
+		    (void *)tf->tf_pc, 0);
 		return EMUL_ARM_SUCCESS;
+	}
-		/*
+	/* Unconditional instruction extension space? */
-		 * Breakpoint used by GDB.
+	if ((insn & 0xf0000000) == 0xf0000000)
-		 */
+		goto unknown_insn;
 		if (insn == 0xe6000011 || insn == 0xe7ffdefe) {
  trap:
 			do_trapsignal(l, SIGTRAP, TRAP_BRKPT,
 			    (void *)tf->tf_pc, 0);
 			return 0;
-		/*
+	/*
-		 * Emulate ARMv6 instructions with cache operations
+	 * Emulate ARMv6 instructions with cache operations
-		 * register (c7), that can be used in user mode.
+	 * register (c7), that can be used in user mode.
-		 */
+	 */
-		switch (insn & 0x0fff0fff) {
+	switch (insn & 0x0fff0fff) {
-		case 0x0e070f95:
+	case 0x0e070f95:
 		if (arm_cond_match(insn, tf->tf_spsr)) {
 			/*
 			 * mcr p15, 0, <Rd>, c7, c5, 4
 			 * (flush prefetch buffer)
 			 */
 			__asm __volatile("isb sy" ::: "memory");
 			goto emulated;
-		case 0x0e070f9a:
+		goto emulated;
 	case 0x0e070f9a:
 		if (arm_cond_match(insn, tf->tf_spsr)) {
 			/*
 			 * mcr p15, 0, <Rd>, c7, c10, 4
 			 * (data synchronization barrier)
 			 */
 			__asm __volatile("dsb sy" ::: "memory");
 			goto emulated;
-		case 0x0e070fba:
+		goto emulated;
 	case 0x0e070fba:
 		if (arm_cond_match(insn, tf->tf_spsr)) {
 			/*
 			 * mcr p15, 0, <Rd>, c7, c10, 5
 			 * (data memory barrier)
 			 */
 			__asm __volatile("dmb sy" ::: "memory");
 			goto emulated;
 		default:
 			break;
+		}
-		break;
+		goto emulated;
 	default:
-		return EMUL_ARM_FAULT;
+		break;
+	}
  unknown_insn:
 	/* unknown, or unsupported instruction */
 	return EMUL_ARM_UNKNOWN;
  emulated:
 	tf->tf_pc += insn_size;
 	return EMUL_ARM_SUCCESS;
+}
 #endif /* COMPAT_NETBSD32 */
 void
 trap_el0_32sync(struct trapframe *tf)
+{
 	struct lwp * const l = curlwp;
 @@ -515,27 +567,27 @@ trap_el0_32sync(struct trapframe *tf)
 	case ESR_EC_BKPT_INSN_A32:
 		do_trapsignal(l, SIGTRAP, TRAP_BRKPT, (void *)tf->tf_pc, esr);
 		userret(l);
 		break;
 	case ESR_EC_UNKNOWN:
 		switch (emul_arm_insn(tf)) {
 		case EMUL_ARM_SUCCESS:
 			break;
 		case EMUL_ARM_UNKNOWN:
 			goto unknown;
 		case EMUL_ARM_FAULT:
 			do_trapsignal(l, SIGSEGV, SEGV_MAPERR,
-			    (void *)tf->tf_pc, esr);
+			    (void *)tf->tf_far, esr);
 			break;
+		}
 		userret(l);
 		break;
 	case ESR_EC_CP15_RT:
 	case ESR_EC_CP15_RRT:
 	case ESR_EC_CP14_RT:
 	case ESR_EC_CP14_DT:
 	case ESR_EC_CP14_RRT:
 unknown:
 #endif /* COMPAT_NETBSD32 */
 	default: