powerpc: Fix page fault with lockdep regression
[linux-2.6.git] / arch / powerpc / kernel / entry_32.S
index 7ecc0d1..ba3aeb4 100644 (file)
@@ -31,6 +31,7 @@
 #include <asm/asm-offsets.h>
 #include <asm/unistd.h>
 #include <asm/ftrace.h>
+#include <asm/ptrace.h>
 
 #undef SHOW_SYSCALLS
 #undef SHOW_SYSCALLS_TASK
@@ -63,7 +64,7 @@ debug_transfer_to_handler:
 
        .globl  crit_transfer_to_handler
 crit_transfer_to_handler:
-#ifdef CONFIG_FSL_BOOKE
+#ifdef CONFIG_PPC_BOOK3E_MMU
        mfspr   r0,SPRN_MAS0
        stw     r0,MAS0(r11)
        mfspr   r0,SPRN_MAS1
@@ -78,7 +79,7 @@ crit_transfer_to_handler:
        mfspr   r0,SPRN_MAS7
        stw     r0,MAS7(r11)
 #endif /* CONFIG_PHYS_64BIT */
-#endif /* CONFIG_FSL_BOOKE */
+#endif /* CONFIG_PPC_BOOK3E_MMU */
 #ifdef CONFIG_44x
        mfspr   r0,SPRN_MMUCR
        stw     r0,MMUCR(r11)
@@ -88,7 +89,7 @@ crit_transfer_to_handler:
        mfspr   r0,SPRN_SRR1
        stw     r0,_SRR1(r11)
 
-       mfspr   r8,SPRN_SPRG3
+       mfspr   r8,SPRN_SPRG_THREAD
        lwz     r0,KSP_LIMIT(r8)
        stw     r0,SAVED_KSP_LIMIT(r11)
        rlwimi  r0,r1,0,0,(31-THREAD_SHIFT)
@@ -108,7 +109,7 @@ crit_transfer_to_handler:
        mfspr   r0,SPRN_SRR1
        stw     r0,crit_srr1@l(0)
 
-       mfspr   r8,SPRN_SPRG3
+       mfspr   r8,SPRN_SPRG_THREAD
        lwz     r0,KSP_LIMIT(r8)
        stw     r0,saved_ksp_limit@l(0)
        rlwimi  r0,r1,0,0,(31-THREAD_SHIFT)
@@ -138,7 +139,7 @@ transfer_to_handler:
        mfspr   r2,SPRN_XER
        stw     r12,_CTR(r11)
        stw     r2,_XER(r11)
-       mfspr   r12,SPRN_SPRG3
+       mfspr   r12,SPRN_SPRG_THREAD
        addi    r2,r12,-THREAD
        tovirt(r2,r2)                   /* set r2 to current */
        beq     2f                      /* if from user, fix up THREAD.regs */
@@ -191,11 +192,67 @@ transfer_to_handler_cont:
        mflr    r9
        lwz     r11,0(r9)               /* virtual address of handler */
        lwz     r9,4(r9)                /* where to go when done */
