1 files changed, 86 insertions, 50 deletions

diff --git a/arch/s390/kernel/ftrace.c b/arch/s390/kernel/ftrace.c
index ca1cabb3a96c..b86bb8823f15 100644
--- a/arch/s390/kernel/ftrace.c
+++ b/arch/s390/kernel/ftrace.c
@@ -7,6 +7,7 @@
  *              Martin Schwidefsky <schwidefsky@de.ibm.com>
  */
 
+#include <linux/moduleloader.h>
 #include <linux/hardirq.h>
 #include <linux/uaccess.h>
 #include <linux/ftrace.h>
@@ -15,60 +16,39 @@
 #include <linux/kprobes.h>
 #include <trace/syscall.h>
 #include <asm/asm-offsets.h>
+#include <asm/cacheflush.h>
 #include "entry.h"
 
-void mcount_replace_code(void);
-void ftrace_disable_code(void);
-void ftrace_enable_insn(void);
-
 /*
  * The mcount code looks like this:
  *      stg     %r14,8(%r15)            # offset 0
  *      larl    %r1,<&counter>          # offset 6
  *      brasl   %r14,_mcount            # offset 12
  *      lg      %r14,8(%r15)            # offset 18
- * Total length is 24 bytes. The complete mcount block initially gets replaced
- * by ftrace_make_nop. Subsequent calls to ftrace_make_call / ftrace_make_nop
- * only patch the jg/lg instruction within the block.
- * Note: we do not patch the first instruction to an unconditional branch,
- * since that would break kprobes/jprobes. It is easier to leave the larl
- * instruction in and only modify the second instruction.
+ * Total length is 24 bytes. Only the first instruction will be patched
+ * by ftrace_make_call / ftrace_make_nop.
  * The enabled ftrace code block looks like this:
- *      larl    %r0,.+24                # offset 0
- * >    lg      %r1,__LC_FTRACE_FUNC    # offset 6
- *      br      %r1                     # offset 12
- *      brcl    0,0                     # offset 14
- *      brc     0,0                     # offset 20
+ * >    brasl   %r0,ftrace_caller       # offset 0
+ *      larl    %r1,<&counter>          # offset 6
+ *      brasl   %r14,_mcount            # offset 12
+ *      lg      %r14,8(%r15)            # offset 18
  * The ftrace function gets called with a non-standard C function call ABI
  * where r0 contains the return address. It is also expected that the called
  * function only clobbers r0 and r1, but restores r2-r15.
+ * For module code we can't directly jump to ftrace caller, but need a
+ * trampoline (ftrace_plt), which clobbers also r1.
  * The return point of the ftrace function has offset 24, so execution
  * continues behind the mcount block.
- *      larl    %r0,.+24                # offset 0
- * >    jg      .+18                    # offset 6
- *      br      %r1                     # offset 12
- *      brcl    0,0                     # offset 14
- *      brc     0,0                     # offset 20
+ * The disabled ftrace code block looks like this:
+ * >    jg      .+24                    # offset 0
+ *      larl    %r1,<&counter>          # offset 6
+ *      brasl   %r14,_mcount            # offset 12
+ *      lg      %r14,8(%r15)            # offset 18
  * The jg instruction branches to offset 24 to skip as many instructions
  * as possible.
  */
-asm(
-        "       .align  4\n"
-        "mcount_replace_code:\n"
-        "       larl    %r0,0f\n"
-        "ftrace_disable_code:\n"
-        "       jg      0f\n"
-        "       br      %r1\n"
-        "       brcl    0,0\n"
-        "       brc     0,0\n"
-        "0:\n"
-        "       .align  4\n"
-        "ftrace_enable_insn:\n"
-        "       lg      %r1,"__stringify(__LC_FTRACE_FUNC)"\n");
-
-#define MCOUNT_BLOCK_SIZE       24
-#define MCOUNT_INSN_OFFSET      6
-#define FTRACE_INSN_SIZE        6
+
+unsigned long ftrace_plt;
 
 int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_addr,
                        unsigned long addr)
