Merge pull request #4 from RT-Thread/master

sync

6 files changed, 113 insertions, 210 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
diff --git a/libcpu/risc-v/common/context_gcc.S b/libcpu/risc-v/common/context_gcc.S
index 62e64d8104..f28b15319b 100644
--- a/libcpu/risc-v/common/context_gcc.S
+++ b/libcpu/risc-v/common/context_gcc.S
@@ -49,45 +49,9 @@ rt_hw_context_switch_to:
     mv   a0,   a1
     jal  rt_cpus_lock_status_restore
 #endif
-
-    /* load epc from stack */
-    LOAD a0,   0 * REGBYTES(sp)
-    csrw mepc, a0
-    LOAD x1,   1 * REGBYTES(sp)
-    /* load mstatus from stack */
     LOAD a0,   2 * REGBYTES(sp)
     csrw mstatus, a0
-    LOAD x4,   4 * REGBYTES(sp)
-    LOAD x5,   5 * REGBYTES(sp)
-    LOAD x6,   6 * REGBYTES(sp)
-    LOAD x7,   7 * REGBYTES(sp)
-    LOAD x8,   8 * REGBYTES(sp)
-    LOAD x9,   9 * REGBYTES(sp)
-    LOAD x10, 10 * REGBYTES(sp)
-    LOAD x11, 11 * REGBYTES(sp)
-    LOAD x12, 12 * REGBYTES(sp)
-    LOAD x13, 13 * REGBYTES(sp)
-    LOAD x14, 14 * REGBYTES(sp)
-    LOAD x15, 15 * REGBYTES(sp)
-    LOAD x16, 16 * REGBYTES(sp)
-    LOAD x17, 17 * REGBYTES(sp)
-    LOAD x18, 18 * REGBYTES(sp)
-    LOAD x19, 19 * REGBYTES(sp)
-    LOAD x20, 20 * REGBYTES(sp)
-    LOAD x21, 21 * REGBYTES(sp)
-    LOAD x22, 22 * REGBYTES(sp)
-    LOAD x23, 23 * REGBYTES(sp)
-    LOAD x24, 24 * REGBYTES(sp)
-    LOAD x25, 25 * REGBYTES(sp)
-    LOAD x26, 26 * REGBYTES(sp)
-    LOAD x27, 27 * REGBYTES(sp)
-    LOAD x28, 28 * REGBYTES(sp)
-    LOAD x29, 29 * REGBYTES(sp)
-    LOAD x30, 30 * REGBYTES(sp)
-    LOAD x31, 31 * REGBYTES(sp)
-
-    addi sp,  sp, 32 * REGBYTES
-    mret
+    j    rt_hw_context_switch_exit
 
 /*
  * #ifdef RT_USING_SMP
@@ -102,7 +66,6 @@ rt_hw_context_switch_to:
  */
     .globl rt_hw_context_switch
 rt_hw_context_switch:
-
     /* saved from thread context
      *     x1/ra       -> sp(0)
      *     x1/ra       -> sp(1)
@@ -163,48 +126,7 @@ save_mpie:
     jal  rt_cpus_lock_status_restore
 #endif /*RT_USING_SMP*/
 
-    /* resw ra to mepc */
-    LOAD a1,   0 * REGBYTES(sp)
-    csrw mepc, a1
-    LOAD x1,   1 * REGBYTES(sp)
-
-    /* force to machin mode(MPP=11) */
-    li a1, 0x00001800;
-    csrs mstatus, a1
-    LOAD a1,   2 * REGBYTES(sp)
-    csrs mstatus, a1
-
-    LOAD x4,   4 * REGBYTES(sp)
-    LOAD x5,   5 * REGBYTES(sp)
-    LOAD x6,   6 * REGBYTES(sp)
-    LOAD x7,   7 * REGBYTES(sp)
-    LOAD x8,   8 * REGBYTES(sp)
-    LOAD x9,   9 * REGBYTES(sp)
-    LOAD x10, 10 * REGBYTES(sp)
-    LOAD x11, 11 * REGBYTES(sp)
-    LOAD x12, 12 * REGBYTES(sp)
-    LOAD x13, 13 * REGBYTES(sp)
-    LOAD x14, 14 * REGBYTES(sp)
-    LOAD x15, 15 * REGBYTES(sp)
-    LOAD x16, 16 * REGBYTES(sp)
-    LOAD x17, 17 * REGBYTES(sp)
-    LOAD x18, 18 * REGBYTES(sp)
-    LOAD x19, 19 * REGBYTES(sp)
-    LOAD x20, 20 * REGBYTES(sp)
-    LOAD x21, 21 * REGBYTES(sp)
-    LOAD x22, 22 * REGBYTES(sp)
-    LOAD x23, 23 * REGBYTES(sp)
-    LOAD x24, 24 * REGBYTES(sp)
-    LOAD x25, 25 * REGBYTES(sp)
-    LOAD x26, 26 * REGBYTES(sp)
-    LOAD x27, 27 * REGBYTES(sp)
-    LOAD x28, 28 * REGBYTES(sp)
-    LOAD x29, 29 * REGBYTES(sp)
-    LOAD x30, 30 * REGBYTES(sp)
-    LOAD x31, 31 * REGBYTES(sp)
-
-    addi sp,  sp, 32 * REGBYTES
-    mret
+    j rt_hw_context_switch_exit
 
 #ifdef RT_USING_SMP
 /*
@@ -220,26 +142,42 @@ rt_hw_context_switch_interrupt:
 
     STORE a0, 0(a1)
 
-    csrr  a1, mepc
-    STORE a1, 0 * REGBYTES(a0)
-
-    csrr  a1, mstatus
-    STORE a1, 2 * REGBYTES(a0)
-
     LOAD  sp, 0(a2)
     move  a0, a3
     call rt_cpus_lock_status_restore
 
+    j rt_hw_context_switch_exit
+
+#endif
+
+.global rt_hw_context_switch_exit
+rt_hw_context_switch_exit:
+#ifdef RT_USING_SMP
+#ifdef RT_USING_SIGNALS
+    mv a0, sp
+
+    csrr  t0, mhartid
+    /* switch interrupt stack of current cpu */
+    la    sp, __stack_start__
+    addi  t1, t0, 1
+    li    t2, __STACKSIZE__
+    mul   t1, t1, t2
+    add   sp, sp, t1 /* sp = (cpuid + 1) * __STACKSIZE__ + __stack_start__ */
+
+    call rt_signal_check
+    mv sp, a0
+#endif
+#endif
     /* resw ra to mepc */
