[IMPROVE] ARM: create kjumper

Change-Id: Ib3ca3a0491175205de07f2c2c02e21d46ed38cea
Signed-off-by: Vyacheslav Cherkashin <v.cherkashin@samsung.com>

8 files changed, 132 insertions, 14 deletions

diff --git a/kprobe/arch/asm-arm/dbi_kprobes.c b/kprobe/arch/asm-arm/dbi_kprobes.c
index f63efca1..c1e80504 100644
--- a/kprobe/arch/asm-arm/dbi_kprobes.c
+++ b/kprobe/arch/asm-arm/dbi_kprobes.c
@@ -321,9 +321,11 @@ int arch_prepare_kprobe(struct kprobe *p, struct slot_manager *sm)
 
 void prepare_singlestep(struct kprobe *p, struct pt_regs *regs)
 {
-	if (p->ss_addr) {
-		regs->ARM_pc = (unsigned long)p->ss_addr;
-		p->ss_addr = NULL;
+	int cpu = smp_processor_id();
+
+	if (p->ss_addr[cpu]) {
+		regs->ARM_pc = (unsigned long)p->ss_addr[cpu];
+		p->ss_addr[cpu] = NULL;
 	} else {
 		regs->ARM_pc = (unsigned long)p->ainsn.insn;
 	}
@@ -431,7 +433,8 @@ int setjmp_pre_handler(struct kprobe *p, struct pt_regs *regs)
 	pre_entry = (kprobe_pre_entry_handler_t)jp->pre_entry;
 
 	if (pre_entry) {
-		p->ss_addr = (void *)pre_entry (jp->priv_arg, regs);
+		p->ss_addr[smp_processor_id()] = (void *)
+						 pre_entry(jp->priv_arg, regs);
 	}
 
 	if (entry) {
@@ -520,6 +523,106 @@ void arch_prepare_kretprobe(struct kretprobe_instance *ri, struct pt_regs *regs)
 
 /*
  ******************************************************************************
+ *                                   kjumper                                  *
+ ******************************************************************************
+ */
+struct kj_cb_data {
+	unsigned long ret_addr;
+
+	struct pt_regs regs;
+
+	jumper_cb_t cb;
+	char data[0];
+};
+
+static struct kj_cb_data * __used kjump_handler(struct kj_cb_data *data)
+{
+	/* call callback */
+	data->cb(data->data);
+
+	return data;
+}
+
+void kjump_trampoline(void);
+__asm(
+	"kjump_trampoline:		\n"
+
+	"mov	r0, r10			\n"
+	"bl	kjump_handler		\n"
+	"nop				\n"	/* for kjump_kprobe */
+);
+
+unsigned long get_kjump_addr(void)
+{
+	return (unsigned long)&kjump_trampoline;
+}
+EXPORT_SYMBOL_GPL(get_kjump_addr);
+
+int set_kjump_cb(unsigned long ret_addr, struct pt_regs *regs,
+		 jumper_cb_t cb, void *data, size_t size)
+{
+	struct kj_cb_data *cb_data;
+
+	cb_data = kmalloc(sizeof(*cb_data) + size, GFP_ATOMIC);
+	if (cb_data == NULL)
+		return -ENOMEM;
+
+	cb_data->ret_addr = ret_addr;
+	cb_data->cb = cb;
+
+	/* save regs */
+	memcpy(&cb_data->regs, regs, sizeof(*regs));
+
+	memcpy(cb_data->data, data, size);
+
+	/* save cb_data to r10 */
+	regs->ARM_r10 = (long)cb_data;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(set_kjump_cb);
+
+static int kjump_pre_handler(struct kprobe *p, struct pt_regs *regs)
+{
+	struct kj_cb_data *data = (struct kj_cb_data *)regs->ARM_r0;
+
+	/* restore regs */
+	memcpy(regs, &data->regs, sizeof(*regs));
+	p->ss_addr[smp_processor_id()] = (void *)data->ret_addr;
+
+	/* FIXME: potential memory leak, when process kill */
+	kfree(data);
+
+	return 0;
+}
+
+static struct kprobe kjump_kprobe = {
+	.pre_handler = kjump_pre_handler,
+	.addr = (unsigned long *)&kjump_trampoline + 2,	/* nop */
+};
+
+static int kjump_init(void)
+{
+	int ret;
+
+	ret = dbi_register_kprobe(&kjump_kprobe);
+	if (ret)
+		printk("ERROR: kjump_init(), ret=%d\n", ret);
+
+	return ret;
+}
+
+static void kjump_exit(void)
+{
+	dbi_unregister_kprobe(&kjump_kprobe);
+}
+
+
+
+
+
+/*
+ ******************************************************************************
  *                                   jumper                                   *
  ******************************************************************************
  */
@@ -618,6 +721,8 @@ static struct undef_hook undef_ho_k = {
 
 int arch_init_kprobes(void)
 {
+	int ret;
+
 	// Register hooks (kprobe_handler)
 	__swap_register_undef_hook = (void *)swap_ksyms("register_undef_hook");
 	if (__swap_register_undef_hook == NULL) {
@@ -634,11 +739,18 @@ int arch_init_kprobes(void)
 
 	swap_register_undef_hook(&undef_ho_k);
 
+	ret = kjump_init();
+	if (ret) {
+		swap_unregister_undef_hook(&undef_ho_k);
+		return ret;
+	}
+
 	return 0;
 }
 
 void arch_exit_kprobes(void)
 {
+	kjump_exit();
 	swap_unregister_undef_hook(&undef_ho_k);
 }
 
diff --git a/kprobe/arch/asm-arm/dbi_kprobes.h b/kprobe/arch/asm-arm/dbi_kprobes.h
index cd7f23bb..efc2679d 100644
--- a/kprobe/arch/asm-arm/dbi_kprobes.h
+++ b/kprobe/arch/asm-arm/dbi_kprobes.h
@@ -533,6 +533,10 @@ static inline unsigned long swap_get_sarg(struct pt_regs *regs, unsigned long n)
 /* jumper */
 typedef unsigned long (*jumper_cb_t)(void *);
 
+unsigned long get_kjump_addr(void);
+int set_kjump_cb(unsigned long ret_addr, struct pt_regs *regs,
+		 jumper_cb_t cb, void *data, size_t size);
+
 unsigned long get_jump_addr(void);
 int set_jump_cb(unsigned long ret_addr, struct pt_regs *regs,
 		jumper_cb_t cb, void *data, size_t size);
diff --git a/kprobe/arch/asm-x86/dbi_kprobes.c b/kprobe/arch/asm-x86/dbi_kprobes.c
index 38ce8b02..20493435 100644
--- a/kprobe/arch/asm-x86/dbi_kprobes.c
+++ b/kprobe/arch/asm-x86/dbi_kprobes.c
@@ -273,10 +273,11 @@ int arch_prepare_kprobe(struct kprobe *p, struct slot_manager *sm)
 
 void prepare_singlestep (struct kprobe *p, struct pt_regs *regs)
 {
-	if(p->ss_addr)
-	{
-		regs->EREG (ip) = (unsigned long)p->ss_addr;
-		p->ss_addr = NULL;
+	int cpu = smp_processor_id();
+
+	if (p->ss_addr[cpu]) {
+		regs->EREG(ip) = (unsigned long)p->ss_addr[cpu];
+		p->ss_addr[cpu] = NULL;
 	}
 	else
 	{
@@ -509,7 +510,8 @@ int setjmp_pre_handler (struct kprobe *p, struct pt_regs *regs)
 	trace_hardirqs_off();
 #endif
 	if (pre_entry)
-		p->ss_addr = (kprobe_opcode_t *)pre_entry(jp->priv_arg, regs);
+		p->ss_addr[smp_processor_id()] = (kprobe_opcode_t *)
+						 pre_entry(jp->priv_arg, regs);
 
 	regs->EREG(ip) = (unsigned long)(jp->entry);
 
diff --git a/kprobe/dbi_kprobes.c b/kprobe/dbi_kprobes.c
index 6a043a5f..9374f05d 100644
--- a/kprobe/dbi_kprobes.c
+++ b/kprobe/dbi_kprobes.c
@@ -378,7 +378,6 @@ static inline void copy_kprobe(struct kprobe *old_p, struct kprobe *p)
 {
 	memcpy(&p->opcode, &old_p->opcode, sizeof(kprobe_opcode_t));
 	memcpy(&p->ainsn, &old_p->ainsn, sizeof(struct arch_specific_insn));
-	p->ss_addr = old_p->ss_addr;
 #ifdef CONFIG_ARM
 	p->safe_arm = old_p->safe_arm;
 	p->safe_thumb = old_p->safe_thumb;
diff --git a/kprobe/dbi_kprobes.h b/kprobe/dbi_kprobes.h
index 9e8b3939..457f786f 100644
--- a/kprobe/dbi_kprobes.h
+++ b/kprobe/dbi_kprobes.h
@@ -132,7 +132,7 @@ struct kprobe
 	// if jprobe.entry should return address of function or NULL
 	// if original function should be called
 	// not supported for X86, not tested for MIPS
-	kprobe_opcode_t					*ss_addr;
+	kprobe_opcode_t					*ss_addr[NR_CPUS];
 	// safe/unsafe to use probe
 #ifdef CONFIG_ARM
 	unsigned					safe_arm:1;
diff --git a/uprobe/arch/asm-arm/swap_uprobes.c b/uprobe/arch/asm-arm/swap_uprobes.c
index e46b4cc2..41748771 100644
--- a/uprobe/arch/asm-arm/swap_uprobes.c
+++ b/uprobe/arch/asm-arm/swap_uprobes.c
@@ -687,7 +687,8 @@ int setjmp_upre_handler(struct kprobe *p, struct pt_regs *regs)
 	entry_point_t entry = (entry_point_t)jp->entry;
 
 	if (pre_entry) {
-		p->ss_addr = (kprobe_opcode_t *)pre_entry(jp->priv_arg, regs);
+		p->ss_addr[smp_processor_id()] = (kprobe_opcode_t *)
+						 pre_entry(jp->priv_arg, regs);
 	}
 
 	if (entry) {
diff --git a/uprobe/arch/asm-x86/swap_uprobes.c b/uprobe/arch/asm-x86/swap_uprobes.c
index 750f32db..e398dfc1 100644
--- a/uprobe/arch/asm-x86/swap_uprobes.c
+++ b/uprobe/arch/asm-x86/swap_uprobes.c
@@ -115,7 +115,8 @@ int setjmp_upre_handler(struct kprobe *p, struct pt_regs *regs)
 		panic("failed to read user space func arguments %lx!\n", regs->EREG(sp) + 4);
 
 	if (pre_entry)
-		p->ss_addr = (kprobe_opcode_t *)pre_entry(jp->priv_arg, regs);
+		p->ss_addr[smp_processor_id()] = (kprobe_opcode_t *)
+						 pre_entry(jp->priv_arg, regs);
 
 	if (entry)
 		entry(args[0], args[1], args[2], args[3], args[4], args[5]);
diff --git a/uprobe/swap_uprobes.c b/uprobe/swap_uprobes.c
index e9257a45..42185d08 100644
--- a/uprobe/swap_uprobes.c
+++ b/uprobe/swap_uprobes.c
@@ -109,7 +109,6 @@ static inline void copy_uprobe(struct kprobe *old_p, struct kprobe *p)
 {
 	memcpy(&p->opcode, &old_p->opcode, sizeof(kprobe_opcode_t));
 	memcpy(&p->ainsn, &old_p->ainsn, sizeof(struct arch_specific_insn));
-	p->ss_addr = old_p->ss_addr;
 #ifdef CONFIG_ARM
 	p->safe_arm = old_p->safe_arm;
 	p->safe_thumb = old_p->safe_thumb;