diff --git a/arch/arm64/core/boot.h b/arch/arm64/core/boot.h new file mode 100644 index 00000000000..aa9e4526bba --- /dev/null +++ b/arch/arm64/core/boot.h @@ -0,0 +1,26 @@ +/* + * Copyright (c) 2019 Carlo Caione + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @file + * @brief Definitions for boot code + */ + +#ifndef _BOOT_H_ +#define _BOOT_H_ + +#ifndef _ASMLANGUAGE + +extern void *_vector_table[]; +extern void __start(void); + +#endif /* _ASMLANGUAGE */ + +/* Offsets into the boot_params structure */ +#define BOOT_PARAM_MPID_OFFSET 0 +#define BOOT_PARAM_SP_OFFSET 8 + +#endif /* _BOOT_H_ */ diff --git a/arch/arm64/core/reset.S b/arch/arm64/core/reset.S index dac2f5e9713..a29e19314a8 100644 --- a/arch/arm64/core/reset.S +++ b/arch/arm64/core/reset.S @@ -7,7 +7,7 @@ #include #include #include -#include "vector_table.h" +#include "boot.h" #include "macro_priv.inc" _ASM_FILE_PROLOGUE @@ -16,7 +16,7 @@ _ASM_FILE_PROLOGUE * Platform specific pre-C init code * * Note: - Stack is not yet available - * - x23 must be preserved + * - x23, x24 and x25 must be preserved */ WTEXT(z_arm64_el3_plat_prep_c) @@ -75,33 +75,11 @@ out: /* Select SP_EL0 */ msr SPSel, #0 -#if CONFIG_MP_NUM_CPUS > 1 - get_cpu_id x0 - cbnz x0, L_secondary_stack -#endif - /* Initialize stack */ - ldr x0, =(z_interrupt_stacks) - add x0, x0, #(CONFIG_ISR_STACK_SIZE) - mov sp, x0 + mov sp, x24 ret x23 -#if CONFIG_MP_NUM_CPUS > 1 -L_secondary_stack: - get_cpu_id x1 - adr x0, arm64_cpu_init - mov x2, #ARM64_CPU_INIT_SIZE - madd x0, x1, x2, x0 - ldr x0, [x0] - cbz x0, L_enable_secondary - dmb ld - - mov sp, x0 - - ret x23 -#endif - /* * Reset vector * @@ -118,6 +96,33 @@ SECTION_SUBSEC_FUNC(TEXT,_reset_section,__start) /* Mask all exceptions */ msr DAIFSet, #0xf +#if CONFIG_MP_NUM_CPUS > 1 + + ldr x0, =arm64_cpu_boot_params + get_cpu_id x1 + ldr x2, [x0, #BOOT_PARAM_MPID_OFFSET] + cmp x2, #-1 + beq primary_core + + /* loop until our turn comes */ +1: dmb ld + ldr x2, [x0] + cmp x1, x2 + bne 1b + + /* we can now load our stack pointer value and move on */ + ldr x24, [x0, #BOOT_PARAM_SP_OFFSET] + ldr x25, =z_arm64_secondary_prep_c + b 2f + +primary_core: + /* advertise ourself */ + str x1, [x0, #BOOT_PARAM_MPID_OFFSET] +#endif + /* load primary stack and entry point */ + ldr x24, =(z_interrupt_stacks + CONFIG_ISR_STACK_SIZE) + ldr x25, =z_arm64_prep_c +2: /* Prepare for calling C code */ bl __reset_prep_c @@ -155,14 +160,4 @@ switch_el: msr DAIFClr, #(DAIFCLR_ABT_BIT) isb -#if CONFIG_MP_NUM_CPUS > 1 - get_cpu_id x0 - cbnz x0, L_enable_secondary -#endif - - b z_arm64_prep_c - -#if CONFIG_MP_NUM_CPUS > 1 -L_enable_secondary: - b z_arm64_secondary_prep_c -#endif + ret x25 /* either z_arm64_prep_c or z_arm64_secondary_prep_c */ diff --git a/arch/arm64/core/reset.c b/arch/arm64/core/reset.c index b643aff3ed1..6e362747260 100644 --- a/arch/arm64/core/reset.c +++ b/arch/arm64/core/reset.c @@ -5,7 +5,7 @@ */ #include -#include "vector_table.h" +#include "boot.h" void z_arm64_el2_init(void); diff --git a/arch/arm64/core/smp.c b/arch/arm64/core/smp.c index 0150fe5097a..9960525031f 100644 --- a/arch/arm64/core/smp.c +++ b/arch/arm64/core/smp.c @@ -12,6 +12,7 @@ #include #include +#include #include #include #include @@ -22,48 +23,98 @@ #include #include #include +#include "boot.h" #define SGI_SCHED_IPI 0 #define SGI_PTABLE_IPI 1 -volatile struct { - void *sp; /* Fixed at the first entry */ +struct boot_params { + uint64_t mpid; + char *sp; arch_cpustart_t fn; void *arg; -} __aligned(L1_CACHE_BYTES) arm64_cpu_init[CONFIG_MP_NUM_CPUS]; + int cpu_num; +}; -extern void __start(void); +/* Offsets used in reset.S */ +BUILD_ASSERT(offsetof(struct boot_params, mpid) == BOOT_PARAM_MPID_OFFSET); +BUILD_ASSERT(offsetof(struct boot_params, sp) == BOOT_PARAM_SP_OFFSET); + +volatile struct boot_params __aligned(L1_CACHE_BYTES) arm64_cpu_boot_params = { + .mpid = -1, +}; + +#define CPU_REG_ID(cpu_node_id) DT_REG_ADDR(cpu_node_id), + +static const uint64_t cpu_node_list[] = { + DT_FOREACH_CHILD(DT_PATH(cpus), CPU_REG_ID) +}; /* Called from Zephyr initialization */ void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz, arch_cpustart_t fn, void *arg) { - __ASSERT(sizeof(arm64_cpu_init[0]) == ARM64_CPU_INIT_SIZE, - "ARM64_CPU_INIT_SIZE != sizeof(arm64_cpu_init[0]\n"); + int cpu_count, i, j; + uint64_t cpu_mpid, master_core_mpid; - arm64_cpu_init[cpu_num].fn = fn; - arm64_cpu_init[cpu_num].arg = arg; - arm64_cpu_init[cpu_num].sp = - (void *)(Z_THREAD_STACK_BUFFER(stack) + sz); + /* Now it is on master core */ + master_core_mpid = MPIDR_TO_CORE(GET_MPIDR()); + __ASSERT(arm64_cpu_boot_params.mpid == master_core_mpid, ""); - arch_dcache_range((void *)&arm64_cpu_init[cpu_num], - sizeof(arm64_cpu_init[cpu_num]), K_CACHE_WB_INVD); + cpu_count = ARRAY_SIZE(cpu_node_list); + __ASSERT(cpu_count == CONFIG_MP_NUM_CPUS, + "The count of CPU Cores nodes in dts is not equal to CONFIG_MP_NUM_CPUS\n"); - /* TODO: get mpidr from device tree, using cpu_num */ - if (pm_cpu_on(cpu_num, (uint64_t)&__start)) - printk("Failed to boot CPU%d\n", cpu_num); + for (i = 0, j = 0; i < cpu_count; i++) { + if (cpu_node_list[i] == master_core_mpid) { + continue; + } + if (j == cpu_num - 1) { + cpu_mpid = cpu_node_list[i]; + break; + } + j++; + } + if (i == cpu_count) { + printk("Can't find CPU Core %d from dts and failed to boot it\n", cpu_num); + return; + } + + arm64_cpu_boot_params.sp = Z_THREAD_STACK_BUFFER(stack) + sz; + arm64_cpu_boot_params.fn = fn; + arm64_cpu_boot_params.arg = arg; + arm64_cpu_boot_params.cpu_num = cpu_num; + + dsb(); + + /* store mpid last as this is our synchronization point */ + arm64_cpu_boot_params.mpid = cpu_mpid; + + arch_dcache_range((void *)&arm64_cpu_boot_params, + sizeof(arm64_cpu_boot_params), + K_CACHE_WB_INVD); + + if (pm_cpu_on(cpu_mpid, (uint64_t)&__start)) { + printk("Failed to boot secondary CPU core %d (MPID:%#llx)\n", + cpu_num, cpu_mpid); + return; + } /* Wait secondary cores up, see z_arm64_secondary_start */ - while (arm64_cpu_init[cpu_num].fn) { + while (arm64_cpu_boot_params.fn) { wfe(); } + printk("Secondary CPU core %d (MPID:%#llx) is up\n", cpu_num, cpu_mpid); } /* the C entry of secondary cores */ void z_arm64_secondary_start(void) { + int cpu_num = arm64_cpu_boot_params.cpu_num; arch_cpustart_t fn; - int cpu_num = MPIDR_TO_CORE(GET_MPIDR()); + void *arg; + + __ASSERT(arm64_cpu_boot_params.mpid == MPIDR_TO_CORE(GET_MPIDR()), ""); /* Initialize tpidrro_el0 with our struct _cpu instance address */ write_tpidrro_el0((uintptr_t)&_kernel.cpus[cpu_num]); @@ -79,18 +130,20 @@ void z_arm64_secondary_start(void) #endif #endif - fn = arm64_cpu_init[cpu_num].fn; + fn = arm64_cpu_boot_params.fn; + arg = arm64_cpu_boot_params.arg; + dsb(); /* * Secondary core clears .fn to announce its presence. - * Primary core is polling for this. + * Primary core is polling for this. We no longer own + * arm64_cpu_boot_params afterwards. */ - arm64_cpu_init[cpu_num].fn = NULL; - + arm64_cpu_boot_params.fn = NULL; dsb(); sev(); - fn(arm64_cpu_init[cpu_num].arg); + fn(arg); } #ifdef CONFIG_SMP diff --git a/arch/arm64/core/vector_table.S b/arch/arm64/core/vector_table.S index 4aa47c7ad25..29c85343ba7 100644 --- a/arch/arm64/core/vector_table.S +++ b/arch/arm64/core/vector_table.S @@ -13,7 +13,6 @@ #include #include #include -#include "vector_table.h" _ASM_FILE_PROLOGUE @@ -104,8 +103,10 @@ _ASM_FILE_PROLOGUE * +------------------+------------------+-------------------------+ */ - /* The whole table must be 2K aligned */ +GDATA(_vector_table) SECTION_SUBSEC_FUNC(exc_vector_table,_vector_table_section,_vector_table) + + /* The whole table must be 2K aligned */ .align 11 /* Current EL with SP0 / Synchronous */ diff --git a/arch/arm64/core/vector_table.h b/arch/arm64/core/vector_table.h deleted file mode 100644 index 35699d5d896..00000000000 --- a/arch/arm64/core/vector_table.h +++ /dev/null @@ -1,47 +0,0 @@ -/* - * Copyright (c) 2019 Carlo Caione - * - * SPDX-License-Identifier: Apache-2.0 - */ - -/** - * @file - * @brief Definitions for the boot vector table - * - * - * Definitions for the boot vector table. - * - * System exception handler names all have the same format: - * - * __ - * - * No other symbol has the same format, so they are easy to spot. - */ - -#ifndef _VECTOR_TABLE_H_ -#define _VECTOR_TABLE_H_ - -#ifdef _ASMLANGUAGE - -#include -#include - -GTEXT(__start) -GDATA(_vector_table) -GTEXT(_isr_wrapper) - -#else /* _ASMLANGUAGE */ - -#ifdef __cplusplus -extern "C" { -#endif - -extern void *_vector_table[]; - -#ifdef __cplusplus -} -#endif - -#endif /* _ASMLANGUAGE */ - -#endif /* _VECTOR_TABLE_H_ */