Add support for branch instruction on armv7 with ptrace single step

Previous code supported only "continuous" code without any kind of
branch instructions. To change that, new function was implemented
which parses current instruction and returns an addres where
the jump might happen (alternative addr).
mdthread structure was extended to support two breakpoints
(one directly below current instruction and the second placed
at the alternative location).
One of them must trigger regardless the instruction has or has not been
executed due to condition field.
Upon cleanup, both software breakpoints are removed.

This implementation parses only the most common instructions
that are present in the code (like 99.99% of all), but there
is a chance there are some left, not covered by the parsing routine.
Parsing is done only for 32-bit instruction, no Thumb nor Thumb-2
support is provided.

Reviewed by:   kib
Submitted by:  Wojciech Macek <wma@semihalf.com>
Obtained from: Semihalf
Sponsored by:  Juniper Networks Inc.
Differential Revision: https://reviews.freebsd.org/D4021

6 files changed, 266 insertions, 99 deletions

diff --git a/sys/arm/arm/db_interface.c b/sys/arm/arm/db_interface.c
index 3a2515e..25d1706 100644
--- a/sys/arm/arm/db_interface.c
+++ b/sys/arm/arm/db_interface.c
@@ -53,6 +53,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_extern.h>
 
 #include <machine/db_machdep.h>
+#include <machine/machdep.h>
 #include <machine/vmparam.h>
 #include <machine/cpu.h>
 
@@ -291,94 +292,35 @@ db_fetch_reg(int reg)
 	}
 }
 
+static u_int
+db_branch_taken_read_int(void *cookie __unused, vm_offset_t offset, u_int *val)
+{
+	u_int ret;
+
+	db_read_bytes(offset, 4, (char *)&ret);
+	*val = ret;
+
+	return (0);
+}
+
+static u_int
+db_branch_taken_fetch_reg(void *cookie __unused, int reg)
+{
+
+	return (db_fetch_reg(reg));
+}
+
 u_int
 branch_taken(u_int insn, db_addr_t pc)
 {
-	u_int addr, nregs, offset = 0;
-
-	switch ((insn >> 24) & 0xf) {
-	case 0x2:	/* add pc, reg1, #value */
-	case 0x0:	/* add pc, reg1, reg2, lsl #offset */
-		addr = db_fetch_reg((insn >> 16) & 0xf);
-		if (((insn >> 16) & 0xf) == 15)
-			addr += 8;
-		if (insn & 0x0200000) {
-			offset = (insn >> 7) & 0x1e;
-			offset = (insn & 0xff) << (32 - offset) |
-			    (insn & 0xff) >> offset;
-		} else {
-
-			offset = db_fetch_reg(insn & 0x0f);
-			if ((insn & 0x0000ff0) != 0x00000000) {
-				if (insn & 0x10)
-					nregs = db_fetch_reg((insn >> 8) & 0xf);
-				else
-					nregs = (insn >> 7) & 0x1f;
-				switch ((insn >> 5) & 3) {
-				case 0:
-					/* lsl */
-					offset = offset << nregs;
-					break;
-				case 1:
-					/* lsr */
-					offset = offset >> nregs;
-					break;
-				default:
-					break; /* XXX */
-				}
-			}
-			return (addr + offset);
-		}
-	case 0xa:	/* b ... */
-	case 0xb:	/* bl ... */
-		addr = ((insn << 2) & 0x03ffffff);
-		if (addr & 0x02000000)
-			addr |= 0xfc000000;
-		return (pc + 8 + addr);
-	case 0x7:	/* ldr pc, [pc, reg, lsl #2] */
-		addr = db_fetch_reg(insn & 0xf);
-		addr = pc + 8 + (addr << 2);
-		db_read_bytes(addr, 4, (char *)&addr);
-		return (addr);
-	case 0x1:	/* mov pc, reg */
-		addr = db_fetch_reg(insn & 0xf);
-		return (addr);
-	case 0x4:
-	case 0x5:	/* ldr pc, [reg] */
-		addr = db_fetch_reg((insn >> 16) & 0xf);
-		/* ldr pc, [reg, #offset] */
-		if (insn & (1 << 24))
-			offset = insn & 0xfff;
-		if (insn & 0x00800000)
-			addr += offset;
-		else
-			addr -= offset;
-		db_read_bytes(addr, 4, (char *)&addr);
-		return (addr);
-	case 0x8:	/* ldmxx reg, {..., pc} */
-	case 0x9:
-		addr = db_fetch_reg((insn >> 16) & 0xf);
-		nregs = (insn  & 0x5555) + ((insn  >> 1) & 0x5555);
-		nregs = (nregs & 0x3333) + ((nregs >> 2) & 0x3333);
-		nregs = (nregs + (nregs >> 4)) & 0x0f0f;
-		nregs = (nregs + (nregs >> 8)) & 0x001f;
-		switch ((insn >> 23) & 0x3) {
-		case 0x0:	/* ldmda */
-			addr = addr - 0;
-			break;
-		case 0x1:	/* ldmia */
-			addr = addr + 0 + ((nregs - 1) << 2);
-			break;
-		case 0x2:	/* ldmdb */
-			addr = addr - 4;
-			break;
-		case 0x3:	/* ldmib */
-			addr = addr + 4 + ((nregs - 1) << 2);
-			break;
-		}
-		db_read_bytes(addr, 4, (char *)&addr);
-		return (addr);
-	default:
-		panic("branch_taken: botch");
-	}
+	register_t new_pc;
+	int ret;
+
+	ret = arm_predict_branch(NULL, insn, (register_t)pc, &new_pc,
+	    db_branch_taken_fetch_reg, db_branch_taken_read_int);
+
+	if (ret != 0)
+		kdb_reenter();
+
+	return (new_pc);
 }
