DEFINE(THREAD_FPSTATE, offsetof(struct thread_struct, fp_state));
        DEFINE(THREAD_FPSAVEAREA, offsetof(struct thread_struct, fp_save_area));
        DEFINE(FPSTATE_FPSCR, offsetof(struct thread_fp_state, fpscr));
+       DEFINE(THREAD_LOAD_FP, offsetof(struct thread_struct, load_fp));
 #ifdef CONFIG_ALTIVEC
        DEFINE(THREAD_VRSTATE, offsetof(struct thread_struct, vr_state));
        DEFINE(THREAD_VRSAVEAREA, offsetof(struct thread_struct, vr_save_area));
        DEFINE(THREAD_VRSAVE, offsetof(struct thread_struct, vrsave));
        DEFINE(THREAD_USED_VR, offsetof(struct thread_struct, used_vr));
        DEFINE(VRSTATE_VSCR, offsetof(struct thread_vr_state, vscr));
+       DEFINE(THREAD_LOAD_VEC, offsetof(struct thread_struct, load_vec));
 #endif /* CONFIG_ALTIVEC */
 #ifdef CONFIG_VSX
        DEFINE(THREAD_USED_VSR, offsetof(struct thread_struct, used_vsr));
 
        li      r11,-MAX_ERRNO
        andi.   r0,r9,(_TIF_SYSCALL_DOTRACE|_TIF_SINGLESTEP|_TIF_USER_WORK_MASK|_TIF_PERSYSCALL_MASK)
        bne-    syscall_exit_work
-       cmpld   r3,r11
+
+       andi.   r0,r8,MSR_FP
+       beq 2f
+#ifdef CONFIG_ALTIVEC
+       andis.  r0,r8,MSR_VEC@h
+       bne     3f
+#endif
+2:     addi    r3,r1,STACK_FRAME_OVERHEAD
+       bl      restore_math
+       ld      r8,_MSR(r1)
+       ld      r3,RESULT(r1)
+       li      r11,-MAX_ERRNO
+
+3:     cmpld   r3,r11
        ld      r5,_CCR(r1)
        bge-    syscall_error
 .Lsyscall_error_cont:
 
        /* Check current_thread_info()->flags */
        andi.   r0,r4,_TIF_USER_WORK_MASK
-#ifdef CONFIG_PPC_BOOK3E
        bne     1f
+#ifdef CONFIG_PPC_BOOK3E
        /*
         * Check to see if the dbcr0 register is set up to debug.
         * Use the internal debug mode bit to do this.
        mtspr   SPRN_DBSR,r10
        b       restore
 #else
-       beq     restore
+       addi    r3,r1,STACK_FRAME_OVERHEAD
+       bl      restore_math
+       b       restore
 #endif
 1:     andi.   r0,r4,_TIF_NEED_RESCHED
        beq     2f
 
        }
 }
 EXPORT_SYMBOL(enable_kernel_fp);
+
+static int restore_fp(struct task_struct *tsk) {
+       if (tsk->thread.load_fp) {
+               load_fp_state(¤t->thread.fp_state);
+               current->thread.load_fp++;
+               return 1;
+       }
+       return 0;
+}
+#else
+static int restore_fp(struct task_struct *tsk) { return 0; }
 #endif /* CONFIG_PPC_FPU */
 
 #ifdef CONFIG_ALTIVEC
+#define loadvec(thr) ((thr).load_vec)
+
 void giveup_altivec(struct task_struct *tsk)
 {
        check_if_tm_restore_required(tsk);
        }
 }
 EXPORT_SYMBOL_GPL(flush_altivec_to_thread);
+
+static int restore_altivec(struct task_struct *tsk)
+{
+       if (cpu_has_feature(CPU_FTR_ALTIVEC) && tsk->thread.load_vec) {
+               load_vr_state(&tsk->thread.vr_state);
+               tsk->thread.used_vr = 1;
+               tsk->thread.load_vec++;
+
+               return 1;
+       }
+       return 0;
+}
+#else
+#define loadvec(thr) 0
+static inline int restore_altivec(struct task_struct *tsk) { return 0; }
 #endif /* CONFIG_ALTIVEC */
 
 #ifdef CONFIG_VSX
        }
 }
 EXPORT_SYMBOL_GPL(flush_vsx_to_thread);
+
+static int restore_vsx(struct task_struct *tsk)
+{
+       if (cpu_has_feature(CPU_FTR_VSX)) {
+               tsk->thread.used_vsr = 1;
+               return 1;
+       }
+
+       return 0;
+}
+#else
+static inline int restore_vsx(struct task_struct *tsk) { return 0; }
 #endif /* CONFIG_VSX */
 
 #ifdef CONFIG_SPE
 }
 EXPORT_SYMBOL(giveup_all);
 
+void restore_math(struct pt_regs *regs)
+{
+       unsigned long msr;
+
+       if (!current->thread.load_fp && !loadvec(current->thread))
+               return;
+
+       msr = regs->msr;
+       msr_check_and_set(msr_all_available);
+
+       /*
+        * Only reload if the bit is not set in the user MSR, the bit BEING set
+        * indicates that the registers are hot
+        */
+       if ((!(msr & MSR_FP)) && restore_fp(current))
+               msr |= MSR_FP | current->thread.fpexc_mode;
+
+       if ((!(msr & MSR_VEC)) && restore_altivec(current))
+               msr |= MSR_VEC;
+
+       if ((msr & (MSR_FP | MSR_VEC)) == (MSR_FP | MSR_VEC) &&
+                       restore_vsx(current)) {
+               msr |= MSR_VSX;
+       }
+
+       msr_check_and_clear(msr_all_available);
+
+       regs->msr = msr;
+}
+
 void flush_all_to_thread(struct task_struct *tsk)
 {
        if (tsk->thread.regs) {
 
        msr_diff = current->thread.ckpt_regs.msr & ~regs->msr;
        msr_diff &= MSR_FP | MSR_VEC | MSR_VSX;
-       if (msr_diff & MSR_FP) {
-               msr_check_and_set(MSR_FP);
-               load_fp_state(¤t->thread.fp_state);
-               msr_check_and_clear(MSR_FP);
-               regs->msr |= current->thread.fpexc_mode;
-       }
-       if (msr_diff & MSR_VEC) {
-               msr_check_and_set(MSR_VEC);
-               load_vr_state(¤t->thread.vr_state);
-               msr_check_and_clear(MSR_VEC);
-       }
+
+       restore_math(regs);
+
        regs->msr |= msr_diff;
 }
 
                batch = this_cpu_ptr(&ppc64_tlb_batch);
                batch->active = 1;
        }
+
+       if (current_thread_info()->task->thread.regs)
+               restore_math(current_thread_info()->task->thread.regs);
+
 #endif /* CONFIG_PPC_BOOK3S_64 */
 
        return last;