Make 'qemu-vexpress-a9' and 'imx6ul' use the same libcpu code.

libcpu/arm/cortex-a/start_gcc.S

@@ -6,8 +6,12 @@
  * Change Logs:
  * Date           Author       Notes
  * 2013-07-05     Bernard      the first version
+ * 2018-11-22     Jesven       in the interrupt context, use rt_scheduler_do_irq_switch checks
+ *                             and switches to a new thread
  */
 
+#include "rtconfig.h"
+
 .equ Mode_USR,        0x10
 .equ Mode_FIQ,        0x11
 .equ Mode_IRQ,        0x12
@@ -20,11 +24,11 @@
 .equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
 
 .equ UND_Stack_Size,     0x00000000
-.equ SVC_Stack_Size,     0x00000100
+.equ SVC_Stack_Size,     0x00000400
 .equ ABT_Stack_Size,     0x00000000
 .equ RT_FIQ_STACK_PGSZ,  0x00000000
-.equ RT_IRQ_STACK_PGSZ,  0x00000100
-.equ USR_Stack_Size,     0x00000100
+.equ RT_IRQ_STACK_PGSZ,  0x00000800
+.equ USR_Stack_Size,     0x00000400
 
 #define ISR_Stack_Size  (UND_Stack_Size + SVC_Stack_Size + ABT_Stack_Size + \
                  RT_FIQ_STACK_PGSZ + RT_IRQ_STACK_PGSZ)
@@ -44,12 +48,8 @@ stack_top:
 /* reset entry */
 .globl _reset
 _reset:
-    bl      rt_cpu_mmu_disable
     /* set the cpu to SVC32 mode and disable interrupt */
-    mrs     r0, cpsr
-    bic     r0, r0, #0x1f
-    orr     r0, r0, #0x13
-    msr     cpsr_c, r0
+    cps #Mode_SVC
 
     /* setup stack */
     bl      stack_setup
@@ -64,6 +64,20 @@ bss_loop:
     strlo   r0,[r1],#4              /* clear 4 bytes                    */
     blo     bss_loop                /* loop until done                  */
 
+#ifdef RT_USING_SMP
+    mrc p15, 0, r1, c1, c0, 1
+    mov r0, #(1<<6)
+    orr r1, r0
+    mcr p15, 0, r1, c1, c0, 1 //enable smp
+#endif
+
+    /* initialize the mmu table and enable mmu */
+    ldr r0, =platform_mem_desc
+    ldr r1, =platform_mem_desc_size
+    ldr r1, [r1]
+    bl rt_hw_init_mmu_table
+    bl rt_hw_mmu_init
+
     /* call C++ constructors of global objects                          */
     ldr     r0, =__ctors_start__
     ldr     r1, =__ctors_end__
@@ -137,12 +151,22 @@ vector_fiq:
     .align  5
 .globl vector_irq
 vector_irq:
+#ifdef RT_USING_SMP
+    clrex
+#endif
     stmfd   sp!, {r0-r12,lr}
 
     bl      rt_interrupt_enter
     bl      rt_hw_trap_irq
     bl      rt_interrupt_leave
 
+#ifdef RT_USING_SMP
+    mov     r0, sp
+    bl      rt_scheduler_do_irq_switch
+
+    ldmfd   sp!, {r0-r12,lr}
+    subs    pc,  lr, #4
+#else
     @ 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
@@ -174,6 +198,11 @@ rt_hw_context_switch_interrupt_do:
     stmfd   sp!, {r1-r4}    @ push old task's r0-r3
     stmfd   sp!, {r0}       @ push old task's cpsr
 
+#ifdef RT_USING_LWP
+    stmfd sp, {r13, r14}^  @push usr_sp, usr_lr
+    sub sp, #8
+#endif
+
     ldr     r4,  =rt_interrupt_from_thread
     ldr     r5,  [r4]
     str     sp,  [r5]       @ store sp in preempted tasks's TCB
@@ -182,11 +211,18 @@ rt_hw_context_switch_interrupt_do:
     ldr     r6,  [r6]
     ldr     sp,  [r6]       @ get new task's stack pointer
 
+#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
 
+#endif
+
 .macro push_svc_reg
     sub     sp, sp, #17 * 4         @/* Sizeof(struct rt_hw_exp_stack)  */
     stmia   sp, {r0 - r12}          @/* Calling r0-r12                  */
@@ -200,36 +236,86 @@ rt_hw_context_switch_interrupt_do:
 .endm
 
     .align  5
-    .globl	vector_swi
+    .globl  vector_swi
+.weak SVC_Handler
+SVC_Handler:
 vector_swi:
     push_svc_reg
     bl      rt_hw_trap_swi
     b       .
 
     .align  5
-    .globl	vector_undef
+    .globl  vector_undef
 vector_undef:
     push_svc_reg
     bl      rt_hw_trap_undef
     b       .
 
     .align  5
-    .globl	vector_pabt
+    .globl  vector_pabt
 vector_pabt:
     push_svc_reg
     bl      rt_hw_trap_pabt
     b       .
 
     .align  5
-    .globl	vector_dabt
+    .globl  vector_dabt
 vector_dabt:
     push_svc_reg
     bl      rt_hw_trap_dabt
     b       .
 
     .align  5
-    .globl	vector_resv
+    .globl  vector_resv
 vector_resv:
     push_svc_reg
     bl      rt_hw_trap_resv
     b       .
+
+#ifdef RT_USING_SMP
+.global set_secondary_cpu_boot_address
+set_secondary_cpu_boot_address:
+    ldr r0, =secondary_cpu_start
+
+    mvn r1, #0 //0xffffffff
+    ldr r2, =0x10000034
+    str r1, [r2]
+    str r0, [r2, #-4]
+    mov pc, lr
+
+.global secondary_cpu_start
+secondary_cpu_start:
+    mrc p15, 0, r1, c1, c0, 1
+    mov r0, #(1<<6)
+    orr r1, r0
+    mcr p15, 0, r1, c1, c0, 1 //enable smp
+
+    mrc p15, 0, r0, c1, c0, 0
+    bic r0, #(1<<13)
+    mcr p15, 0, r0, c1, c0, 0
+
+    cps #Mode_IRQ
+    ldr sp, =irq_stack_2_limit
+
+    cps #Mode_FIQ
+    ldr sp, =irq_stack_2_limit
+
+    cps #Mode_SVC
+    ldr sp, =svc_stack_2_limit
+
+    /* initialize the mmu table and enable mmu */
+    bl rt_hw_mmu_init
+
+    b secondary_cpu_c_start
+#endif
+
+.bss
+.align 2   //align to  2~2=4
+svc_stack_2:
+    .space (1 << 10)
+svc_stack_2_limit:
+
+irq_stack_2:
+    .space (1 << 10)
+irq_stack_2_limit:
+