diff --git a/sys/arm/arm/machdep.c b/sys/arm/arm/machdep.c
index 1c422f3..bd01383 100644
--- a/sys/arm/arm/machdep.c
+++ b/sys/arm/arm/machdep.c
@@ -95,6 +95,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/atags.h>
 #include <machine/cpu.h>
 #include <machine/cpuinfo.h>
+#include <machine/db_machdep.h>
 #include <machine/devmap.h>
 #include <machine/frame.h>
 #include <machine/intr.h>
@@ -627,11 +628,81 @@ ptrace_write_int(struct thread *td, vm_offset_t addr, u_int32_t v)
 	return proc_rwmem(td->td_proc, &uio);
 }
 
+static u_int
+ptrace_get_usr_reg(void *cookie, int reg)
+{
+	int ret;
+	struct thread *td = cookie;
+
+	KASSERT(((reg >= 0) && (reg <= ARM_REG_NUM_PC)),
+	 ("reg is outside range"));
+
+	switch(reg) {
+	case ARM_REG_NUM_PC:
+		ret = td->td_frame->tf_pc;
+		break;
+	case ARM_REG_NUM_LR:
+		ret = td->td_frame->tf_usr_lr;
+		break;
+	case ARM_REG_NUM_SP:
+		ret = td->td_frame->tf_usr_sp;
+		break;
+	default:
+		ret = *((register_t*)&td->td_frame->tf_r0 + reg);
+		break;
+	}
+
+	return (ret);
+}
+
+static u_int
+ptrace_get_usr_int(void* cookie, vm_offset_t offset, u_int* val)
+{
+	struct thread *td = cookie;
+	u_int error;
+
+	error = ptrace_read_int(td, offset, val);
+
+	return (error);
+}
+
+/**
+ * This function parses current instruction opcode and decodes
+ * any possible jump (change in PC) which might occur after
+ * the instruction is executed.
+ *
+ * @param     td                Thread structure of analysed task
+ * @param     cur_instr         Currently executed instruction
+ * @param     alt_next_address  Pointer to the variable where
+ *                              the destination address of the
+ *                              jump instruction shall be stored.
+ *
+ * @return    <0>               when jump is possible
+ *            <EINVAL>          otherwise
+ */
+static int
+ptrace_get_alternative_next(struct thread *td, uint32_t cur_instr,
+    uint32_t *alt_next_address)
+{
+	int error;
+
+	if (inst_branch(cur_instr) || inst_call(cur_instr) ||
+	    inst_return(cur_instr)) {
+		error = arm_predict_branch(td, cur_instr, td->td_frame->tf_pc,
+		    alt_next_address, ptrace_get_usr_reg, ptrace_get_usr_int);
+
+		return (error);
+	}
+
+	return (EINVAL);
+}
+
 int
 ptrace_single_step(struct thread *td)
 {
 	struct proc *p;
-	int error;
+	int error, error_alt;
+	uint32_t cur_instr, alt_next = 0;
 
 	/* TODO: This needs to be updated for Thumb-2 */
 	if ((td->td_frame->tf_spsr & PSR_T) != 0)
@@ -639,20 +710,48 @@ ptrace_single_step(struct thread *td)
 
 	KASSERT(td->td_md.md_ptrace_instr == 0,
 	 ("Didn't clear single step"));
+	KASSERT(td->td_md.md_ptrace_instr_alt == 0,
+	 ("Didn't clear alternative single step"));
 	p = td->td_proc;
 	PROC_UNLOCK(p);
-	error = ptrace_read_int(td, td->td_frame->tf_pc + 4,
-	    &td->td_md.md_ptrace_instr);
+
+	error = ptrace_read_int(td, td->td_frame->tf_pc,
+	    &cur_instr);
 	if (error)
 		goto out;
-	error = ptrace_write_int(td, td->td_frame->tf_pc + 4,
-	    PTRACE_BREAKPOINT);
-	if (error)
-		td->td_md.md_ptrace_instr = 0;
-	td->td_md.md_ptrace_addr = td->td_frame->tf_pc + 4;
+
+	error = ptrace_read_int(td, td->td_frame->tf_pc + INSN_SIZE,
+	    &td->td_md.md_ptrace_instr);
+	if (error == 0) {
+		error = ptrace_write_int(td, td->td_frame->tf_pc + INSN_SIZE,
+		    PTRACE_BREAKPOINT);
+		if (error) {
+			td->td_md.md_ptrace_instr = 0;
+		} else {
+			td->td_md.md_ptrace_addr = td->td_frame->tf_pc +
+			    INSN_SIZE;
+		}
+	}
+
+	error_alt = ptrace_get_alternative_next(td, cur_instr, &alt_next);
+	if (error_alt == 0) {
+		error_alt = ptrace_read_int(td, alt_next,
+		    &td->td_md.md_ptrace_instr_alt);
+		if (error_alt) {
+			td->td_md.md_ptrace_instr_alt = 0;
+		} else {
+			error_alt = ptrace_write_int(td, alt_next,
+			    PTRACE_BREAKPOINT);
+			if (error_alt)
+				td->td_md.md_ptrace_instr_alt = 0;
+			else
+				td->td_md.md_ptrace_addr_alt = alt_next;
+		}
+	}
+
 out:
 	PROC_LOCK(p);
-	return (error);
+	return ((error != 0) && (error_alt != 0));
 }
 
 int
