ARM: 7331/1: extract out insn generation code from ftrace

[platform/adaptation/renesas_rcar/renesas_kernel.git] / arch / arm / kernel / ftrace.c

diff --git a/arch/arm/kernel/ftrace.c b/arch/arm/kernel/ftrace.c
index c0062ad..df0bf0c 100644 (file)
--- a/arch/arm/kernel/ftrace.c
+++ b/arch/arm/kernel/ftrace.c
@@ -16,10 +16,13 @@
 #include <linux/uaccess.h>
 
 #include <asm/cacheflush.h>
+#include <asm/opcodes.h>
 #include <asm/ftrace.h>
 
+#include "insn.h"
+
 #ifdef CONFIG_THUMB2_KERNEL
-#define        NOP             0xeb04f85d      /* pop.w {lr} */
+#define        NOP             0xf85deb04      /* pop.w {lr} */
 #else
 #define        NOP             0xe8bd4000      /* pop {lr} */
 #endif
@@ -60,76 +63,31 @@ static unsigned long adjust_address(struct dyn_ftrace *rec, unsigned long addr)
 }
 #endif
 
-#ifdef CONFIG_THUMB2_KERNEL
-static unsigned long ftrace_gen_branch(unsigned long pc, unsigned long addr,
-                                      bool link)
-{
-       unsigned long s, j1, j2, i1, i2, imm10, imm11;
-       unsigned long first, second;
-       long offset;
-
-       offset = (long)addr - (long)(pc + 4);
-       if (offset < -16777216 || offset > 16777214) {
-               WARN_ON_ONCE(1);
-               return 0;
-       }
-
-       s       = (offset >> 24) & 0x1;
-       i1      = (offset >> 23) & 0x1;
-       i2      = (offset >> 22) & 0x1;
-       imm10   = (offset >> 12) & 0x3ff;
-       imm11   = (offset >>  1) & 0x7ff;
-
-       j1 = (!i1) ^ s;
-       j2 = (!i2) ^ s;
-
-       first = 0xf000 | (s << 10) | imm10;
-       second = 0x9000 | (j1 << 13) | (j2 << 11) | imm11;
-       if (link)
-               second |= 1 << 14;
-
-       return (second << 16) | first;
-}
-#else
-static unsigned long ftrace_gen_branch(unsigned long pc, unsigned long addr,
-                                      bool link)
-{
-       unsigned long opcode = 0xea000000;
-       long offset;
-
-       if (link)
-               opcode |= 1 << 24;
-
-       offset = (long)addr - (long)(pc + 8);
-       if (unlikely(offset < -33554432 || offset > 33554428)) {
-               /* Can't generate branches that far (from ARM ARM). Ftrace
-                * doesn't generate branches outside of kernel text.
-                */
-               WARN_ON_ONCE(1);
-               return 0;
-       }
-
-       offset = (offset >> 2) & 0x00ffffff;
-
-       return opcode | offset;
-}
-#endif
-
 static unsigned long ftrace_call_replace(unsigned long pc, unsigned long addr)
 {
-       return ftrace_gen_branch(pc, addr, true);
+       return arm_gen_branch_link(pc, addr);
 }
 
 static int ftrace_modify_code(unsigned long pc, unsigned long old,
-                             unsigned long new)
+                             unsigned long new, bool validate)
 {
        unsigned long replaced;
 
-       if (probe_kernel_read(&replaced, (void *)pc, MCOUNT_INSN_SIZE))
-               return -EFAULT;
+       if (IS_ENABLED(CONFIG_THUMB2_KERNEL)) {
+               old = __opcode_to_mem_thumb32(old);
+               new = __opcode_to_mem_thumb32(new);
+       } else {
+               old = __opcode_to_mem_arm(old);
+               new = __opcode_to_mem_arm(new);
+       }
 
-       if (replaced != old)
-               return -EINVAL;
+       if (validate) {
+               if (probe_kernel_read(&replaced, (void *)pc, MCOUNT_INSN_SIZE))
+                       return -EFAULT;
+
+               if (replaced != old)
+                       return -EINVAL;
+       }
 
        if (probe_kernel_write((void *)pc, &new, MCOUNT_INSN_SIZE))
                return -EPERM;
@@ -141,23 +99,21 @@ static int ftrace_modify_code(unsigned long pc, unsigned long old,
 
 int ftrace_update_ftrace_func(ftrace_func_t func)
 {
-       unsigned long pc, old;
+       unsigned long pc;
        unsigned long new;
        int ret;
 
        pc = (unsigned long)&ftrace_call;
-       memcpy(&old, &ftrace_call, MCOUNT_INSN_SIZE);
        new = ftrace_call_replace(pc, (unsigned long)func);
 
-       ret = ftrace_modify_code(pc, old, new);
+       ret = ftrace_modify_code(pc, 0, new, false);
 
 #ifdef CONFIG_OLD_MCOUNT
        if (!ret) {
                pc = (unsigned long)&ftrace_call_old;
-               memcpy(&old, &ftrace_call_old, MCOUNT_INSN_SIZE);
                new = ftrace_call_replace(pc, (unsigned long)func);
 
-               ret = ftrace_modify_code(pc, old, new);
+               ret = ftrace_modify_code(pc, 0, new, false);
        }
 #endif
 
@@ -172,7 +128,7 @@ int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr)
        old = ftrace_nop_replace(rec);
        new = ftrace_call_replace(ip, adjust_address(rec, addr));
 
-       return ftrace_modify_code(rec->ip, old, new);
+       return ftrace_modify_code(rec->ip, old, new, true);
 }
 
 int ftrace_make_nop(struct module *mod,
@@ -185,7 +141,7 @@ int ftrace_make_nop(struct module *mod,
 
        old = ftrace_call_replace(ip, adjust_address(rec, addr));
        new = ftrace_nop_replace(rec);
-       ret = ftrace_modify_code(ip, old, new);
+       ret = ftrace_modify_code(ip, old, new, true);
 
 #ifdef CONFIG_OLD_MCOUNT
        if (ret == -EINVAL && addr == MCOUNT_ADDR) {
@@ -193,7 +149,7 @@ int ftrace_make_nop(struct module *mod,
 
                old = ftrace_call_replace(ip, adjust_address(rec, addr));
                new = ftrace_nop_replace(rec);
-               ret = ftrace_modify_code(ip, old, new);
+               ret = ftrace_modify_code(ip, old, new, true);
        }
 #endif
 
@@ -249,12 +205,12 @@ static int __ftrace_modify_caller(unsigned long *callsite,
 {
        unsigned long caller_fn = (unsigned long) func;
        unsigned long pc = (unsigned long) callsite;
-       unsigned long branch = ftrace_gen_branch(pc, caller_fn, false);
+       unsigned long branch = arm_gen_branch(pc, caller_fn);
        unsigned long nop = 0xe1a00000; /* mov r0, r0 */
        unsigned long old = enable ? nop : branch;
        unsigned long new = enable ? branch : nop;
 
-       return ftrace_modify_code(pc, old, new);
+       return ftrace_modify_code(pc, old, new, true);
 }
 
 static int ftrace_modify_graph_caller(bool enable)