+#ifdef CONFIG_TRACE_IRQFLAGS
+       lis     r12,reenable_mmu@h
+       ori     r12,r12,reenable_mmu@l
+       mtspr   SPRN_SRR0,r12
+       mtspr   SPRN_SRR1,r10
+       SYNC
+       RFI
+reenable_mmu:                          /* re-enable mmu so we can */
+       mfmsr   r10
+       lwz     r12,_MSR(r1)
+       xor     r10,r10,r12
+       andi.   r10,r10,MSR_EE          /* Did EE change? */
+       beq     1f
+
+       /*
+        * The trace_hardirqs_off will use CALLER_ADDR0 and CALLER_ADDR1.
+        * If from user mode there is only one stack frame on the stack, and
+        * accessing CALLER_ADDR1 will cause oops. So we need create a dummy
+        * stack frame to make trace_hardirqs_off happy.
+        *
+        * This is handy because we also need to save a bunch of GPRs,
+        * r3 can be different from GPR3(r1) at this point, r9 and r11
+        * contains the old MSR and handler address respectively,
+        * r4 & r5 can contain page fault arguments that need to be passed
+        * along as well. r12, CCR, CTR, XER etc... are left clobbered as
+        * they aren't useful past this point (aren't syscall arguments),
+        * the rest is restored from the exception frame.
+        */
+       stwu    r1,-32(r1)
+       stw     r9,8(r1)
+       stw     r11,12(r1)
+       stw     r3,16(r1)
+       stw     r4,20(r1)
+       stw     r5,24(r1)
+       andi.   r12,r12,MSR_PR
+       b       11f
+       bl      trace_hardirqs_off
+       b       12f
+11:
+       bl      trace_hardirqs_off
+12:
+       lwz     r5,24(r1)
+       lwz     r4,20(r1)
+       lwz     r3,16(r1)
+       lwz     r11,12(r1)
+       lwz     r9,8(r1)
+       addi    r1,r1,32
+       lwz     r0,GPR0(r1)
+       lwz     r6,GPR6(r1)
+       lwz     r7,GPR7(r1)
+       lwz     r8,GPR8(r1)
+1:     mtctr   r11
+       mtlr    r9
+       bctr                            /* jump to handler */
+#else /* CONFIG_TRACE_IRQFLAGS */
        mtspr   SPRN_SRR0,r11
        mtspr   SPRN_SRR1,r10
        mtlr    r9
        SYNC
        RFI                             /* jump to handler, enable MMU */
+#endif /* CONFIG_TRACE_IRQFLAGS */
 
 #if defined (CONFIG_6xx) || defined(CONFIG_E500)
 4:     rlwinm  r12,r12,0,~_TLF_NAPPING
@@ -251,6 +308,31 @@ _GLOBAL(DoSyscall)
 #ifdef SHOW_SYSCALLS
        bl      do_show_syscall
 #endif /* SHOW_SYSCALLS */
+#ifdef CONFIG_TRACE_IRQFLAGS
+       /* Return from syscalls can (and generally will) hard enable
+        * interrupts. You aren't supposed to call a syscall with
+        * interrupts disabled in the first place. However, to ensure
+        * that we get it right vs. lockdep if it happens, we force
+        * that hard enable here with appropriate tracing if we see
+        * that we have been called with interrupts off
+        */
+       mfmsr   r11
+       andi.   r12,r11,MSR_EE
+       bne+    1f
+       /* We came in with interrupts disabled, we enable them now */
+       bl      trace_hardirqs_on
+       mfmsr   r11
+       lwz     r0,GPR0(r1)
+       lwz     r3,GPR3(r1)
+       lwz     r4,GPR4(r1)
+       ori     r11,r11,MSR_EE
+       lwz     r5,GPR5(r1)
+       lwz     r6,GPR6(r1)
+       lwz     r7,GPR7(r1)
+       lwz     r8,GPR8(r1)
+       mtmsr   r11
+1:
+#endif /* CONFIG_TRACE_IRQFLAGS */
        rlwinm  r10,r1,0,0,(31-THREAD_SHIFT)    /* current_thread_info() */
        lwz     r11,TI_FLAGS(r10)
        andi.   r11,r11,_TIF_SYSCALL_T_OR_A
@@ -275,6 +357,7 @@ ret_from_syscall:
        rlwinm  r12,r1,0,0,(31-THREAD_SHIFT)    /* current_thread_info() */
        /* disable interrupts so current_thread_info()->flags can't change */
        LOAD_MSR_KERNEL(r10,MSR_KERNEL) /* doesn't include MSR_EE */
+       /* Note: We don't bother telling lockdep about it */
        SYNC
        MTMSRD(r10)
        lwz     r9,TI_FLAGS(r12)