@@ -664,7 +763,7 @@ ptrace_clear_single_step(struct thread *td)
 	if ((td->td_frame->tf_spsr & PSR_T) != 0)
 		return (EINVAL);
 
-	if (td->td_md.md_ptrace_instr) {
+	if (td->td_md.md_ptrace_instr != 0) {
 		p = td->td_proc;
 		PROC_UNLOCK(p);
 		ptrace_write_int(td, td->td_md.md_ptrace_addr,
@@ -672,6 +771,16 @@ ptrace_clear_single_step(struct thread *td)
 		PROC_LOCK(p);
 		td->td_md.md_ptrace_instr = 0;
 	}
+
+	if (td->td_md.md_ptrace_instr_alt != 0) {
+		p = td->td_proc;
+		PROC_UNLOCK(p);
+		ptrace_write_int(td, td->td_md.md_ptrace_addr_alt,
+		    td->td_md.md_ptrace_instr_alt);
+		PROC_LOCK(p);
+		td->td_md.md_ptrace_instr_alt = 0;
+	}
+
 	return (0);
 }
 
@@ -1074,6 +1183,111 @@ init_proc0(vm_offset_t kstack)
 	pcpup->pc_curpcb = thread0.td_pcb;
 }
 
+int
+arm_predict_branch(void *cookie, u_int insn, register_t pc, register_t *new_pc,
+    u_int (*fetch_reg)(void*, int), u_int (*read_int)(void*, vm_offset_t, u_int*))
+{
+	u_int addr, nregs, offset = 0;
+	int error = 0;
+
+	switch ((insn >> 24) & 0xf) {
+	case 0x2:	/* add pc, reg1, #value */
+	case 0x0:	/* add pc, reg1, reg2, lsl #offset */
+		addr = fetch_reg(cookie, (insn >> 16) & 0xf);
+		if (((insn >> 16) & 0xf) == 15)
+			addr += 8;
+		if (insn & 0x0200000) {
+			offset = (insn >> 7) & 0x1e;
+			offset = (insn & 0xff) << (32 - offset) |
+			    (insn & 0xff) >> offset;
+		} else {
+
+			offset = fetch_reg(cookie, insn & 0x0f);
+			if ((insn & 0x0000ff0) != 0x00000000) {
+				if (insn & 0x10)
+					nregs = fetch_reg(cookie,
+					    (insn >> 8) & 0xf);
+				else
+					nregs = (insn >> 7) & 0x1f;
+				switch ((insn >> 5) & 3) {
+				case 0:
+					/* lsl */
+					offset = offset << nregs;
+					break;
+				case 1:
+					/* lsr */
+					offset = offset >> nregs;
+					break;
+				default:
+					break; /* XXX */
+				}
+
+			}
+			*new_pc = addr + offset;
+			return (0);
+
+		}
+
+	case 0xa:	/* b ... */
+	case 0xb:	/* bl ... */
+		addr = ((insn << 2) & 0x03ffffff);
+		if (addr & 0x02000000)
+			addr |= 0xfc000000;
+		*new_pc = (pc + 8 + addr);
+		return (0);
+	case 0x7:	/* ldr pc, [pc, reg, lsl #2] */
+		addr = fetch_reg(cookie, insn & 0xf);
+		addr = pc + 8 + (addr << 2);
+		error = read_int(cookie, addr, &addr);
+		*new_pc = addr;
+		return (error);
+	case 0x1:	/* mov pc, reg */
+		*new_pc = fetch_reg(cookie, insn & 0xf);
+		return (0);
+	case 0x4:
+	case 0x5:	/* ldr pc, [reg] */
+		addr = fetch_reg(cookie, (insn >> 16) & 0xf);
+		/* ldr pc, [reg, #offset] */
+		if (insn & (1 << 24))
+			offset = insn & 0xfff;
+		if (insn & 0x00800000)
+			addr += offset;
+		else
+			addr -= offset;
+		error = read_int(cookie, addr, &addr);
+		*new_pc = addr;
+
+		return (error);
+	case 0x8:	/* ldmxx reg, {..., pc} */
+	case 0x9:
+		addr = fetch_reg(cookie, (insn >> 16) & 0xf);
+		nregs = (insn  & 0x5555) + ((insn  >> 1) & 0x5555);
+		nregs = (nregs & 0x3333) + ((nregs >> 2) & 0x3333);
+		nregs = (nregs + (nregs >> 4)) & 0x0f0f;
+		nregs = (nregs + (nregs >> 8)) & 0x001f;
+		switch ((insn >> 23) & 0x3) {
+		case 0x0:	/* ldmda */
+			addr = addr - 0;
+			break;
+		case 0x1:	/* ldmia */
+			addr = addr + 0 + ((nregs - 1) << 2);
+			break;
+		case 0x2:	/* ldmdb */
+			addr = addr - 4;
+			break;
+		case 0x3:	/* ldmib */
+			addr = addr + 4 + ((nregs - 1) << 2);
+			break;
+		}
+		error = read_int(cookie, addr, &addr);
+		*new_pc = addr;
+
+		return (error);
+	default:
+		return (EINVAL);
+	}
+}
+
 #ifdef ARM_NEW_PMAP
 void
 set_stackptrs(int cpu)
