1 files changed, 101 insertions, 8 deletions

diff --git a/arch/powerpc/kernel/trace/ftrace_32.S b/arch/powerpc/kernel/trace/ftrace_32.S
index c7d57124cc59..0a02c0cb12d9 100644
--- a/arch/powerpc/kernel/trace/ftrace_32.S
+++ b/arch/powerpc/kernel/trace/ftrace_32.S
@@ -9,6 +9,7 @@
 #include <asm/asm-offsets.h>
 #include <asm/ftrace.h>
 #include <asm/export.h>
+#include <asm/ptrace.h>
 
 _GLOBAL(mcount)
 _GLOBAL(_mcount)
@@ -29,17 +30,21 @@ _GLOBAL(ftrace_caller)
 	MCOUNT_SAVE_FRAME
 	/* r3 ends up with link register */
 	subi	r3, r3, MCOUNT_INSN_SIZE
+	lis	r5,function_trace_op@ha
+	lwz	r5,function_trace_op@l(r5)
+	li	r6, 0
 .globl ftrace_call
 ftrace_call:
 	bl	ftrace_stub
 	nop
+	MCOUNT_RESTORE_FRAME
+ftrace_caller_common:
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
 .globl ftrace_graph_call
 ftrace_graph_call:
 	b	ftrace_graph_stub
 _GLOBAL(ftrace_graph_stub)
 #endif
-	MCOUNT_RESTORE_FRAME
 	/* old link register ends up in ctr reg */
 	bctr
 
@@ -47,16 +52,92 @@ _GLOBAL(ftrace_graph_stub)
 _GLOBAL(ftrace_stub)
 	blr
 
+_GLOBAL(ftrace_regs_caller)
+	/* Save the original return address in A's stack frame */
+	stw	r0,LRSAVE(r1)
+
+	/* Create our stack frame + pt_regs */
+	stwu	r1,-INT_FRAME_SIZE(r1)
+
+	/* Save all gprs to pt_regs */
+	stw	r0, GPR0(r1)
+	stmw	r2, GPR2(r1)
+
+	/* Save previous stack pointer (r1) */
+	addi	r8, r1, INT_FRAME_SIZE
+	stw	r8, GPR1(r1)
+
+	/* Load special regs for save below */
+	mfmsr   r8
+	mfctr   r9
+	mfxer   r10
+	mfcr	r11
+
+	/* Get the _mcount() call site out of LR */
+	mflr	r7
+	/* Save it as pt_regs->nip */
+	stw     r7, _NIP(r1)
+	/* Save the read LR in pt_regs->link */
+	stw     r0, _LINK(r1)
+
+	lis	r3,function_trace_op@ha
+	lwz	r5,function_trace_op@l(r3)
+
+	/* Calculate ip from nip-4 into r3 for call below */
+	subi    r3, r7, MCOUNT_INSN_SIZE
+
+	/* Put the original return address in r4 as parent_ip */
+	mr	r4, r0
+
+	/* Save special regs */
+	stw     r8, _MSR(r1)
+	stw     r9, _CTR(r1)
+	stw     r10, _XER(r1)
+	stw     r11, _CCR(r1)
+
+	/* Load &pt_regs in r6 for call below */
+	addi    r6, r1, STACK_FRAME_OVERHEAD
+
+	/* ftrace_call(r3, r4, r5, r6) */
+.globl ftrace_regs_call
+ftrace_regs_call:
+	bl	ftrace_stub
+	nop
+
+	/* Load ctr with the possibly modified NIP */
+	lwz	r3, _NIP(r1)
+	mtctr	r3
+
+	/* Restore gprs */
+	lmw	r2, GPR2(r1)
+
+	/* Restore possibly modified LR */
+	lwz	r0, _LINK(r1)
+	mtlr	r0
+
+	/* Pop our stack frame */
+	addi r1, r1, INT_FRAME_SIZE
+
+	b	ftrace_caller_common
+
 #ifdef CONFIG_FUNCTION_GRAPH_TRACER
 _GLOBAL(ftrace_graph_caller)
+	stwu	r1,-48(r1)
+	stw	r3, 12(r1)
+	stw	r4, 16(r1)
+	stw	r5, 20(r1)
+	stw	r6, 24(r1)
+	stw	r7, 28(r1)
+	stw	r8, 32(r1)
+	stw	r9, 36(r1)
+	stw	r10,40(r1)
+
 	addi	r5, r1, 48
-	/* load r4 with local address */
-	lwz	r4, 44(r1)
+	mfctr	r4		/* ftrace_caller has moved local addr here */
+	stw	r4, 44(r1)
+	mflr	r3		/* ftrace_caller has restored LR from stack */
 	subi	r4, r4, MCOUNT_INSN_SIZE
 
-	/* Grab the LR out of the caller stack frame */
-	lwz	r3,52(r1)
-
 	bl	prepare_ftrace_return
 	nop
 
@@ -65,9 +146,21 @@ _GLOBAL(ftrace_graph_caller)
          * Change the LR in the callers stack frame to this.
          */
 	stw	r3,52(r1)
+	mtlr	r3
+	lwz	r0,44(r1)
+	mtctr	r0
+
+	lwz	r3, 12(r1)
+	lwz	r4, 16(r1)
+	lwz	r5, 20(r1)
+	lwz	r6, 24(r1)
+	lwz	r7, 28(r1)
+	lwz	r8, 32(r1)
+	lwz	r9, 36(r1)
+	lwz	r10,40(r1)
+
+	addi	r1, r1, 48
 
-	MCOUNT_RESTORE_FRAME
-	/* old link register ends up in ctr reg */
 	bctr
 
 _GLOBAL(return_to_handler)