2 files changed, 37 insertions, 3 deletions

diff --git a/arch/powerpc/kernel/entry_64.S b/arch/powerpc/kernel/entry_64.S
index 9f61fd61f277..abfc32330479 100644
--- a/arch/powerpc/kernel/entry_64.S
+++ b/arch/powerpc/kernel/entry_64.S
@@ -971,6 +971,28 @@ _GLOBAL(ftrace_graph_caller)
 
 _GLOBAL(return_to_handler)
         /* need to save return values */
+        std     r4,  -24(r1)
+        std     r3,  -16(r1)
+        std     r31, -8(r1)
+        mr      r31, r1
+        stdu    r1, -112(r1)
+
+        bl      .ftrace_return_to_handler
+        nop
+
+        /* return value has real return address */
+        mtlr    r3
+
+        ld      r1, 0(r1)
+        ld      r4,  -24(r1)
+        ld      r3,  -16(r1)
+        ld      r31, -8(r1)
+
+        /* Jump back to real return address */
+        blr
+
+_GLOBAL(mod_return_to_handler)
+        /* need to save return values */
         std     r4,  -32(r1)
         std     r3,  -24(r1)
         /* save TOC */
@@ -979,7 +1001,10 @@ _GLOBAL(return_to_handler)
         mr      r31, r1
         stdu    r1, -112(r1)
 
-        /* update the TOC */
+        /*
+         * We are in a module using the module's TOC.
+         * Switch to our TOC to run inside the core kernel.
+         */
         LOAD_REG_IMMEDIATE(r4,ftrace_return_to_handler)
         ld      r2, 8(r4)
 
diff --git a/arch/powerpc/kernel/ftrace.c b/arch/powerpc/kernel/ftrace.c
index 7538b944fa52..5c6dfa97e838 100644
--- a/arch/powerpc/kernel/ftrace.c
+++ b/arch/powerpc/kernel/ftrace.c
@@ -567,6 +567,10 @@ int ftrace_disable_ftrace_graph_caller(void)
 }
 #endif /* CONFIG_DYNAMIC_FTRACE */
 
+#ifdef CONFIG_PPC64
+extern void mod_return_to_handler(void);
+#endif
+
 /*
  * Hook the return address and push it in the stack of return addrs
  * in current thread info.
@@ -577,12 +581,17 @@ void prepare_ftrace_return(unsigned long *parent, unsigned long self_addr)
         unsigned long long calltime;
         int faulted;
         struct ftrace_graph_ent trace;
-        unsigned long return_hooker = (unsigned long)
-                                &return_to_handler;
+        unsigned long return_hooker = (unsigned long)&return_to_handler;
 
         if (unlikely(atomic_read(&current->tracing_graph_pause)))
                 return;
 
+#if CONFIG_PPC64
+        /* non core kernel code needs to save and restore the TOC */
+        if (REGION_ID(self_addr) != KERNEL_REGION_ID)
+                return_hooker = (unsigned long)&mod_return_to_handler;
+#endif
+
         return_hooker = GET_ADDR(return_hooker);
 
         /*