Merge branch 'master' into stm32_uart_tx_dma

4 files changed, 72 insertions, 68 deletions

diff --git a/libcpu/arm/arm926/trap.c b/libcpu/arm/arm926/trap.c
index de04c3f8cb..9e7ba48037 100644
--- a/libcpu/arm/arm926/trap.c
+++ b/libcpu/arm/arm926/trap.c
@@ -197,14 +197,14 @@ void rt_hw_trap_resv(struct rt_hw_register *regs)
     rt_hw_cpu_shutdown();
 }
 
-extern void rt_interrupt_dispatch(void);
+extern void rt_interrupt_dispatch(rt_uint32_t fiq_irq);
 
 void rt_hw_trap_irq(void)
 {
-    rt_interrupt_dispatch();
+    rt_interrupt_dispatch(INT_IRQ);
 }
 
 void rt_hw_trap_fiq(void)
 {
-    rt_interrupt_dispatch();
+    rt_interrupt_dispatch(INT_FIQ);
 }
diff --git a/libcpu/arm/cortex-a/context_gcc.S b/libcpu/arm/cortex-a/context_gcc.S
index b009630fa8..dff7a1de68 100644
--- a/libcpu/arm/cortex-a/context_gcc.S
+++ b/libcpu/arm/cortex-a/context_gcc.S
@@ -34,8 +34,9 @@ rt_hw_interrupt_enable:
     bx  lr
 
 /*
- * void rt_hw_context_switch_to(rt_uint32 to);
- * r0 --> to
+ * void rt_hw_context_switch_to(rt_uint32 to, struct rt_thread *to_thread);
+ * r0 --> to (thread stack)
+ * r1 --> to_thread
  */
 .globl rt_hw_context_switch_to
 rt_hw_context_switch_to:
@@ -45,16 +46,7 @@ rt_hw_context_switch_to:
     mov     r0, r1
     bl      rt_cpus_lock_status_restore
 #endif /*RT_USING_SMP*/
-
-#ifdef RT_USING_LWP
-    ldmfd sp, {r13, r14}^   @ pop usr_sp usr_lr
-    add sp, #8
-#endif
-
-    ldmfd sp!, {r4}         @ pop new task spsr
-    msr spsr_cxsf, r4
-
-    ldmfd sp!, {r0-r12, lr, pc}^   @ pop new task r0-r12, lr & pc
+    b       rt_hw_context_switch_exit
 
 .section .bss.share.isr
 _guest_switch_lvl:
@@ -64,9 +56,10 @@ _guest_switch_lvl:
 
 .section .text.isr, "ax"
 /*
- * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to);
- * r0 --> from
- * r1 --> to
+ * void rt_hw_context_switch(rt_uint32 from, rt_uint32 to, struct rt_thread *to_thread);
+ * r0 --> from (from_thread stack)
+ * r1 --> to (to_thread stack)
+ * r2 --> to_thread
  */
 .globl rt_hw_context_switch
 rt_hw_context_switch:
@@ -91,15 +84,7 @@ rt_hw_context_switch:
     mov     r0, r2
     bl      rt_cpus_lock_status_restore
 #endif /*RT_USING_SMP*/
-
-#ifdef RT_USING_LWP
-    ldmfd sp, {r13, r14}^   @ pop usr_sp usr_lr
-    add sp, #8
-#endif
-
-    ldmfd sp!, {r4}         @ pop new task cpsr to spsr
-    msr spsr_cxsf, r4
-    ldmfd sp!, {r0-r12, lr, pc}^  @ pop new task r0-r12, lr & pc, copy spsr to cpsr
+    b       rt_hw_context_switch_exit
 
 /*
  * void rt_hw_context_switch_interrupt(rt_uint32 from, rt_uint32 to);
@@ -121,51 +106,19 @@ rt_hw_context_switch:
 .globl rt_hw_context_switch_interrupt
 rt_hw_context_switch_interrupt:
 #ifdef RT_USING_SMP
-    /* r0 :irq_mod context
+    /* r0 :svc_mod context
      * r1 :addr of from_thread's sp
      * r2 :addr of to_thread's sp
      * r3 :to_thread's tcb
      */
 
-    @ r0 point to {r0-r3} in stack
-    push    {r1 - r3}
-    mov     r1, r0
-    add     r0, r0, #4*4
-    ldmfd   r0!, {r4-r12,lr}@ reload saved registers
-    mrs     r3,  spsr       @ get cpsr of interrupt thread
-    sub     r2,  lr, #4     @ save old task's pc to r2
-    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
-
-    stmfd   sp!, {r2}       @ push old task's pc
-    stmfd   sp!, {r4-r12,lr}@ push old task's lr,r12-r4
-    ldmfd   r1,  {r4-r7}    @ restore r0-r3 of the interrupt thread
-    stmfd   sp!, {r4-r7}    @ push old task's r0-r3
-    stmfd   sp!, {r3}       @ push old task's cpsr
-
-#ifdef RT_USING_LWP
-    stmfd sp, {r13,r14}^    @push usr_sp usr_lr
-    sub sp, #8
-#endif
-
-    msr     cpsr_c, #I_Bit|F_Bit|Mode_IRQ
-    pop     {r1 - r3}
-    mov     sp, r0
-    msr     cpsr_c, #I_Bit|F_Bit|Mode_SVC
-    str     sp, [r1]
+    str     r0, [r1]
 
     ldr     sp, [r2]
     mov     r0, r3
     bl      rt_cpus_lock_status_restore
 