@@ -288,6 +371,19 @@ ret_from_syscall:
        oris    r11,r11,0x1000  /* Set SO bit in CR */
        stw     r11,_CCR(r1)
 syscall_exit_cont:
+       lwz     r8,_MSR(r1)
+#ifdef CONFIG_TRACE_IRQFLAGS
+       /* If we are going to return from the syscall with interrupts
+        * off, we trace that here. It shouldn't happen though but we
+        * want to catch the bugger if it does right ?
+        */
+       andi.   r10,r8,MSR_EE
+       bne+    1f
+       stw     r3,GPR3(r1)
+       bl      trace_hardirqs_off
+       lwz     r3,GPR3(r1)
+1:
+#endif /* CONFIG_TRACE_IRQFLAGS */
 #if defined(CONFIG_4xx) || defined(CONFIG_BOOKE)
        /* If the process has its own DBCR0 value, load it up.  The internal
           debug mode bit tells us that dbcr0 should be loaded. */
@@ -296,11 +392,13 @@ syscall_exit_cont:
        bnel-   load_dbcr0
 #endif
 #ifdef CONFIG_44x
+BEGIN_MMU_FTR_SECTION
        lis     r4,icache_44x_need_flush@ha
        lwz     r5,icache_44x_need_flush@l(r4)
        cmplwi  cr0,r5,0
        bne-    2f
 1:
+END_MMU_FTR_SECTION_IFCLR(MMU_FTR_TYPE_47x)
 #endif /* CONFIG_44x */
 BEGIN_FTR_SECTION
        lwarx   r7,0,r1
@@ -311,7 +409,6 @@ END_FTR_SECTION_IFSET(CPU_FTR_NEED_PAIRED_STWCX)
        mtlr    r4
        mtcr    r5
        lwz     r7,_NIP(r1)
-       lwz     r8,_MSR(r1)
        FIX_SRR1(r8, r0)
        lwz     r2,GPR2(r1)
        lwz     r1,GPR1(r1)
@@ -394,7 +491,9 @@ syscall_exit_work:
        andi.   r0,r9,(_TIF_SYSCALL_T_OR_A|_TIF_SINGLESTEP)
        beq     ret_from_except
 
-       /* Re-enable interrupts */
+       /* Re-enable interrupts. There is no need to trace that with
+        * lockdep as we are supposed to have IRQs on at this point
+        */
        ori     r10,r10,MSR_EE
        SYNC
        MTMSRD(r10)
@@ -602,7 +701,7 @@ END_FTR_SECTION_IFSET(CPU_FTR_SPE)
 
        tophys(r0,r4)
        CLR_TOP32(r0)
-       mtspr   SPRN_SPRG3,r0   /* Update current THREAD phys addr */
+       mtspr   SPRN_SPRG_THREAD,r0     /* Update current THREAD phys addr */
        lwz     r1,KSP(r4)      /* Load new stack pointer */
 
        /* save the old current 'last' for return value */
@@ -705,6 +804,7 @@ ret_from_except:
        /* Hard-disable interrupts so that current_thread_info()->flags
         * can't change between when we test it and when we return
         * from the interrupt. */
+       /* Note: We don't bother telling lockdep about it */
        LOAD_MSR_KERNEL(r10,MSR_KERNEL)
        SYNC                    /* Some chip revs have problems here... */
        MTMSRD(r10)             /* disable interrupts */
@@ -744,11 +844,24 @@ resume_kernel:
        beq+    restore
        andi.   r0,r3,MSR_EE    /* interrupts off? */
        beq     restore         /* don't schedule if so */
+#ifdef CONFIG_TRACE_IRQFLAGS
+       /* Lockdep thinks irqs are enabled, we need to call
+        * preempt_schedule_irq with IRQs off, so we inform lockdep
+        * now that we -did- turn them off already
+        */
+       bl      trace_hardirqs_off
+#endif
 1:     bl      preempt_schedule_irq
        rlwinm  r9,r1,0,0,(31-THREAD_SHIFT)
        lwz     r3,TI_FLAGS(r9)
        andi.   r0,r3,_TIF_NEED_RESCHED
        bne-    1b
