1 files changed, 69 insertions, 29 deletions

diff --git a/arch/x86/kernel/entry_64.S b/arch/x86/kernel/entry_64.S
index 3194636a4293..e28c7a987793 100644
--- a/arch/x86/kernel/entry_64.S
+++ b/arch/x86/kernel/entry_64.S
@@ -67,16 +67,10 @@ ENTRY(mcount)
 END(mcount)
 
 ENTRY(ftrace_caller)
+        cmpl $0, function_trace_stop
+        jne  ftrace_stub
 
-        /* taken from glibc */
-        subq $0x38, %rsp
-        movq %rax, (%rsp)
-        movq %rcx, 8(%rsp)
-        movq %rdx, 16(%rsp)
-        movq %rsi, 24(%rsp)
-        movq %rdi, 32(%rsp)
-        movq %r8, 40(%rsp)
-        movq %r9, 48(%rsp)
+        MCOUNT_SAVE_FRAME
 
         movq 0x38(%rsp), %rdi
         movq 8(%rbp), %rsi
@@ -86,14 +80,13 @@ ENTRY(ftrace_caller)
 ftrace_call:
         call ftrace_stub
 
-        movq 48(%rsp), %r9
-        movq 40(%rsp), %r8
-        movq 32(%rsp), %rdi
-        movq 24(%rsp), %rsi
-        movq 16(%rsp), %rdx
-        movq 8(%rsp), %rcx
-        movq (%rsp), %rax
-        addq $0x38, %rsp
+        MCOUNT_RESTORE_FRAME
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+.globl ftrace_graph_call
+ftrace_graph_call:
+        jmp ftrace_stub
+#endif
 
 .globl ftrace_stub
 ftrace_stub:
@@ -102,15 +95,63 @@ END(ftrace_caller)
 
 #else /* ! CONFIG_DYNAMIC_FTRACE */
 ENTRY(mcount)
+        cmpl $0, function_trace_stop
+        jne  ftrace_stub
+
         cmpq $ftrace_stub, ftrace_trace_function
         jnz trace
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+        cmpq $ftrace_stub, ftrace_graph_return
+        jnz ftrace_graph_caller
+
+        cmpq $ftrace_graph_entry_stub, ftrace_graph_entry
+        jnz ftrace_graph_caller
+#endif
+
 .globl ftrace_stub
 ftrace_stub:
         retq
 
 trace:
-        /* taken from glibc */
-        subq $0x38, %rsp
+        MCOUNT_SAVE_FRAME
+
+        movq 0x38(%rsp), %rdi
+        movq 8(%rbp), %rsi
+        subq $MCOUNT_INSN_SIZE, %rdi
+
+        call   *ftrace_trace_function
+
+        MCOUNT_RESTORE_FRAME
+
+        jmp ftrace_stub
+END(mcount)
+#endif /* CONFIG_DYNAMIC_FTRACE */
+#endif /* CONFIG_FUNCTION_TRACER */
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+ENTRY(ftrace_graph_caller)
+        cmpl $0, function_trace_stop
+        jne ftrace_stub
+
+        MCOUNT_SAVE_FRAME
+
+        leaq 8(%rbp), %rdi
+        movq 0x38(%rsp), %rsi
+        subq $MCOUNT_INSN_SIZE, %rsi
+
+        call    prepare_ftrace_return
+
+        MCOUNT_RESTORE_FRAME
+
+        retq
+END(ftrace_graph_caller)
+
+
+.globl return_to_handler
+return_to_handler:
+        subq  $80, %rsp
+
         movq %rax, (%rsp)
         movq %rcx, 8(%rsp)
         movq %rdx, 16(%rsp)
@@ -118,13 +159,14 @@ trace:
         movq %rdi, 32(%rsp)
         movq %r8, 40(%rsp)
         movq %r9, 48(%rsp)
+        movq %r10, 56(%rsp)
+        movq %r11, 64(%rsp)
 
-        movq 0x38(%rsp), %rdi
-        movq 8(%rbp), %rsi
-        subq $MCOUNT_INSN_SIZE, %rdi
-
-        call   *ftrace_trace_function
+        call ftrace_return_to_handler
 
+        movq %rax, 72(%rsp)
+        movq 64(%rsp), %r11
+        movq 56(%rsp), %r10
         movq 48(%rsp), %r9
         movq 40(%rsp), %r8
         movq 32(%rsp), %rdi
@@ -132,12 +174,10 @@ trace:
         movq 16(%rsp), %rdx
         movq 8(%rsp), %rcx
         movq (%rsp), %rax
-        addq $0x38, %rsp
+        addq $72, %rsp
+        retq
+#endif
 
-        jmp ftrace_stub
-END(mcount)
-#endif /* CONFIG_DYNAMIC_FTRACE */
-#endif /* CONFIG_FUNCTION_TRACER */
 
 #ifndef CONFIG_PREEMPT
 #define retint_kernel retint_restore_args