arm64: decrustify and extend SMP boot code
The SMP boot code depends on physical CPU #0 to be first to boot and subsequent CPUs to follow suit in a linear fashion. Let's decouple physical and logical numbering so that any physical CPU can be the boot CPU. This is based on a prior code proposal from Jiafei Pan <Jiafei.Pan@nxp.com>. This, however, was about to turn the boot code into some hairy mess. So let's clean things up and simplify the code as well while at it. Both the extension and the clean up aren't separate commits because they actually depend on each other. The BOOT_PARAM_*_OFFSET defines are locally hardcoded as there is no point exposing the related structure widely. Build time assertions ensure they don't go out of sync with the struct definition. And vector_table.h is repurposed into boot.h to gather boot related definitions. Signed-off-by: Nicolas Pitre <npitre@baylibre.com> Signed-off-by: Jiafei Pan <Jiafei.Pan@nxp.com>
This commit is contained in:
Nicolas Pitre
Anas Nashif
parent
7889364771
commit
29c8e9bf66
6 changed files with 136 additions and 108 deletions
26
arch/arm64/core/boot.h
Normal file
26
arch/arm64/core/boot.h
Normal file
|
|
@ -0,0 +1,26 @@
|
|||
/*
|
||||
* Copyright (c) 2019 Carlo Caione <ccaione@baylibre.com>
|
||||
*
|
||||
* 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_ */
|
||||
|
|
@ -7,7 +7,7 @@
|
|||
#include <toolchain.h>
|
||||
#include <linker/sections.h>
|
||||
#include <arch/cpu.h>
|
||||
#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 */
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@
|
|||
*/
|
||||
|
||||
#include <kernel_internal.h>
|
||||
#include "vector_table.h"
|
||||
#include "boot.h"
|
||||
|
||||
void z_arm64_el2_init(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
|
||||
#include <cache.h>
|
||||
#include <device.h>
|
||||
#include <devicetree.h>
|
||||
#include <kernel.h>
|
||||
#include <kernel_structs.h>
|
||||
#include <ksched.h>
|
||||
|
|
@ -22,48 +23,98 @@
|
|||
#include <drivers/interrupt_controller/gic.h>
|
||||
#include <drivers/pm_cpu_ops.h>
|
||||
#include <sys/arch_interface.h>
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
#include <offsets.h>
|
||||
#include <arch/cpu.h>
|
||||
#include <arch/arm64/tpidrro_el0.h>
|
||||
#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 */
|
||||
|
|
|
|||
|
|
@ -1,47 +0,0 @@
|
|||
/*
|
||||
* Copyright (c) 2019 Carlo Caione <ccaione@baylibre.com>
|
||||
*
|
||||
* 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:
|
||||
*
|
||||
* __<exception name with underscores>
|
||||
*
|
||||
* No other symbol has the same format, so they are easy to spot.
|
||||
*/
|
||||
|
||||
#ifndef _VECTOR_TABLE_H_
|
||||
#define _VECTOR_TABLE_H_
|
||||
|
||||
#ifdef _ASMLANGUAGE
|
||||
|
||||
#include <toolchain.h>
|
||||
#include <linker/sections.h>
|
||||
|
||||
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_ */
|
||||
Loading…
Add table
Add a link
Reference in a new issue