Index: sys/arm/arm/machdep.c
===================================================================
--- sys/arm/arm/machdep.c
+++ sys/arm/arm/machdep.c
@@ -627,11 +627,140 @@
 	return proc_rwmem(td->td_proc, &uio);
 }
 
+static int
+ptrace_get_usr_reg(struct thread *td, uint8_t reg)
+{
+	int ret;
+
+	if (reg > ARM_REG_NUM_PC)
+		panic("invalid register number %d\n", reg);
+
+	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);
+}
+
+/**
+ * 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 offset = 0;
+	uint32_t reg, shift, tmp, address;
+
+	if (alt_next_address == NULL)
+		panic("alt_next_address is NULL\n");
+
+	/* B/BL instruction */
+	if (DB_ARM_INST_IS_BRANCH(cur_instr)) {
+		offset = cur_instr & (DB_ARM_INST_BRANCH_OFF_MASK);
+		/* Check sign */
+		if (offset >= (DB_ARM_INST_BRANCH_OFF_MASK/2 + 1)) {
+			/* Negative value */
+			offset = ~offset & DB_ARM_INST_BRANCH_OFF_MASK;
+			offset = offset + 1;
+			*alt_next_address = td->td_frame->tf_pc -
+			    (offset * INSN_SIZE) + DB_ARM_INST_BRANCH_PC_AHEAD;
+		} else {
+			*alt_next_address = td->td_frame->tf_pc +
+			    (offset * INSN_SIZE) + DB_ARM_INST_BRANCH_PC_AHEAD;
+		}
+		return (0);
+	}
+
+	/* BX/BLX */
+	if (DB_ARM_INST_IS_BRANCHX(cur_instr)) {
+		reg = cur_instr & DB_ARM_INST_BRANCHX_REG_MASK;
+		*alt_next_address = ptrace_get_usr_reg(td, reg);
+		return (0);
+	}
+
+	/* LDR PC [reg, offset] */
+	if (DB_ARM_INST_IS_LDR_PC(cur_instr)) {
+		/* Reg offset */
+		if (cur_instr & DB_ARM_INST_LDR_PC_IOREG_OFF_FLAG) {
+			/* OFFSET field is in format: <shift>:<reg> */
+			ptrace_read_int(td, ptrace_get_usr_reg(td,
+			    cur_instr & DB_ARM_INST_LDR_PC_IOREG_MASK),
+			    &offset);
+			shift = (cur_instr >> DB_ARM_INST_LDR_PC_SHIFT_OFF) &
+			    DB_ARM_INST_LDR_PC_SHIFT_MASK;
+			offset = offset << shift;
+		} else { /* Immediate, unsigned offset */
+			offset = cur_instr & DB_ARM_INST_LDR_PC_IMM_MASK;
+		}
+
+		/* Check if offset should be subtracted */
+		if ((cur_instr & DB_ARM_INST_LDR_PC_REG_UP_FLAG) == 0)
+			offset = -offset;
+
+		reg = (cur_instr >> DB_ARM_INST_LDR_PC_BASEREG_OFF) &
+		    DB_ARM_INST_LDR_PC_BASEREG_MASK;
+		address = ptrace_get_usr_reg(td, reg);
+		ptrace_read_int(td, address + offset, &tmp);
+
+		*alt_next_address = tmp;
+		return (0);
+	}
+
+	/* POP {rX, fp, pc} */
+	if (DB_ARM_INST_IS_POP_PC(cur_instr)) {
+		int a, cnt = 0;
+
+		/* Count registers lower than PC */
+		for (a = 0; a < ARM_REG_NUM_PC; a++) {
+			if (cur_instr & (1 << a))
+				cnt++;
+		}
+
+		/* Read alternative PC from the stack */
+		ptrace_read_int(td, td->td_frame->tf_usr_sp +
+		    ARM_REG_SIZE * cnt, &tmp);
+
+		*alt_next_address = tmp;
+		return (0);
+	}
+
+	/*
+	 * TODO: Other, rare instructions
+	 *       add     pc, pc, r2, lsl #2
+	 */
+
+	return (EINVAL);
+}
+
 int
 ptrace_single_step(struct thread *td)
 {
 	struct proc *p;
 	int error;
+	uint32_t cur_instr, alt_next;
 
 	/* TODO: This needs to be updated for Thumb-2 */
 	if ((td->td_frame->tf_spsr & PSR_T) != 0)
@@ -639,17 +768,44 @@
 
 	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,
+
+	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;
-	td->td_md.md_ptrace_addr = td->td_frame->tf_pc + 4;
+		else
+			td->td_md.md_ptrace_addr = td->td_frame->tf_pc +
+			    INSN_SIZE;
+	}
+
+	error = ptrace_get_alternative_next(td, cur_instr, &alt_next);
+	if (error == 0) {
+		error = ptrace_read_int(td, alt_next,
+		    &td->td_md.md_ptrace_instr_alt);
+		if (error) {
+			td->td_md.md_ptrace_instr_alt = 0;
+		} else {
+			error = ptrace_write_int(td, alt_next,
+			    PTRACE_BREAKPOINT);
+			if (error)
+				td->td_md.md_ptrace_instr_alt = 0;
+			else
+				td->td_md.md_ptrace_addr_alt = alt_next;
+		}
+	}
+
 out:
 	PROC_LOCK(p);
 	return (error);