-#ifdef RT_USING_LWP
-    ldmfd sp, {r13,r14}^  @pop usr_sp usr_lr
-    add sp, #8
-#endif
-
-    ldmfd   sp!, {r4}       @ pop new task's cpsr to spsr
-    msr     spsr_cxsf, r4
-
-    ldmfd   sp!, {r0-r12,lr,pc}^ @ pop new task's r0-r12,lr & pc, copy spsr to cpsr
+    b       rt_hw_context_switch_exit
 
 #else /*RT_USING_SMP*/
     ldr r2, =rt_thread_switch_interrupt_flag
@@ -181,3 +134,25 @@ _reswitch:
     str r1, [r2]
     bx  lr
 #endif /*RT_USING_SMP*/
+
+.global rt_hw_context_switch_exit
+rt_hw_context_switch_exit:
+
+#ifdef RT_USING_SMP
+#ifdef RT_USING_SIGNALS
+    mov     r0, sp
+    cps #Mode_IRQ
+    bl      rt_signal_check
+    cps #Mode_SVC
+    mov     sp, r0
+#endif
+#endif
+
+#ifdef RT_USING_LWP
+    ldmfd   sp, {r13, r14}^ /* usr_sp, usr_lr */
+    add     sp, #8
+#endif
+    ldmfd   sp!, {r1}
+    msr     spsr_cxsf, r1        /* original mode */
+    ldmfd   sp!, {r0-r12,lr,pc}^ /* irq return */
+
diff --git a/libcpu/arm/cortex-a/interrupt.h b/libcpu/arm/cortex-a/interrupt.h
index 31f79cc66e..442187edee 100644
--- a/libcpu/arm/cortex-a/interrupt.h
+++ b/libcpu/arm/cortex-a/interrupt.h
@@ -17,6 +17,8 @@
 #define INT_IRQ     0x00
 #define INT_FIQ     0x01
 
+void rt_hw_vector_init(void);
+
 void rt_hw_interrupt_control(int vector, int priority, int route);
 
 void rt_hw_interrupt_init(void);
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 51026b7ed5..014846f3aa 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -11,7 +11,6 @@
  */
 
 #include "rtconfig.h"
-
 .equ Mode_USR,        0x10
 .equ Mode_FIQ,        0x11
 .equ Mode_IRQ,        0x12
@@ -158,20 +157,48 @@ vector_fiq:
 vector_irq:
 #ifdef RT_USING_SMP
     clrex
+
+    stmfd   sp!, {r0, r1}
+    cps     #Mode_SVC
+    mov     r0, sp          /* svc_sp */
+    mov     r1, lr          /* svc_lr */
+
+    cps     #Mode_IRQ
+    sub     lr, #4
+    stmfd   r0!, {r1, lr}       /* svc_lr, svc_pc */
+    stmfd   r0!, {r2 - r12}
+    ldmfd   sp!, {r1, r2}     /* original r0, r1 */
+    stmfd   r0!, {r1 - r2}
+    mrs     r1,  spsr         /* original mode */
+    stmfd   r0!, {r1}
+
+#ifdef RT_USING_LWP
+    stmfd   r0, {r13, r14}^ /* usr_sp, usr_lr */
+    sub   r0, #8
 #endif
-    stmfd   sp!, {r0-r12,lr}
+    /* now irq stack is clean */
+    /* r0 is task svc_sp */
+    /* backup r0 -> r8 */
+    mov r8, r0
 
     bl      rt_interrupt_enter
     bl      rt_hw_trap_irq
     bl      rt_interrupt_leave
 
-#ifdef RT_USING_SMP
-    mov     r0, sp
+    cps     #Mode_SVC
+    mov     sp, r8
+    mov     r0, r8
     bl      rt_scheduler_do_irq_switch
 
-    ldmfd   sp!, {r0-r12,lr}
-    subs    pc,  lr, #4
+    b       rt_hw_context_switch_exit
+
 #else
+    stmfd   sp!, {r0-r12,lr}
+
+    bl      rt_interrupt_enter
+    bl      rt_hw_trap_irq
+    bl      rt_interrupt_leave
+
     @ if rt_thread_switch_interrupt_flag set, jump to
     @ rt_hw_context_switch_interrupt_do and don't return
     ldr     r0, =rt_thread_switch_interrupt_flag