+#ifdef CONFIG_TRACE_IRQFLAGS
+       /* And now, to properly rebalance the above, we tell lockdep they
+        * are being turned back on, which will happen when we return
+        */
+       bl      trace_hardirqs_on
+#endif
 #else
 resume_kernel:
 #endif /* CONFIG_PREEMPT */
@@ -756,6 +869,9 @@ resume_kernel:
        /* interrupts are hard-disabled at this point */
 restore:
 #ifdef CONFIG_44x
+BEGIN_MMU_FTR_SECTION
+       b       1f
+END_MMU_FTR_SECTION_IFSET(MMU_FTR_TYPE_47x)
        lis     r4,icache_44x_need_flush@ha
        lwz     r5,icache_44x_need_flush@l(r4)
        cmplwi  cr0,r5,0
@@ -765,6 +881,39 @@ restore:
        stw     r6,icache_44x_need_flush@l(r4)
 1:
 #endif  /* CONFIG_44x */
+
+       lwz     r9,_MSR(r1)
+#ifdef CONFIG_TRACE_IRQFLAGS
+       /* Lockdep doesn't know about the fact that IRQs are temporarily turned
+        * off in this assembly code while peeking at TI_FLAGS() and such. However
+        * we need to inform it if the exception turned interrupts off, and we
+        * are about to trun them back on.
+        *
+        * The problem here sadly is that we don't know whether the exceptions was
+        * one that turned interrupts off or not. So we always tell lockdep about
+        * turning them on here when we go back to wherever we came from with EE
+        * on, even if that may meen some redudant calls being tracked. Maybe later
+        * we could encode what the exception did somewhere or test the exception
+        * type in the pt_regs but that sounds overkill
+        */
+       andi.   r10,r9,MSR_EE
+       beq     1f
+       /*
+        * Since the ftrace irqsoff latency trace checks CALLER_ADDR1,
+        * which is the stack frame here, we need to force a stack frame
+        * in case we came from user space.
+        */
+       stwu    r1,-32(r1)
+       mflr    r0
+       stw     r0,4(r1)
+       stwu    r1,-32(r1)
+       bl      trace_hardirqs_on
+       lwz     r1,0(r1)
+       lwz     r1,0(r1)
+       lwz     r9,_MSR(r1)
+1:
+#endif /* CONFIG_TRACE_IRQFLAGS */
+
        lwz     r0,GPR0(r1)
        lwz     r2,GPR2(r1)
        REST_4GPRS(3, r1)
@@ -782,7 +931,6 @@ END_FTR_SECTION_IFSET(CPU_FTR_NEED_PAIRED_STWCX)
        stwcx.  r0,0,r1                 /* to clear the reservation */
 
 #if !(defined(CONFIG_4xx) || defined(CONFIG_BOOKE))
-       lwz     r9,_MSR(r1)
        andi.   r10,r9,MSR_RI           /* check if this exception occurred */
        beql    nonrecoverable          /* at a bad place (MSR:RI = 0) */
 
@@ -805,7 +953,6 @@ END_FTR_SECTION_IFSET(CPU_FTR_NEED_PAIRED_STWCX)
        MTMSRD(r10)             /* clear the RI bit */
        .globl exc_exit_restart
 exc_exit_restart:
-       lwz     r9,_MSR(r1)
        lwz     r12,_NIP(r1)
        FIX_SRR1(r9,r10)
        mtspr   SPRN_SRR0,r12
@@ -914,7 +1061,7 @@ exc_exit_restart_end:
        mtspr   SPRN_##exc_lvl_srr0,r9;                                 \
        mtspr   SPRN_##exc_lvl_srr1,r10;
 