diff --git a/sys/arm/include/armreg.h b/sys/arm/include/armreg.h
index a300ddf..aa0e713 100644
--- a/sys/arm/include/armreg.h
+++ b/sys/arm/include/armreg.h
@@ -444,6 +444,12 @@
 #define INSN_COND_MASK		0xf0000000	/* Condition mask */
 #define INSN_COND_AL		0xe0000000	/* Always condition */
 
+/* ARM register defines */
+#define	ARM_REG_SIZE		4
+#define	ARM_REG_NUM_PC		15
+#define	ARM_REG_NUM_LR		14
+#define	ARM_REG_NUM_SP		13
+
 #define THUMB_INSN_SIZE		2		/* Some are 4 bytes.  */
 
 #endif /* !MACHINE_ARMREG_H */
diff --git a/sys/arm/include/db_machdep.h b/sys/arm/include/db_machdep.h
index 741cae9..42d3135 100644
--- a/sys/arm/include/db_machdep.h
+++ b/sys/arm/include/db_machdep.h
@@ -74,7 +74,7 @@ typedef int		db_expr_t;
 
 #define	inst_branch(ins)	(((ins) & 0x0f000000) == 0x0a000000 || \
 				 ((ins) & 0x0fdffff0) == 0x079ff100 || \
-				 ((ins) & 0x0cf0f000) == 0x0490f000 || \
+				 ((ins) & 0x0cd0f000) == 0x0490f000 || \
 				 ((ins) & 0x0ffffff0) == 0x012fff30 || /* blx */ \
 				 ((ins) & 0x0de0f000) == 0x0080f000)
 
