1 files changed, 14 insertions, 8 deletions

diff --git a/arch/arm/kernel/entry-armv.S b/arch/arm/kernel/entry-armv.S
index be439cab92c6..d9fb819bf7cc 100644
--- a/arch/arm/kernel/entry-armv.S
+++ b/arch/arm/kernel/entry-armv.S
@@ -47,6 +47,13 @@
         movne   r0, sp
         adrne   lr, 1b
         bne     do_IPI
+
+#ifdef CONFIG_LOCAL_TIMERS
+        test_for_ltirq r0, r6, r5, lr
+        movne   r0, sp
+        adrne   lr, 1b
+        bne     do_local_timer
+#endif
 #endif
 
         .endm
@@ -785,7 +792,7 @@ __kuser_helper_end:
  * SP points to a minimal amount of processor-private memory, the address
  * of which is copied into r0 for the mode specific abort handler.
  */
-        .macro  vector_stub, name, correction=0
+        .macro  vector_stub, name, mode, correction=0
         .align  5
 
 vector_\name:
@@ -805,15 +812,14 @@ vector_\name:
         @ Prepare for SVC32 mode.  IRQs remain disabled.
         @
         mrs     r0, cpsr
-        bic     r0, r0, #MODE_MASK
-        orr     r0, r0, #SVC_MODE
+        eor     r0, r0, #(\mode ^ SVC_MODE)
         msr     spsr_cxsf, r0
 
         @
         @ the branch table must immediately follow this code
         @
-        mov     r0, sp
         and     lr, lr, #0x0f
+        mov     r0, sp
         ldr     lr, [pc, lr, lsl #2]
         movs    pc, lr                  @ branch to handler in SVC mode
         .endm
@@ -823,7 +829,7 @@ __stubs_start:
 /*
  * Interrupt dispatcher
  */
-        vector_stub     irq, 4
+        vector_stub     irq, IRQ_MODE, 4
 
         .long   __irq_usr                       @  0  (USR_26 / USR_32)
         .long   __irq_invalid                   @  1  (FIQ_26 / FIQ_32)
@@ -846,7 +852,7 @@ __stubs_start:
  * Data abort dispatcher
  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
  */
-        vector_stub     dabt, 8
+        vector_stub     dabt, ABT_MODE, 8
 
         .long   __dabt_usr                      @  0  (USR_26 / USR_32)
         .long   __dabt_invalid                  @  1  (FIQ_26 / FIQ_32)
@@ -869,7 +875,7 @@ __stubs_start:
  * Prefetch abort dispatcher
  * Enter in ABT mode, spsr = USR CPSR, lr = USR PC
  */
-        vector_stub     pabt, 4
+        vector_stub     pabt, ABT_MODE, 4
 
         .long   __pabt_usr                      @  0 (USR_26 / USR_32)
         .long   __pabt_invalid                  @  1 (FIQ_26 / FIQ_32)
@@ -892,7 +898,7 @@ __stubs_start:
  * Undef instr entry dispatcher
  * Enter in UND mode, spsr = SVC/USR CPSR, lr = SVC/USR PC
  */
-        vector_stub     und
+        vector_stub     und, UND_MODE
 
         .long   __und_usr                       @  0 (USR_26 / USR_32)
         .long   __und_invalid                   @  1 (FIQ_26 / FIQ_32)