[IMPROVE] B.W instrumentation for THUMB2

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

diff --git a/uprobe/arch/arm/swap-asm/decode_thumb.c b/uprobe/arch/arm/swap-asm/decode_thumb.c
index 3b1f671..9e35adb 100644 (file)
--- a/uprobe/arch/arm/swap-asm/decode_thumb.c
+++ b/uprobe/arch/arm/swap-asm/decode_thumb.c
@@ -22,6 +22,7 @@
 
 #include <linux/types.h>
 #include <linux/errno.h>
+#include <linux/ptrace.h>
 #include "decode_thumb.h"
 #include "thumb_tramps.h"
 
@@ -113,6 +114,69 @@ static int t32_b1110_100(thumb_insn_t insn, struct decode_info *info)
        return thumb_not_implement(insn, info);
 }
 
+static void t32_simulate_branch(unsigned long insn,
+                               struct arch_insn *ainsn,
+                               struct pt_regs *regs)
+{
+       unsigned long pc = regs->ARM_pc;
+       thumb_insn_t i = { .val = insn };
+
+       long offset = GET_FIELD(i.hw2, 0, 11);          /* imm11 */
+       offset += GET_FIELD(i.hw1, 0, 10) << 11;        /* imm10 */
+       offset += GET_BIT(i.hw2, 13) << 21;             /* J1 */
+       offset += GET_BIT(i.hw2, 11) << 22;             /* J2 */
+
+       /* check S bit */
+       if (GET_BIT(i.hw1, 10))
+               offset -= 0x00800000;   /* Apply sign bit */
+       else
+               offset ^= 0x00600000;   /* Invert J1 and J2 */
+
+       /* check link */
+       if (GET_BIT(i.hw2, 14)) {
+               /* BL or BLX */
+               regs->ARM_lr = regs->ARM_pc | 1;
+               if (!GET_BIT(i.hw2, 12)) {
+                       /* BLX so switch to ARM mode */
+                       regs->ARM_cpsr &= ~PSR_T_BIT;
+                       pc &= ~3;
+               }
+       }
+
+       regs->ARM_pc = pc + (offset * 2);
+}
+
+static int t32_branch(thumb_insn_t insn, struct decode_info *info)
+{
+       info->handeler = t32_simulate_branch;
+
+       return 0;
+}
+
+static decode_handler_t table_branches[8] = {
+       /* hw2[14 12 0] */
+       /* Bc   0  0 0  */      thumb_not_implement,
+       /* Bc   0  0 1  */      thumb_not_implement,
+       /* B    0  1 0  */      t32_branch,
+       /* B    0  1 1  */      t32_branch,
+       /* BLX  1  0 0  */      t32_branch,
+       /* res  1  0 1  */      thumb_unpredictable,
+       /* BL   1  1 0  */      t32_branch,
+       /* BL   1  1 1  */      t32_branch,
+};
+
+
+
+static int t32_b1111_0xxx_xxxx_xxxx_1(thumb_insn_t insn,
+                                     struct decode_info *info)
+{
+       unsigned long s = GET_BIT(insn.hw2, 14) << 2 |
+                         GET_BIT(insn.hw2, 12) << 1 |
+                         GET_BIT(insn.hw2, 0);
+
+       return table_branches[s](insn, info);
+}
+
 static int b111(thumb_insn_t insn, struct decode_info *info)
 {
        /* hw1[111x xxx? ???? ????] */
@@ -121,6 +185,10 @@ static int b111(thumb_insn_t insn, struct decode_info *info)
                return t32_b1110_100(insn, info);
        }
 
+       /* [1111 0xxx xxxx xxxx 1xxx xxxx xxxx xxxx] */
+       if (GET_FIELD(insn.hw1, 11, 2) == 0b10 && GET_BIT(insn.hw2, 15) == 1)
+               return t32_b1111_0xxx_xxxx_xxxx_1(insn, info);
+
        return thumb_not_implement(insn, info);
 }
 

diff --git a/uprobe/arch/arm/swap-asm/decode_thumb.h b/uprobe/arch/arm/swap-asm/decode_thumb.h
index b392417..8c1142a 100644 (file)
--- a/uprobe/arch/arm/swap-asm/decode_thumb.h
+++ b/uprobe/arch/arm/swap-asm/decode_thumb.h
@@ -24,9 +24,13 @@
 #define _ARM_DECODE_THUMB_H
 
 
+#include "swap_uprobes.h"
+
+
 struct decode_info {
        unsigned long vaddr;
        void *tramp;
+       uprobe_handler_t handeler;
 };
 
 

diff --git a/uprobe/arch/arm/swap-asm/swap_uprobes.c b/uprobe/arch/arm/swap-asm/swap_uprobes.c
index 857c447..ccd7e3c 100644 (file)
--- a/uprobe/arch/arm/swap-asm/swap_uprobes.c
+++ b/uprobe/arch/arm/swap-asm/swap_uprobes.c
@@ -609,9 +609,15 @@ int arch_prepare_uprobe(struct uprobe *p)
                        struct decode_info info = {
                                .vaddr = vaddr,
                                .tramp = tramp,
+                               .handeler = NULL,
                        };
 
                        ret = decode_thumb(insn, &info);
+                       if (info.handeler) {
+                               unsigned short *tr = (unsigned short *)tramp;
+                               tr[13] = 0xdeff; /* breakpoint for uretprobe */
+                               p->ainsn.handler = info.handeler;
+                       }
                }
        } else {
                ret = arch_make_trampoline_arm(vaddr, insn, tramp);
@@ -938,7 +944,12 @@ static void arch_prepare_singlestep(struct uprobe *p, struct pt_regs *regs)
                regs->ARM_pc = (unsigned long)p->ss_addr[cpu];
                p->ss_addr[cpu] = NULL;
        } else {
-               regs->ARM_pc = (unsigned long)p->ainsn.insn;
+               if (p->ainsn.handler) {
+                       regs->ARM_pc += 4;
+                       p->ainsn.handler(p->opcode, &p->ainsn, regs);
+               } else {
+                       regs->ARM_pc = (unsigned long)p->ainsn.insn;
+               }
        }
 }
 

diff --git a/uprobe/arch/arm/swap-asm/swap_uprobes.h b/uprobe/arch/arm/swap-asm/swap_uprobes.h
index 4065285..fdf7fde 100644 (file)
--- a/uprobe/arch/arm/swap-asm/swap_uprobes.h
+++ b/uprobe/arch/arm/swap-asm/swap_uprobes.h
@@ -41,10 +41,15 @@
 
 struct task_struct;
 struct uprobe;
+struct arch_insn;
 struct uretprobe;
 struct uretprobe_instance;
 
 typedef unsigned long uprobe_opcode_t;
+typedef void (*uprobe_handler_t)(unsigned long insn,
+                                struct arch_insn *, struct pt_regs *);
+
+
 
 /**
  * @struct arch_insn
@@ -54,6 +59,7 @@ typedef unsigned long uprobe_opcode_t;
  */
 struct arch_insn {
        uprobe_opcode_t *insn;
+       uprobe_handler_t handler;
 };