@@ -90,7 +90,7 @@ typedef int		db_expr_t;
 
 int db_validate_address(vm_offset_t);
 
-u_int branch_taken (u_int insn, u_int pc);
+u_int branch_taken (u_int insn, db_addr_t pc);
 
 #ifdef __ARMEB__
 #define BYTE_MSF	(1)
diff --git a/sys/arm/include/machdep.h b/sys/arm/include/machdep.h
index 0f43284..fdf59be 100644
--- a/sys/arm/include/machdep.h
+++ b/sys/arm/include/machdep.h
@@ -43,4 +43,7 @@ void arm_generic_initclocks(void);
 void board_set_serial(uint64_t);
 void board_set_revision(uint32_t);
 
+int arm_predict_branch(void *, u_int, register_t, register_t *,
+    u_int (*)(void*, int), u_int (*)(void*, vm_offset_t, u_int*));
+
 #endif /* !_MACHINE_MACHDEP_H_ */
diff --git a/sys/arm/include/proc.h b/sys/arm/include/proc.h
index 1409a9d..090aaba 100644
--- a/sys/arm/include/proc.h
+++ b/sys/arm/include/proc.h
@@ -51,6 +51,8 @@ struct mdthread {
 	register_t md_spurflt_addr;     /* (k) Spurious page fault address. */
 	int md_ptrace_instr;
 	int md_ptrace_addr;
+	int md_ptrace_instr_alt;
+	int md_ptrace_addr_alt;
 	register_t md_tp;
 	void *md_ras_start;
 	void *md_ras_end;