-#if defined(CONFIG_FSL_BOOKE)
+#if defined(CONFIG_PPC_BOOK3E_MMU)
 #ifdef CONFIG_PHYS_64BIT
 #define        RESTORE_MAS7                                                    \
        lwz     r11,MAS7(r1);                                           \
@@ -945,7 +1092,7 @@ exc_exit_restart_end:
 #ifdef CONFIG_40x
        .globl  ret_from_crit_exc
 ret_from_crit_exc:
-       mfspr   r9,SPRN_SPRG3
+       mfspr   r9,SPRN_SPRG_THREAD
        lis     r10,saved_ksp_limit@ha;
        lwz     r10,saved_ksp_limit@l(r10);
        tovirt(r9,r9);
@@ -956,22 +1103,22 @@ ret_from_crit_exc:
        lwz     r10,crit_srr1@l(r10);
        mtspr   SPRN_SRR0,r9;
        mtspr   SPRN_SRR1,r10;
-       RET_FROM_EXC_LEVEL(SPRN_CSRR0, SPRN_CSRR1, RFCI)
+       RET_FROM_EXC_LEVEL(SPRN_CSRR0, SPRN_CSRR1, PPC_RFCI)
 #endif /* CONFIG_40x */
 
 #ifdef CONFIG_BOOKE
        .globl  ret_from_crit_exc
 ret_from_crit_exc:
-       mfspr   r9,SPRN_SPRG3
+       mfspr   r9,SPRN_SPRG_THREAD
        lwz     r10,SAVED_KSP_LIMIT(r1)
        stw     r10,KSP_LIMIT(r9)
        RESTORE_xSRR(SRR0,SRR1);
        RESTORE_MMU_REGS;
-       RET_FROM_EXC_LEVEL(SPRN_CSRR0, SPRN_CSRR1, RFCI)
+       RET_FROM_EXC_LEVEL(SPRN_CSRR0, SPRN_CSRR1, PPC_RFCI)
 
        .globl  ret_from_debug_exc
 ret_from_debug_exc:
-       mfspr   r9,SPRN_SPRG3
+       mfspr   r9,SPRN_SPRG_THREAD
        lwz     r10,SAVED_KSP_LIMIT(r1)
        stw     r10,KSP_LIMIT(r9)
        lwz     r9,THREAD_INFO-THREAD(r9)
@@ -981,18 +1128,18 @@ ret_from_debug_exc:
        RESTORE_xSRR(SRR0,SRR1);
        RESTORE_xSRR(CSRR0,CSRR1);
        RESTORE_MMU_REGS;
-       RET_FROM_EXC_LEVEL(SPRN_DSRR0, SPRN_DSRR1, RFDI)
+       RET_FROM_EXC_LEVEL(SPRN_DSRR0, SPRN_DSRR1, PPC_RFDI)
 
        .globl  ret_from_mcheck_exc
 ret_from_mcheck_exc:
-       mfspr   r9,SPRN_SPRG3
+       mfspr   r9,SPRN_SPRG_THREAD
        lwz     r10,SAVED_KSP_LIMIT(r1)
        stw     r10,KSP_LIMIT(r9)
        RESTORE_xSRR(SRR0,SRR1);
        RESTORE_xSRR(CSRR0,CSRR1);
        RESTORE_xSRR(DSRR0,DSRR1);
        RESTORE_MMU_REGS;
-       RET_FROM_EXC_LEVEL(SPRN_MCSRR0, SPRN_MCSRR1, RFMCI)
+       RET_FROM_EXC_LEVEL(SPRN_MCSRR0, SPRN_MCSRR1, PPC_RFMCI)
 #endif /* CONFIG_BOOKE */
 
 /*
@@ -1035,11 +1182,18 @@ do_work:                        /* r10 contains MSR_KERNEL here */
        beq     do_user_signal
 
 do_resched:                    /* r10 contains MSR_KERNEL here */
