From 42ce237dc969fa3e6c5f891e87e925f42d5d38ea Mon Sep 17 00:00:00 2001 From: zhouji <956133287@qq.com> Date: Sat, 1 May 2021 16:06:02 +0800 Subject: [PATCH] =?UTF-8?q?[update]=20=E6=95=B4=E7=90=86cortex-a=20aarch32?= =?UTF-8?q?=E5=90=AF=E5=8A=A8=E4=BB=A3=E7=A0=81=201.=20=E5=8E=BB=E9=99=A4s?= =?UTF-8?q?tart=5Fgcc.s=E4=B8=ADset=5Fsecondary=5Fcpu=5Fboot=5Faddress?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=EF=BC=8C=E8=BF=99=E9=83=A8=E5=88=86=E6=8F=90?= =?UTF-8?q?=E5=8F=96=E5=88=B0qemu-vexpress-a9=20bsp=E4=B8=AD=E3=80=82=202.?= =?UTF-8?q?=20=E7=A7=BB=E5=8A=A8cpu.c=E4=B8=ADrt=5Fhw=5Fcpu=5Fid=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E5=88=B0cp15=5Fgcc.s=EF=BC=8C=E4=BD=BF=E7=94=A8?= =?UTF-8?q?=E6=B1=87=E7=BC=96=E5=AE=9E=E7=8E=B0=EF=BC=8C=E9=87=87=E7=94=A8?= =?UTF-8?q?wake=E5=B1=9E=E6=80=A7=EF=BC=8C=E6=96=B9=E4=BE=BFbsp=E6=A0=B9?= =?UTF-8?q?=E6=8D=AEcpu=E7=89=B9=E6=80=A7=E8=8E=B7=E5=8F=96CPU=20ID?= =?UTF-8?q?=EF=BC=88=E5=A4=9Acpu=E9=9B=86=E7=BE=A4=E4=B8=AD=EF=BC=8C?= =?UTF-8?q?=E4=B8=8D=E5=90=8C=E5=8E=82=E5=AE=B6=E4=BD=BF=E7=94=A8=E7=BB=84?= =?UTF-8?q?=E5=90=88=E4=B8=8D=E4=B8=80=E6=A0=B7=EF=BC=89.=203.=20=E6=95=B4?= =?UTF-8?q?=E7=90=86start=5Fgcc.s=20=E9=80=82=E5=BA=94=E5=A4=9A=E6=A0=B8?= =?UTF-8?q?=E5=90=AF=E5=8A=A8=EF=BC=8C=E5=8E=9F=E6=9D=A5=E7=9A=84=E4=BB=A3?= =?UTF-8?q?=E7=A0=81=E5=8F=AA=E8=80=83=E8=99=91=E5=88=B0=E5=8F=8C=E6=A0=B8?= =?UTF-8?q?=E7=9A=84=E6=83=85=E5=86=B5=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bsp/qemu-vexpress-a9/drivers/secondary_cpu.c | 10 +- libcpu/arm/cortex-a/cp15.h | 3 +- libcpu/arm/cortex-a/cp15_gcc.S | 8 +- libcpu/arm/cortex-a/cpu.c | 11 -- libcpu/arm/cortex-a/start_gcc.S | 119 +++++++++++++------ 5 files changed, 97 insertions(+), 54 deletions(-) diff --git a/bsp/qemu-vexpress-a9/drivers/secondary_cpu.c b/bsp/qemu-vexpress-a9/drivers/secondary_cpu.c index 3b0428859f..c0fcb30aa4 100644 --- a/bsp/qemu-vexpress-a9/drivers/secondary_cpu.c +++ b/bsp/qemu-vexpress-a9/drivers/secondary_cpu.c @@ -26,10 +26,16 @@ static void rt_hw_timer2_isr(int vector, void *param) timer_clear_pending(0); } +void set_secondary_cpu_boot_address(void) +{ + extern void secondary_cpu_start(void); + uint32_t *boot_address = (uint32_t *)0x10000030; + *(boot_address + 1) = ~0ul; + *boot_address = (uint32_t )&secondary_cpu_start; +} + void rt_hw_secondary_cpu_up(void) { - extern void set_secondary_cpu_boot_address(void); - set_secondary_cpu_boot_address(); __asm__ volatile ("dsb":::"memory"); rt_hw_ipi_send(0, 1 << 1); diff --git a/libcpu/arm/cortex-a/cp15.h b/libcpu/arm/cortex-a/cp15.h index 6896fd9f71..2bf91ac5ad 100644 --- a/libcpu/arm/cortex-a/cp15.h +++ b/libcpu/arm/cortex-a/cp15.h @@ -15,8 +15,7 @@ #define __get_cp64(cp, op1, Rt, CRm) __asm__ volatile("MRRC p" # cp ", " # op1 ", %Q0, %R0, c" # CRm : "=r" (Rt) : : "memory" ) #define __set_cp64(cp, op1, Rt, CRm) __asm__ volatile("MCRR p" # cp ", " # op1 ", %Q0, %R0, c" # CRm : : "r" (Rt) : "memory" ) -unsigned long rt_cpu_get_smp_id(void); - +int rt_hw_cpu_id(void); void rt_cpu_mmu_disable(void); void rt_cpu_mmu_enable(void); void rt_cpu_tlb_set(volatile unsigned long*); diff --git a/libcpu/arm/cortex-a/cp15_gcc.S b/libcpu/arm/cortex-a/cp15_gcc.S index dd2436ffee..983ae522ef 100644 --- a/libcpu/arm/cortex-a/cp15_gcc.S +++ b/libcpu/arm/cortex-a/cp15_gcc.S @@ -8,9 +8,11 @@ * 2013-07-05 Bernard the first version */ -.globl rt_cpu_get_smp_id -rt_cpu_get_smp_id: - mrc p15, #0, r0, c0, c0, #5 +.weak rt_hw_cpu_id +rt_hw_cpu_id: + mrc p15, #0, r0, c0, c0, #5 @ read multiprocessor affinity register + ldr r1, =0xFFFF03 @ Affinity mask off, leaving CPU ID field, [0:1]CPU ID, [8:15]Cluster ID Aff1, [16:23]Cluster ID Aff2 + and r0, r0, r1 bx lr .globl rt_cpu_vector_set_base diff --git a/libcpu/arm/cortex-a/cpu.c b/libcpu/arm/cortex-a/cpu.c index 9555e01e9c..7367bddd9f 100644 --- a/libcpu/arm/cortex-a/cpu.c +++ b/libcpu/arm/cortex-a/cpu.c @@ -15,17 +15,6 @@ #ifdef RT_USING_SMP -int rt_hw_cpu_id(void) -{ - int cpu_id; - __asm__ volatile ( - "mrc p15, 0, %0, c0, c0, 5" - :"=r"(cpu_id) - ); - cpu_id &= 0xf; - return cpu_id; -}; - void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock) { lock->slock = 0; diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S index 2e99db7218..bd4bc84604 100644 --- a/libcpu/arm/cortex-a/start_gcc.S +++ b/libcpu/arm/cortex-a/start_gcc.S @@ -22,20 +22,26 @@ .equ I_Bit, 0x80 @ when I bit is set, IRQ is disabled .equ F_Bit, 0x40 @ when F bit is set, FIQ is disabled -#ifdef RT_USING_FPU .equ UND_Stack_Size, 0x00000400 -#else -.equ UND_Stack_Size, 0x00000000 -#endif .equ SVC_Stack_Size, 0x00000400 -.equ ABT_Stack_Size, 0x00000000 +.equ ABT_Stack_Size, 0x00000400 .equ RT_FIQ_STACK_PGSZ, 0x00000000 .equ RT_IRQ_STACK_PGSZ, 0x00000800 .equ USR_Stack_Size, 0x00000400 +.equ SUB_UND_Stack_Size, 0x00000400 +.equ SUB_SVC_Stack_Size, 0x00000400 +.equ SUB_ABT_Stack_Size, 0x00000400 +.equ SUB_RT_FIQ_STACK_PGSZ, 0x00000000 +.equ SUB_RT_IRQ_STACK_PGSZ, 0x00000400 +.equ SUB_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) +#define SUB_ISR_Stack_Size (SUB_UND_Stack_Size + SUB_SVC_Stack_Size + SUB_ABT_Stack_Size + \ + SUB_RT_FIQ_STACK_PGSZ + SUB_RT_IRQ_STACK_PGSZ) + .section .data.share.isr /* stack */ .globl stack_start @@ -82,9 +88,40 @@ continue: /* disable the data alignment check */ mrc p15, 0, r1, c1, c0, 0 - bic r1, #(1<<1) + bic r1, #(1<<0) /* Disable MMU */ + bic r1, #(1<<1) /* Disable Alignment fault checking */ + bic r1, #(1<<2) /* Disable data cache */ + bic r1, #(1<<11) /* Disable program flow prediction */ + bic r1, #(1<<12) /* Disable instruction cache */ + bic r1, #(3<<19) /* bit[20:19] must be zero */ mcr p15, 0, r1, c1, c0, 0 + @ get cpu id, and subtract the offset from the stacks base address + bl rt_hw_cpu_id + mov r5, r0 + + cmp r5, #0 @ cpu id == 0 + beq normal_setup + + @ cpu id > 0, stop or wait +#ifdef RT_SMP_AUTO_BOOT + ldr r0, =secondary_cpu_entry + mov r1, #0 + str r1, [r0] /* clean secondary_cpu_entry */ +#endif /* RT_SMP_AUTO_BOOT */ + +secondary_loop: + @ cpu core 1 goes into sleep until core 0 wakeup it + wfe +#ifdef RT_SMP_AUTO_BOOT + ldr r1, =secondary_cpu_entry + ldr r0, [r1] + cmp r0, #0 + blxne r0 /* if(secondary_cpu_entry) secondary_cpu_entry(); */ +#endif /* RT_SMP_AUTO_BOOT */ + b secondary_loop + +normal_setup: /* setup stack */ bl stack_setup @@ -105,6 +142,11 @@ bss_loop: mcr p15, 0, r1, c1, c0, 1 //enable smp #endif + /* enable branch prediction */ + mrc p15, 0, r0, c1, c0, 0 + orr r0, r0, #(1<<11) + mcr p15, 0, r0, c1, c0, 0 + /* initialize the mmu table and enable mmu */ ldr r0, =platform_mem_desc ldr r1, =platform_mem_desc_size @@ -137,6 +179,7 @@ stack_setup: @ Set the startup stack for svc mov sp, r0 + sub r0, r0, #SVC_Stack_Size @ Enter Undefined Instruction Mode and set its Stack Pointer msr cpsr_c, #Mode_UND|I_Bit|F_Bit @@ -378,16 +421,6 @@ vector_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: @@ -405,38 +438,52 @@ secondary_cpu_start: bic r0, #(1<<13) mcr p15, 0, r0, c1, c0, 0 -#ifdef RT_USING_FPU - cps #Mode_UND - ldr sp, =und_stack_2_limit -#endif + /* enable branch prediction */ + mrc p15, 0, r0, c1, c0, 0 + orr r0, r0, #(1<<11) + mcr p15, 0, r0, c1, c0, 0 - cps #Mode_IRQ - ldr sp, =irq_stack_2_limit + @ get cpu id, and subtract the offset from the stacks base address + bl rt_hw_cpu_id + sub r5, r0, #1 - cps #Mode_FIQ - ldr sp, =irq_stack_2_limit + ldr r0, =SUB_ISR_Stack_Size + mul r0, r0, r5 @r0 = SUB_ISR_Stack_Size * (cpuid - 1) + ldr r1, =sub_stack_top + sub r0, r1, r0 @r0 = sub_stack_top - (SUB_ISR_Stack_Size * (cpuid - 1)) + + cps #Mode_SVC + mov sp, r0 + sub r0, r0, #SUB_SVC_Stack_Size + + cps #Mode_UND + mov sp, r0 + sub r0, r0, #SUB_UND_Stack_Size + + cps #Mode_ABT + mov sp, r0 + sub r0, r0, #SUB_ABT_Stack_Size + + cps #Mode_FIQ + mov sp, r0 + sub r0, r0, #SUB_RT_FIQ_STACK_PGSZ + + cps #Mode_IRQ + mov sp, r0 + sub r0, r0, #SUB_RT_IRQ_STACK_PGSZ 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: +sub_stack_start: + .space (SUB_ISR_Stack_Size * (RT_CPUS_NR-1)) +sub_stack_top: -#ifdef RT_USING_FPU -und_stack_2: - .space (1 << 10) -und_stack_2_limit: #endif