@@ -79,24 +59,62 @@ int ftrace_modify_call(struct dyn_ftrace *rec, unsigned long old_addr,
 int ftrace_make_nop(struct module *mod, struct dyn_ftrace *rec,
                     unsigned long addr)
 {
-        /* Initial replacement of the whole mcount block */
-        if (addr == MCOUNT_ADDR) {
-                if (probe_kernel_write((void *) rec->ip - MCOUNT_INSN_OFFSET,
-                                       mcount_replace_code,
-                                       MCOUNT_BLOCK_SIZE))
-                        return -EPERM;
-                return 0;
+        struct ftrace_insn insn;
+        unsigned short op;
+        void *from, *to;
+        size_t size;
+
+        ftrace_generate_nop_insn(&insn);
+        size = sizeof(insn);
+        from = &insn;
+        to = (void *) rec->ip;
+        if (probe_kernel_read(&op, (void *) rec->ip, sizeof(op)))
+                return -EFAULT;
+        /*
+         * If we find a breakpoint instruction, a kprobe has been placed
+         * at the beginning of the function. We write the constant
+         * KPROBE_ON_FTRACE_NOP into the remaining four bytes of the original
+         * instruction so that the kprobes handler can execute a nop, if it
+         * reaches this breakpoint.
+         */
+        if (op == BREAKPOINT_INSTRUCTION) {
+                size -= 2;
+                from += 2;
+                to += 2;
+                insn.disp = KPROBE_ON_FTRACE_NOP;
         }
-        if (probe_kernel_write((void *) rec->ip, ftrace_disable_code,
-                               MCOUNT_INSN_SIZE))
+        if (probe_kernel_write(to, from, size))
                 return -EPERM;
         return 0;
 }
 
 int ftrace_make_call(struct dyn_ftrace *rec, unsigned long addr)
 {
-        if (probe_kernel_write((void *) rec->ip, ftrace_enable_insn,
-                               FTRACE_INSN_SIZE))
+        struct ftrace_insn insn;
+        unsigned short op;
+        void *from, *to;
+        size_t size;
+
+        ftrace_generate_call_insn(&insn, rec->ip);
+        size = sizeof(insn);
+        from = &insn;
+        to = (void *) rec->ip;
+        if (probe_kernel_read(&op, (void *) rec->ip, sizeof(op)))
+                return -EFAULT;
+        /*
+         * If we find a breakpoint instruction, a kprobe has been placed
+         * at the beginning of the function. We write the constant
+         * KPROBE_ON_FTRACE_CALL into the remaining four bytes of the original
+         * instruction so that the kprobes handler can execute a brasl if it
+         * reaches this breakpoint.
+         */
+        if (op == BREAKPOINT_INSTRUCTION) {
+                size -= 2;
+                from += 2;
+                to += 2;
+                insn.disp = KPROBE_ON_FTRACE_CALL;
+        }
+        if (probe_kernel_write(to, from, size))
                 return -EPERM;
         return 0;
 }
@@ -111,13 +129,30 @@ int __init ftrace_dyn_arch_init(void)
         return 0;
 }
 
+static int __init ftrace_plt_init(void)
+{
+        unsigned int *ip;
+
+        ftrace_plt = (unsigned long) module_alloc(PAGE_SIZE);
+        if (!ftrace_plt)
+                panic("cannot allocate ftrace plt\n");
+        ip = (unsigned int *) ftrace_plt;
+        ip[0] = 0x0d10e310; /* basr 1,0; lg 1,10(1); br 1 */
+        ip[1] = 0x100a0004;
+        ip[2] = 0x07f10000;
+        ip[3] = FTRACE_ADDR >> 32;
+        ip[4] = FTRACE_ADDR & 0xffffffff;
+        set_memory_ro(ftrace_plt, 1);
+        return 0;
+}
+device_initcall(ftrace_plt_init);
+
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
 /*
  * Hook the return address and push it in the stack of return addresses
  * in current thread info.
  */
-unsigned long __kprobes prepare_ftrace_return(unsigned long parent,
-                                              unsigned long ip)
+unsigned long prepare_ftrace_return(unsigned long parent, unsigned long ip)
 {
         struct ftrace_graph_ent trace;
 
@@ -137,6 +172,7 @@ unsigned long __kprobes prepare_ftrace_return(unsigned long parent,
 out:
         return parent;
 }
+NOKPROBE_SYMBOL(prepare_ftrace_return);
 
 /*
  * Patch the kernel code at ftrace_graph_caller location. The instruction