Merge branch 'dev' into kernel

Conflicts:
	src/modules/kprobe/arch/asm-arm/dbi_kprobes.c
	src/modules/kprobe/arch/asm-arm/dbi_kprobes_thumb.h

5 files changed, 229 insertions, 16 deletions

diff --git a/uprobe/arch/asm-arm/swap_uprobes.c b/uprobe/arch/asm-arm/swap_uprobes.c
index 77ec1223..8f7764da 100644
--- a/uprobe/arch/asm-arm/swap_uprobes.c
+++ b/uprobe/arch/asm-arm/swap_uprobes.c
@@ -19,6 +19,40 @@
 #define sign_extend(x, signbit) ((x) | (0 - ((x) & (1 << (signbit)))))
 #define branch_displacement(insn) sign_extend(((insn) & 0xffffff) << 2, 25)
 
+static inline long branch_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
+{
+	long offset = insn & 0x3ff;
+	offset -= insn & 0x400;
+	return (insn_addr + 4 + offset * 2);
+}
+
+static inline long branch_cond_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
+{
+	long offset = insn & 0x7f;
+	offset -= insn & 0x80;
+	return (insn_addr + 4 + offset * 2);
+}
+
+static inline long branch_t32_dest(kprobe_opcode_t insn, unsigned int insn_addr)
+{
+	unsigned int poff = insn & 0x3ff;
+	unsigned int offset = (insn & 0x07fe0000) >> 17;
+
+	poff -= (insn & 0x400);
+
+	if (insn & (1 << 12))
+		return ((insn_addr + 4 + (poff << 12) + offset * 4));
+	else
+	return ((insn_addr + 4 + (poff << 12) + offset * 4) & ~3);
+}
+
+static inline long cbz_t16_dest(kprobe_opcode_t insn, unsigned int insn_addr)
+{
+	unsigned int i = (insn & 0x200) >> 3;
+	unsigned int offset = (insn & 0xf8) >> 2;
+	return insn_addr + 4 + i + offset;
+}
+
 static kprobe_opcode_t get_addr_b(kprobe_opcode_t insn, kprobe_opcode_t *addr)
 {
 	// real position less then PC by 8
@@ -150,15 +184,8 @@ static int arch_check_insn_thumb(struct arch_specific_insn *ainsn)
 	if (THUMB_INSN_MATCH(UNDEF, ainsn->insn_thumb[0]) ||
 	    THUMB_INSN_MATCH(SWI, ainsn->insn_thumb[0]) ||
 	    THUMB_INSN_MATCH(BREAK, ainsn->insn_thumb[0]) ||
-	    THUMB2_INSN_MATCH(BL, ainsn->insn_thumb[0]) ||
-	    THUMB_INSN_MATCH(B1, ainsn->insn_thumb[0]) ||
-	    THUMB_INSN_MATCH(B2, ainsn->insn_thumb[0]) ||
-	    THUMB_INSN_MATCH(CBZ, ainsn->insn_thumb[0]) ||
 	    THUMB2_INSN_MATCH(B1, ainsn->insn_thumb[0]) ||
 	    THUMB2_INSN_MATCH(B2, ainsn->insn_thumb[0]) ||
-	    THUMB2_INSN_MATCH(BLX1, ainsn->insn_thumb[0]) ||
-	    THUMB_INSN_MATCH(BLX2, ainsn->insn_thumb[0]) ||
-	    THUMB_INSN_MATCH(BX, ainsn->insn_thumb[0]) ||
 	    THUMB2_INSN_MATCH(BXJ, ainsn->insn_thumb[0]) ||
 	    (THUMB2_INSN_MATCH(ADR, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RD(ainsn->insn_thumb[0]) == 15) ||
 	    (THUMB2_INSN_MATCH(LDRW, ainsn->insn_thumb[0]) && THUMB2_INSN_REG_RT(ainsn->insn_thumb[0]) == 15) ||
@@ -526,6 +553,59 @@ static int arch_copy_trampoline_thumb_uprobe(struct kprobe *p, struct task_struc
 		}
 	}
 
+	if (THUMB_INSN_MATCH(B2, insn[0])) {
+		memcpy(insns, b_off_insn_execbuf_thumb, sizeof(insns));
+		*((unsigned short*)insns + 13) = 0xdeff;
+		addr = branch_t16_dest(insn[0], (unsigned int)p->addr);
+		*((unsigned short*)insns + 14) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 15) = addr >> 16;
+		*((unsigned short*)insns + 16) = 0;
+		*((unsigned short*)insns + 17) = 0;
+
+	} else if (THUMB_INSN_MATCH(B1, insn[0])) {
+		memcpy(insns, b_cond_insn_execbuf_thumb, sizeof(insns));
+		*((unsigned short*)insns + 13) = 0xdeff;
+		*((unsigned short*)insns + 0) |= (insn[0] & 0xf00);
+		addr = branch_cond_t16_dest(insn[0], (unsigned int)p->addr);
+		*((unsigned short*)insns + 14) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 15) = addr >> 16;
+		addr = ((unsigned int)p->addr) + 2;
+		*((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 17) = addr >> 16;
+
+	} else if (THUMB_INSN_MATCH(BLX2, insn[0]) ||
+		   THUMB_INSN_MATCH(BX, insn[0])) {
+		memcpy(insns, b_r_insn_execbuf_thumb, sizeof(insns));
+		*((unsigned short*)insns + 13) = 0xdeff;
+		*((unsigned short*)insns + 4) = insn[0];
+		addr = ((unsigned int)p->addr) + 2;
+		*((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 17) = addr >> 16;
+
+	} else if (THUMB2_INSN_MATCH(BLX1, insn[0]) ||
+		   THUMB2_INSN_MATCH(BL, insn[0])) {
+		memcpy(insns, blx_off_insn_execbuf_thumb, sizeof(insns));
+		*((unsigned short*)insns + 13) = 0xdeff;
+		addr = branch_t32_dest(insn[0], (unsigned int)p->addr);
+		*((unsigned short*)insns + 14) = (addr & 0x0000ffff);
+		*((unsigned short*)insns + 15) = addr >> 16;
+		addr = ((unsigned int)p->addr) + 4;
+		*((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 17) = addr >> 16;
+
+	} else if (THUMB_INSN_MATCH(CBZ, insn[0])) {
+		memcpy(insns, cbz_insn_execbuf_thumb, sizeof(insns));
+		*((unsigned short*)insns + 13) = 0xdeff;
+		*((unsigned short*)insns + 0) = insn[0] &  (~insn[0] & 0xf8);
+		*((unsigned short*)insns + 0) &= 0x20;
+		addr = cbz_t16_dest(insn[0], (unsigned int)p->addr);
+		*((unsigned short*)insns + 14) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 15) = addr >> 16;
+		addr = ((unsigned int)p->addr) + 2;
+		*((unsigned short*)insns + 16) = (addr & 0x0000ffff) | 0x1;
+		*((unsigned short*)insns + 17) = addr >> 16;
+	}
+
 	if (!write_proc_vm_atomic (task, (unsigned long)p->ainsn.insn_thumb, insns, 18 * 2)) {
 		panic("failed to write memory %p!\n", p->ainsn.insn_thumb);
 		// Mr_Nobody: we have to panic, really??...
@@ -594,6 +674,17 @@ int arch_prepare_uprobe(struct uprobe *up, struct hlist_head *page_list)
 	return ret;
 }
 
+int arch_opcode_analysis_uretprobe(kprobe_opcode_t opcode)
+{
+	if (THUMB_INSN_MATCH(BLX2, opcode) ||
+	    THUMB2_INSN_MATCH(BL, opcode) ||
+	    THUMB2_INSN_MATCH(BLX1, opcode)) {
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 void arch_prepare_uretprobe(struct uretprobe_instance *ri,
 			    struct pt_regs *regs)
 {
diff --git a/uprobe/arch/asm-arm/swap_uprobes.h b/uprobe/arch/asm-arm/swap_uprobes.h
index a0c23921..3c5e2fff 100644
--- a/uprobe/arch/asm-arm/swap_uprobes.h
+++ b/uprobe/arch/asm-arm/swap_uprobes.h
@@ -18,6 +18,7 @@ static inline int longjmp_break_uhandler(struct kprobe *p, struct pt_regs *regs)
 	return 0;
 }
 
+int arch_opcode_analysis_uretprobe(kprobe_opcode_t opcode);
 void arch_prepare_uretprobe(struct uretprobe_instance *ri, struct pt_regs *regs);
 
 unsigned long arch_get_trampoline_addr(struct kprobe *p, struct pt_regs *regs);
diff --git a/uprobe/arch/asm-arm/trampoline_thumb.S b/uprobe/arch/asm-arm/trampoline_thumb.S
index 94ba3ddd..508c4404 100644
--- a/uprobe/arch/asm-arm/trampoline_thumb.S
+++ b/uprobe/arch/asm-arm/trampoline_thumb.S
@@ -44,3 +44,113 @@ i1:		nop			// stored PC hi
 		nop			// stored PC lo
 i2:		nop			// stored PC-4(next insn addr) hi
 		nop			// stored PC-4(next insn addr) lo
+
+		.global b_r_insn_execbuf_thumb
+		.align 4
+b_r_insn_execbuf_thumb:
+                nop
+                nop
+                nop
+                nop
+                nop                     // bx,blx (Rm)
+                nop                     //
+                push {r0,r1}
+                ldr r0, np
+                nop
+                str r0, [sp, #4]
+                pop {r0,pc}
+                nop
+                nop                     // ssbreak
+                nop                     // retbreak
+                nop
+                nop
+np:             nop                     // stored PC-4(next insn addr) hi
+                nop                     // stored PC-4(next insn addr) lo
+
+		.global b_off_insn_execbuf_thumb
+		.align 4
+b_off_insn_execbuf_thumb:
+                push {r0,r1}
+                ldr r0, bd
+                str r0, [sp, #4]
+                pop {r0, pc}
+                nop
+                nop
+                push {r0,r1}
+                ldr r0, np2
+                nop
+                str r0, [sp, #4]
+                pop {r0,pc}
+                nop
+                nop                     // ssbreak
+                nop                     // retbreak
+bd:		nop			// branch displacement hi
+                nop			// branch displacement lo
+np2:            nop                     // stored PC-4(next insn addr) hi
+                nop                     // stored PC-4(next insn addr) lo
+
+		.global blx_off_insn_execbuf_thumb
+		.align 4
+blx_off_insn_execbuf_thumb:
+                push {r0}
+                ldr r0, bd3
+                mov lr, r0
+                pop {r0}
+                blx lr
+                nop
+                push {r0,r1}
+                ldr r0, np3
+                nop
+                str r0, [sp, #4]
+                pop {r0,pc}
+                nop
+                nop                     // ssbreak
+                nop                     // retbreak
+bd3:		nop			// branch displacement hi
+                nop			// branch displacement lo
+np3:            nop                     // stored PC-4(next insn addr) hi
+                nop                     // stored PC-4(next insn addr) lo
+
+		.global b_cond_insn_execbuf_thumb
+		.align 4
+b_cond_insn_execbuf_thumb:
+		beq condway
+                push {r0,r1}
+                ldr r0, np4
+                nop
+                str r0, [sp, #4]
+                pop {r0,pc}
+condway:	push {r0,r1}
+                ldr r0, bd4
+                str r0, [sp, #4]
+                pop {r0,pc}
+                nop
+                nop
+                nop                     // ssbreak
+                nop                     // retbreak
+bd4:		nop			// branch displacement hi
+                nop			// branch displacement lo
+np4:            nop                     // stored PC-4(next insn addr) hi
+                nop                     // stored PC-4(next insn addr) lo
+
+		.global cbz_insn_execbuf_thumb
+		.align 4
+cbz_insn_execbuf_thumb:
+		nop			// cbz
+                push {r0,r1}
+                ldr r0, np5
+                nop
+                str r0, [sp, #4]
+                pop {r0,pc}
+		push {r0,r1}
+                ldr r0, bd5
+                str r0, [sp, #4]
+                pop {r0,pc}
+                nop
+                nop
+                nop                     // ssbreak
+                nop                     // retbreak
+bd5:		nop			// branch displacement hi
+                nop			// branch displacement lo
+np5:            nop                     // stored PC-4(next insn addr) hi
+                nop                     // stored PC-4(next insn addr) lo
diff --git a/uprobe/arch/asm-arm/trampoline_thumb.h b/uprobe/arch/asm-arm/trampoline_thumb.h
index 9bb84a80..8d89428b 100644
--- a/uprobe/arch/asm-arm/trampoline_thumb.h
+++ b/uprobe/arch/asm-arm/trampoline_thumb.h
@@ -3,5 +3,10 @@
 
 void gen_insn_execbuf_thumb(void);
 void pc_dep_insn_execbuf_thumb(void);
+void b_r_insn_execbuf_thumb(void);
+void b_off_insn_execbuf_thumb(void);
+void blx_off_insn_execbuf_thumb(void);
+void b_cond_insn_execbuf_thumb(void);
+void cbz_insn_execbuf_thumb(void);
 
 #endif /* __ASM_ARM_TRAMPOLINE_THUMB_H */
diff --git a/uprobe/swap_uprobes.c b/uprobe/swap_uprobes.c
index e008b759..8884e7e6 100644
--- a/uprobe/swap_uprobes.c
+++ b/uprobe/swap_uprobes.c
@@ -760,11 +760,20 @@ int dbi_register_uretprobe(struct uretprobe *rp)
 
 	DBPRINTF ("START\n");
 
-	rp->up.kp.pre_handler = pre_handler_uretprobe;
+	rp->up.kp.pre_handler = NULL;
 	rp->up.kp.post_handler = NULL;
 	rp->up.kp.fault_handler = NULL;
 	rp->up.kp.break_handler = NULL;
 
+	/* Establish function entry probe point */
+	ret = dbi_register_uprobe(&rp->up);
+	if (ret)
+		return ret;
+
+	ret = arch_opcode_analysis_uretprobe(rp->up.kp.opcode);
+	if (ret)
+		goto unregister;
+
 	/* Pre-allocate memory for max kretprobe instances */
 	if (rp->maxactive <= 0) {
 #if 1//def CONFIG_PREEMPT
@@ -782,7 +791,7 @@ int dbi_register_uretprobe(struct uretprobe *rp)
 		if (inst == NULL) {
 			free_urp_inst(rp);
 			ret = -ENOMEM;
-			goto out;
+			goto unregister;
 		}
 
 		INIT_HLIST_NODE(&inst->uflist);
@@ -790,15 +799,12 @@ int dbi_register_uretprobe(struct uretprobe *rp)
 	}
 
 	rp->nmissed = 0;
+	rp->up.kp.pre_handler = pre_handler_uretprobe;
 
-	/* Establish function entry probe point */
-	ret = dbi_register_uprobe(&rp->up);
-	if (ret) {
-		free_urp_inst(rp);
-		goto out;
-	}
+	return 0;
 
-out:
+unregister:
+	dbi_unregister_uprobe(&rp->up);
 	return ret;
 }