@@ -664,7 +820,7 @@
 	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 +828,16 @@
 		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);
 }
 
Index: sys/arm/include/armreg.h
===================================================================
--- sys/arm/include/armreg.h
+++ sys/arm/include/armreg.h
@@ -444,6 +444,44 @@
 #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
+
+/* ARM constants for 32-bit instruction opcode matching (for debug only) */
+#define	DB_ARM_INST_BRANCH_MASK		0x0E000000
+#define	DB_ARM_INST_BRANCH_MATCH	0x0A000000
+#define	DB_ARM_INST_IS_BRANCH(instr)					\
+    (((instr) & DB_ARM_INST_BRANCH_MASK) == DB_ARM_INST_BRANCH_MATCH)
+#define	DB_ARM_INST_BRANCH_OFF_MASK	0x00FFFFFF
+#define	DB_ARM_INST_BRANCH_PC_AHEAD	8
+
+#define	DB_ARM_INST_BRANCHX_MASK	0x0FFFFFD0
+#define	DB_ARM_INST_BRANCHX_MATCH	0x012FFF10
+#define	DB_ARM_INST_IS_BRANCHX(instr)					\
+    (((instr) & DB_ARM_INST_BRANCHX_MASK) == DB_ARM_INST_BRANCHX_MATCH)
+#define	DB_ARM_INST_BRANCHX_REG_MASK	0xF
+
+#define	DB_ARM_INST_LDR_PC_MASK		0x0C00F000
+#define	DB_ARM_INST_LDR_PC_MATCH	0x0400F000
+#define	DB_ARM_INST_IS_LDR_PC(instr)					\
+    (((instr) & DB_ARM_INST_LDR_PC_MASK) == DB_ARM_INST_LDR_PC_MATCH)
+#define	DB_ARM_INST_LDR_PC_IOREG_OFF_FLAG	(1 << 25)
+#define	DB_ARM_INST_LDR_PC_IOREG_MASK	0xF
+#define	DB_ARM_INST_LDR_PC_SHIFT_OFF	4
+#define	DB_ARM_INST_LDR_PC_SHIFT_MASK	0xFF
+#define	DB_ARM_INST_LDR_PC_IMM_MASK	0xFFF
+#define	DB_ARM_INST_LDR_PC_REG_UP_FLAG	(1 << 23)
+#define	DB_ARM_INST_LDR_PC_BASEREG_OFF	(16)
+#define	DB_ARM_INST_LDR_PC_BASEREG_MASK	0xF
+
+#define	DB_ARM_INST_POP_PC_MASK		0x0E108000
+#define	DB_ARM_INST_POP_PC_MATCH	0x08108000
+#define	DB_ARM_INST_IS_POP_PC(instr)					\
+    (((instr) & DB_ARM_INST_POP_PC_MASK) == DB_ARM_INST_POP_PC_MATCH)
+
 #define THUMB_INSN_SIZE		2		/* Some are 4 bytes.  */
 
 #endif /* !MACHINE_ARMREG_H */
Index: sys/arm/include/proc.h
===================================================================
--- sys/arm/include/proc.h
+++ sys/arm/include/proc.h
@@ -51,6 +51,8 @@
 	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;