1 files changed, 55 insertions, 3 deletions

diff --git a/arch/powerpc/kernel/entry_64.S b/arch/powerpc/kernel/entry_64.S
index 383ed6eb0085..a32699e74c3c 100644
--- a/arch/powerpc/kernel/entry_64.S
+++ b/arch/powerpc/kernel/entry_64.S
@@ -931,13 +931,65 @@ _GLOBAL(_mcount)
         ld      r5,0(r5)
         mtctr   r5
         bctrl
-
         nop
+
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+        b       ftrace_graph_caller
+#endif
         ld      r0, 128(r1)
         mtlr    r0
         addi    r1, r1, 112
 _GLOBAL(ftrace_stub)
         blr
 
-#endif
-#endif
+#endif /* CONFIG_DYNAMIC_FTRACE */
+
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+ftrace_graph_caller:
+        /* load r4 with local address */
+        ld      r4, 128(r1)
+        subi    r4, r4, MCOUNT_INSN_SIZE
+
+        /* get the parent address */
+        ld      r11, 112(r1)
+        addi    r3, r11, 16
+
+        bl      .prepare_ftrace_return
+        nop
+
+        ld      r0, 128(r1)
+        mtlr    r0
+        addi    r1, r1, 112
+        blr
+
+_GLOBAL(return_to_handler)
+        /* need to save return values */
+        std     r4,  -32(r1)
+        std     r3,  -24(r1)
+        /* save TOC */
+        std     r2,  -16(r1)
+        std     r31, -8(r1)
+        mr      r31, r1
+        stdu    r1, -112(r1)
+
+        /* update the TOC */
+        LOAD_REG_IMMEDIATE(r4,ftrace_return_to_handler)
+        ld      r2, 8(r4)
+
+        bl      .ftrace_return_to_handler
+        nop
+
+        /* return value has real return address */
+        mtlr    r3
+
+        ld      r1, 0(r1)
+        ld      r4,  -32(r1)
+        ld      r3,  -24(r1)
+        ld      r2,  -16(r1)
+        ld      r31, -8(r1)
+
+        /* Jump back to real return address */
+        blr
+#endif /* CONFIG_FUNCTION_GRAPH_TRACER */
+#endif /* CONFIG_FUNCTION_TRACER */