-    LOAD a1,   0 * REGBYTES(sp)
-    csrw mepc, a1
+    LOAD a0,   0 * REGBYTES(sp)
+    csrw mepc, a0
+
     LOAD x1,   1 * REGBYTES(sp)
 
-    /* force to machin mode(MPP=11) */
-    li a1, 0x00001800;
-    csrs mstatus, a1
-    LOAD a1,   2 * REGBYTES(sp)
-    csrs mstatus, a1
+    li    t0, 0x00001800
+    csrs  mstatus, t0
+    LOAD a0,   2 * REGBYTES(sp)
+    csrs mstatus, a0
 
     LOAD x4,   4 * REGBYTES(sp)
     LOAD x5,   5 * REGBYTES(sp)
@@ -272,5 +210,3 @@ rt_hw_context_switch_interrupt:
 
     addi sp,  sp, 32 * REGBYTES
     mret
-
-#endif
diff --git a/libcpu/risc-v/k210/interrupt_gcc.S b/libcpu/risc-v/k210/interrupt_gcc.S
index aeda72ab31..0393ea51f8 100644
--- a/libcpu/risc-v/k210/interrupt_gcc.S
+++ b/libcpu/risc-v/k210/interrupt_gcc.S
@@ -20,8 +20,12 @@ trap_entry:
     addi sp, sp, -32 * REGBYTES
 
     STORE x1,   1 * REGBYTES(sp)
-    li    t0,   0x80
-    STORE t0,   2 * REGBYTES(sp)
+
+    csrr  x1, mstatus
+    STORE x1,   2 * REGBYTES(sp)
+
+    csrr  x1, mepc
+    STORE x1, 0 * REGBYTES(sp)
 
     STORE x4,   4 * REGBYTES(sp)
     STORE x5,   5 * REGBYTES(sp)
@@ -75,9 +79,11 @@ trap_entry:
 
 #ifdef RT_USING_SMP
     /* s0 --> sp */
+    mv  sp, s0
     mv  a0, s0
     call rt_scheduler_do_irq_switch
-    mv  sp, s0
+    j   rt_hw_context_switch_exit
+
 #else
 
     /* switch to from_thread stack */
@@ -89,9 +95,6 @@ trap_entry:
     beqz  s2, spurious_interrupt
     sw    zero, 0(s0)
 
-    csrr  a0, mepc
-    STORE a0, 0 * REGBYTES(sp)
-
     la    s0, rt_interrupt_from_thread
     LOAD  s1, 0(s0)
     STORE sp, 0(s1)
@@ -100,47 +103,7 @@ trap_entry:
     LOAD  s1, 0(s0)
     LOAD  sp, 0(s1)
 
-    LOAD  a0,  0 * REGBYTES(sp)
-    csrw  mepc, a0
 #endif
 
 spurious_interrupt:
-    LOAD  x1,   1 * REGBYTES(sp)
-
-    /* Remain in M-mode after mret */
-    li    t0, 0x00001800
-    csrs  mstatus, t0
-    LOAD  t0,   2 * REGBYTES(sp)
-    csrs  mstatus, t0 
-
-    LOAD  x4,   4 * REGBYTES(sp)
-    LOAD  x5,   5 * REGBYTES(sp)
-    LOAD  x6,   6 * REGBYTES(sp)
-    LOAD  x7,   7 * REGBYTES(sp)
-    LOAD  x8,   8 * REGBYTES(sp)
-    LOAD  x9,   9 * REGBYTES(sp)
-    LOAD  x10, 10 * REGBYTES(sp)
-    LOAD  x11, 11 * REGBYTES(sp)
-    LOAD  x12, 12 * REGBYTES(sp)
-    LOAD  x13, 13 * REGBYTES(sp)
-    LOAD  x14, 14 * REGBYTES(sp)
-    LOAD  x15, 15 * REGBYTES(sp)
-    LOAD  x16, 16 * REGBYTES(sp)
-    LOAD  x17, 17 * REGBYTES(sp)
-    LOAD  x18, 18 * REGBYTES(sp)
-    LOAD  x19, 19 * REGBYTES(sp)
-    LOAD  x20, 20 * REGBYTES(sp)
-    LOAD  x21, 21 * REGBYTES(sp)
-    LOAD  x22, 22 * REGBYTES(sp)
-    LOAD  x23, 23 * REGBYTES(sp)
-    LOAD  x24, 24 * REGBYTES(sp)
-    LOAD  x25, 25 * REGBYTES(sp)
-    LOAD  x26, 26 * REGBYTES(sp)
-    LOAD  x27, 27 * REGBYTES(sp)
-    LOAD  x28, 28 * REGBYTES(sp)
-    LOAD  x29, 29 * REGBYTES(sp)
-    LOAD  x30, 30 * REGBYTES(sp)
-    LOAD  x31, 31 * REGBYTES(sp)
-
-    addi  sp, sp, 32 * REGBYTES
-    mret
+    j rt_hw_context_switch_exit