+       /* Note: We don't need to inform lockdep that we are enabling
+        * interrupts here. As far as it knows, they are already enabled
+        */
        ori     r10,r10,MSR_EE
        SYNC
        MTMSRD(r10)             /* hard-enable interrupts */
        bl      schedule
 recheck:
+       /* Note: And we don't tell it we are disabling them again
+        * neither. Those disable/enable cycles used to peek at
+        * TI_FLAGS aren't advertised.
+        */
        LOAD_MSR_KERNEL(r10,MSR_KERNEL)
        SYNC
        MTMSRD(r10)             /* disable interrupts */
@@ -1062,7 +1216,7 @@ do_user_signal:                   /* r10 contains MSR_KERNEL here */
        stw     r3,_TRAP(r1)
 2:     addi    r3,r1,STACK_FRAME_OVERHEAD
        mr      r4,r9
-       bl      do_signal
+       bl      do_notify_resume
        REST_NVGPRS(r1)
        b       recheck
 
@@ -1136,7 +1290,7 @@ _GLOBAL(enter_rtas)
        MTMSRD(r0)              /* don't get trashed */
        li      r9,MSR_KERNEL & ~(MSR_IR|MSR_DR)
        mtlr    r6
-       mtspr   SPRN_SPRG2,r7
+       mtspr   SPRN_SPRG_RTAS,r7
        mtspr   SPRN_SRR0,r8
        mtspr   SPRN_SRR1,r9
        RFI
@@ -1146,7 +1300,7 @@ _GLOBAL(enter_rtas)
        FIX_SRR1(r9,r0)
        addi    r1,r1,INT_FRAME_SIZE
        li      r0,0
-       mtspr   SPRN_SPRG2,r0
+       mtspr   SPRN_SPRG_RTAS,r0
        mtspr   SPRN_SRR0,r8
        mtspr   SPRN_SRR1,r9
        RFI                     /* return to caller */
@@ -1162,95 +1316,41 @@ machine_check_in_rtas:
 #ifdef CONFIG_DYNAMIC_FTRACE
 _GLOBAL(mcount)
 _GLOBAL(_mcount)
-       stwu    r1,-48(r1)
-       stw     r3, 12(r1)
-       stw     r4, 16(r1)
-       stw     r5, 20(r1)
-       stw     r6, 24(r1)
-       mflr    r3
-       stw     r7, 28(r1)
-       mfcr    r5
-       stw     r8, 32(r1)
-       stw     r9, 36(r1)
-       stw     r10,40(r1)
-       stw     r3, 44(r1)
-       stw     r5, 8(r1)
-       subi    r3, r3, MCOUNT_INSN_SIZE
-       .globl mcount_call
-mcount_call:
-       bl      ftrace_stub
-       nop
-       lwz     r6, 8(r1)
-       lwz     r0, 44(r1)
-       lwz     r3, 12(r1)
+       /*
+        * It is required that _mcount on PPC32 must preserve the
+        * link register. But we have r0 to play with. We use r0
+        * to push the return address back to the caller of mcount
+        * into the ctr register, restore the link register and
+        * then jump back using the ctr register.
+        */
+       mflr    r0
        mtctr   r0
-       lwz     r4, 16(r1)
-       mtcr    r6
-       lwz     r5, 20(r1)
-       lwz     r6, 24(r1)
-       lwz     r0, 52(r1)
-       lwz     r7, 28(r1)
-       lwz     r8, 32(r1)
+       lwz     r0, 4(r1)
        mtlr    r0
-       lwz     r9, 36(r1)
-       lwz     r10,40(r1)
-       addi    r1, r1, 48
        bctr
 
 _GLOBAL(ftrace_caller)
