1 files changed, 25 insertions, 3 deletions

diff --git a/arch/arm/kernel/entry-common.S b/arch/arm/kernel/entry-common.S
index 65c58b34db97..07da010b67d4 100644
--- a/arch/arm/kernel/entry-common.S
+++ b/arch/arm/kernel/entry-common.S
@@ -27,7 +27,15 @@ ret_fast_syscall:
         ldr     r1, [tsk, #TI_FLAGS]
         tst     r1, #_TIF_WORK_MASK
         bne     fast_work_pending
-        fast_restore_user_regs
+
+        @ fast_restore_user_regs
+        ldr     r1, [sp, #S_OFF + S_PSR]        @ get calling cpsr
+        ldr     lr, [sp, #S_OFF + S_PC]!        @ get pc
+        msr     spsr_cxsf, r1                   @ save in spsr_svc
+        ldmdb   sp, {r1 - lr}^                  @ get calling r1 - lr
+        mov     r0, r0
+        add     sp, sp, #S_FRAME_SIZE - S_PC
+        movs    pc, lr                          @ return & move spsr_svc into cpsr
 
 /*
  * Ok, we need to do extra processing, enter the slow path.
@@ -57,7 +65,14 @@ ret_slow_syscall:
         tst     r1, #_TIF_WORK_MASK
         bne     work_pending
 no_work_pending:
-        slow_restore_user_regs
+        @ slow_restore_user_regs
+        ldr     r1, [sp, #S_PSR]                @ get calling cpsr
+        ldr     lr, [sp, #S_PC]!                @ get pc
+        msr     spsr_cxsf, r1                   @ save in spsr_svc
+        ldmdb   sp, {r0 - lr}^                  @ get calling r1 - lr
+        mov     r0, r0
+        add     sp, sp, #S_FRAME_SIZE - S_PC
+        movs    pc, lr                          @ return & move spsr_svc into cpsr
 
 /*
  * This is how we return from a fork.
@@ -109,7 +124,14 @@ ENTRY(ret_from_fork)
 
         .align  5
 ENTRY(vector_swi)
-        save_user_regs
+        sub     sp, sp, #S_FRAME_SIZE
+        stmia   sp, {r0 - r12}                  @ Calling r0 - r12
+        add     r8, sp, #S_PC
+        stmdb   r8, {sp, lr}^                   @ Calling sp, lr
+        mrs     r8, spsr                        @ called from non-FIQ mode, so ok.
+        str     lr, [sp, #S_PC]                 @ Save calling PC
+        str     r8, [sp, #S_PSR]                @ Save CPSR
+        str     r0, [sp, #S_OLD_R0]             @ Save OLD_R0
         zero_fp
 
         /*