1 files changed, 5 insertions, 7 deletions

diff --git a/arch/arm/kernel/entry-header.S b/arch/arm/kernel/entry-header.S
index 4d6ad8348e89..9a8531eadd3d 100644
--- a/arch/arm/kernel/entry-header.S
+++ b/arch/arm/kernel/entry-header.S
@@ -121,15 +121,13 @@
         .endm
 #else   /* CONFIG_THUMB2_KERNEL */
         .macro  svc_exit, rpsr
+        ldr     lr, [sp, #S_SP]                 @ top of the stack
+        ldrd    r0, r1, [sp, #S_LR]             @ calling lr and pc
         clrex                                   @ clear the exclusive monitor
-        ldr     r0, [sp, #S_SP]                 @ top of the stack
-        ldr     r1, [sp, #S_PC]                 @ return address
-        tst     r0, #4                          @ orig stack 8-byte aligned?
-        stmdb   r0, {r1, \rpsr}                 @ rfe context
+        stmdb   lr!, {r0, r1, \rpsr}            @ calling lr and rfe context
         ldmia   sp, {r0 - r12}
-        ldr     lr, [sp, #S_LR]
-        addeq   sp, sp, #S_FRAME_SIZE - 8       @ aligned
-        addne   sp, sp, #S_FRAME_SIZE - 4       @ not aligned
+        mov     sp, lr
+        ldr     lr, [sp], #4
         rfeia   sp!
         .endm