-       /* Based off of objdump optput from glibc */
-       stwu    r1,-48(r1)
-       stw     r3, 12(r1)
-       stw     r4, 16(r1)
-       stw     r5, 20(r1)
-       stw     r6, 24(r1)
-       mflr    r3
-       lwz     r4, 52(r1)
-       mfcr    r5
-       stw     r7, 28(r1)
-       stw     r8, 32(r1)
-       stw     r9, 36(r1)
-       stw     r10,40(r1)
-       stw     r3, 44(r1)
-       stw     r5, 8(r1)
+       MCOUNT_SAVE_FRAME
+       /* r3 ends up with link register */
        subi    r3, r3, MCOUNT_INSN_SIZE
 .globl ftrace_call
 ftrace_call:
        bl      ftrace_stub
        nop
-       lwz     r6, 8(r1)
-       lwz     r0, 44(r1)
-       lwz     r3, 12(r1)
-       mtctr   r0
-       lwz     r4, 16(r1)
-       mtcr    r6
-       lwz     r5, 20(r1)
-       lwz     r6, 24(r1)
-       lwz     r0, 52(r1)
-       lwz     r7, 28(r1)
-       lwz     r8, 32(r1)
-       mtlr    r0
-       lwz     r9, 36(r1)
-       lwz     r10,40(r1)
-       addi    r1, r1, 48
+#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
 #else
 _GLOBAL(mcount)
 _GLOBAL(_mcount)
-       stwu    r1,-48(r1)
-       stw     r3, 12(r1)
-       stw     r4, 16(r1)
-       stw     r5, 20(r1)
-       stw     r6, 24(r1)
-       mflr    r3
-       lwz     r4, 52(r1)
-       mfcr    r5
-       stw     r7, 28(r1)
-       stw     r8, 32(r1)
-       stw     r9, 36(r1)
-       stw     r10,40(r1)
-       stw     r3, 44(r1)
-       stw     r5, 8(r1)
+
+       MCOUNT_SAVE_FRAME
 
        subi    r3, r3, MCOUNT_INSN_SIZE
        LOAD_REG_ADDR(r5, ftrace_trace_function)
@@ -1258,28 +1358,55 @@ _GLOBAL(_mcount)
 
        mtctr   r5
        bctrl
-
        nop
 
-       lwz     r6, 8(r1)
-       lwz     r0, 44(r1)
-       lwz     r3, 12(r1)
-       mtctr   r0
-       lwz     r4, 16(r1)
-       mtcr    r6
-       lwz     r5, 20(r1)
-       lwz     r6, 24(r1)
-       lwz     r0, 52(r1)
-       lwz     r7, 28(r1)
-       lwz     r8, 32(r1)
-       mtlr    r0
-       lwz     r9, 36(r1)
-       lwz     r10,40(r1)
-       addi    r1, r1, 48
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+       b       ftrace_graph_caller
+#endif
+       MCOUNT_RESTORE_FRAME
        bctr
 #endif
 
 _GLOBAL(ftrace_stub)
        blr
 
+#ifdef CONFIG_FUNCTION_GRAPH_TRACER
+_GLOBAL(ftrace_graph_caller)
+       /* load r4 with local address */
+       lwz     r4, 44(r1)
+       subi    r4, r4, MCOUNT_INSN_SIZE
+
+       /* get the parent address */
+       addi    r3, r1, 52
+
+       bl      prepare_ftrace_return
+       nop
+
+       MCOUNT_RESTORE_FRAME
+       /* old link register ends up in ctr reg */
+       bctr
+
+_GLOBAL(return_to_handler)
+       /* need to save return values */
+       stwu    r1, -32(r1)
+       stw     r3, 20(r1)
+       stw     r4, 16(r1)
+       stw     r31, 12(r1)
+       mr      r31, r1
+
+       bl      ftrace_return_to_handler
+       nop
+
+       /* return value has real return address */
+       mtlr    r3
+
+       lwz     r3, 20(r1)
+       lwz     r4, 16(r1)
+       lwz     r31,12(r1)
+       lwz     r1, 0(r1)
+
+       /* Jump back to real return address */
+       blr
+#endif /* CONFIG_FUNCTION_GRAPH_TRACER */
+
 #endif /* CONFIG_MCOUNT */