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 2021-04-16 17:45:00 -04:00 committed by Anas Nashif
commit 29c8e9bf66
6 changed files with 136 additions and 108 deletions

26
arch/arm64/core/boot.h Normal file
View 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_ */

View file

@ -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 */

View file

@ -5,7 +5,7 @@
*/
#include <kernel_internal.h>
#include "vector_table.h"
#include "boot.h"
void z_arm64_el2_init(void);

View file

@ -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

View file

@ -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 */

View file

@ -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_ */