michaelh/zephyr
1
0
Fork 0
Code Releases Activity

arch: cortex_ar: Introduce SMP support into Cortex-A/R aarch32

Browse source 
This commit introduces SMP support into Cortex-A/R aarch32 architecture.

For now, this only supports multiple core start together and only allow
one CPU initialize system as primary core, others loop at the beginning
as the secondary cores and wait for wake up.

Signed-off-by: Huifeng Zhang <Huifeng.Zhang@arm.com>
This commit is contained in:
Huifeng Zhang 2023-07-13 13:06:14 +08:00 • committed by Maureen Helm
commit e898dafb38
8 changed files with 372 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

1
arch/arm/core/cortex_a_r/CMakeLists.txt
View file

@ -15,6 +15,7 @@ zephyr_library_sources(
prep_c.c
thread.c
cpu_idle.S
smp.c
)
zephyr_library_sources_ifdef(CONFIG_GEN_SW_ISR_TABLE isr_wrapper.S)

4
arch/arm/core/cortex_a_r/Kconfig
View file

@ -130,6 +130,7 @@ config ARMV7_R_FP
config AARCH32_ARMV8_R
bool
select ATOMIC_OPERATIONS_BUILTIN
select SCHED_IPI_SUPPORTED if SMP
help
This option signifies the use of an ARMv8-R AArch32 processor
implementation.
@ -188,3 +189,6 @@ config ICACHE_LINE_SIZE
default 32
endif # CPU_AARCH32_CORTEX_R
config TEST_EXTRA_STACK_SIZE
default 1024 if SMP

30
arch/arm/core/cortex_a_r/boot.h Normal file
View file

@ -0,0 +1,30 @@
/*
* Copyright (c) 2023 Arm Limited (or its affiliates). All rights reserved.
* 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_IRQ_SP_OFFSET 4
#define BOOT_PARAM_FIQ_SP_OFFSET 8
#define BOOT_PARAM_ABT_SP_OFFSET 12
#define BOOT_PARAM_UDF_SP_OFFSET 16
#define BOOT_PARAM_SVC_SP_OFFSET 20
#define BOOT_PARAM_SYS_SP_OFFSET 24
#endif /* _BOOT_H_ */

10
arch/arm/core/cortex_a_r/macro_priv.inc
View file

@ -8,6 +8,16 @@
#include <zephyr/arch/arm/cortex_a_r/tpidruro.h>
/*
* Get CPU id
*/
.macro get_cpu_id rreg0
/* Read MPIDR register */
mrc p15, 0, \rreg0, c0, c0, 5
ubfx \rreg0, \rreg0, #0, #24
.endm
.macro get_cpu rreg0
/*
* Get CPU pointer.

56
arch/arm/core/cortex_a_r/reset.S
View file

@ -18,6 +18,8 @@
#include <offsets_short.h>
#include <cortex_a_r/tcm.h>
#include "vector_table.h"
#include "boot.h"
#include "macro_priv.inc"
_ASM_FILE_PROLOGUE
@ -76,6 +78,7 @@ SECTION_SUBSEC_FUNC(TEXT, _reset_section, __start)
EL1_Reset_Handler:
#endif
#if defined(CONFIG_DCLS)
/*
* Initialise CPU registers to a defined state if the processor is
@ -196,33 +199,72 @@ EL1_Reset_Handler:
#endif /* CONFIG_DCLS */
ldr r0, =arm_cpu_boot_params
#if CONFIG_MP_MAX_NUM_CPUS > 1
get_cpu_id r1
ldrex r2, [r0, #BOOT_PARAM_MPID_OFFSET]
cmp r2, #-1
bne 1f
strex r3, r1, [r0, #BOOT_PARAM_MPID_OFFSET]
cmp r3, #0
beq _primary_core
1:
dmb ld
ldr r2, [r0, #BOOT_PARAM_MPID_OFFSET]
cmp r1, r2
bne 1b
/* we can now move on */
ldr r4, =z_arm_secondary_start
ldr r5, [r0, #BOOT_PARAM_FIQ_SP_OFFSET]
ldr r6, [r0, #BOOT_PARAM_IRQ_SP_OFFSET]
ldr r7, [r0, #BOOT_PARAM_ABT_SP_OFFSET]
ldr r8, [r0, #BOOT_PARAM_UDF_SP_OFFSET]
ldr r9, [r0, #BOOT_PARAM_SVC_SP_OFFSET]
ldr r10, [r0, #BOOT_PARAM_SYS_SP_OFFSET]
b 2f
_primary_core:
#endif
ldr r4, =z_arm_prep_c
ldr r5, =(z_arm_fiq_stack + CONFIG_ARMV7_FIQ_STACK_SIZE)
ldr r6, =(z_interrupt_stacks + CONFIG_ISR_STACK_SIZE)
ldr r7, =(z_arm_abort_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE)
ldr r8, =(z_arm_undef_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE)
ldr r9, =(z_arm_svc_stack + CONFIG_ARMV7_SVC_STACK_SIZE)
ldr r10, =(z_arm_sys_stack + CONFIG_ARMV7_SYS_STACK_SIZE)
2:
/*
* Configure stack.
*/
/* FIQ mode stack */
msr CPSR_c, #(MODE_FIQ | I_BIT | F_BIT)
ldr sp, =(z_arm_fiq_stack + CONFIG_ARMV7_FIQ_STACK_SIZE)
mov sp, r5
/* IRQ mode stack */
msr CPSR_c, #(MODE_IRQ | I_BIT | F_BIT)
ldr sp, =(z_interrupt_stacks + CONFIG_ISR_STACK_SIZE)
mov sp, r6
/* ABT mode stack */
msr CPSR_c, #(MODE_ABT | I_BIT | F_BIT)
ldr sp, =(z_arm_abort_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE)
mov sp, r7
/* UND mode stack */
msr CPSR_c, #(MODE_UND | I_BIT | F_BIT)
ldr sp, =(z_arm_undef_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE)
mov sp, r8
/* SVC mode stack */
msr CPSR_c, #(MODE_SVC | I_BIT | F_BIT)
ldr sp, =(z_arm_svc_stack + CONFIG_ARMV7_SVC_STACK_SIZE)
mov sp, r9
/* SYS mode stack */
msr CPSR_c, #(MODE_SYS | I_BIT | F_BIT)
ldr sp, =(z_arm_sys_stack + CONFIG_ARMV7_SYS_STACK_SIZE)
mov sp, r10
#if defined(CONFIG_PLATFORM_SPECIFIC_INIT)
/* Execute platform-specific initialisation if applicable */
@ -238,4 +280,4 @@ EL1_Reset_Handler:
bl z_arm_tcm_disable_ecc
#endif
b z_arm_prep_c
bx r4

264
arch/arm/core/cortex_a_r/smp.c Normal file
View file

@ -0,0 +1,264 @@
/*
* Copyright (c) 2023 Arm Limited (or its affiliates). All rights reserved.
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel/thread_stack.h>
#include <zephyr/kernel.h>
#include <zephyr/arch/arm/cortex_a_r/lib_helpers.h>
#include <zephyr/drivers/interrupt_controller/gic.h>
#include "boot.h"
#include "zephyr/cache.h"
#include "zephyr/kernel/thread_stack.h"
#include "zephyr/toolchain/gcc.h"
#define INV_MPID UINT32_MAX
#define SGI_SCHED_IPI 0
#define SGI_MMCFG_IPI 1
#define SGI_FPU_IPI 2
K_KERNEL_PINNED_STACK_ARRAY_DECLARE(z_interrupt_stacks,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ISR_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DECLARE(z_arm_fiq_stack,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_FIQ_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DECLARE(z_arm_abort_stack,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DECLARE(z_arm_undef_stack,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DECLARE(z_arm_svc_stack,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_SVC_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DECLARE(z_arm_sys_stack,
CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_SVC_STACK_SIZE);
struct boot_params {
uint32_t mpid;
char *irq_sp;
char *fiq_sp;
char *abt_sp;
char *udf_sp;
char *svc_sp;
char *sys_sp;
arch_cpustart_t fn;
void *arg;
int cpu_num;
};
/* Offsets used in reset.S */
BUILD_ASSERT(offsetof(struct boot_params, mpid) == BOOT_PARAM_MPID_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, irq_sp) == BOOT_PARAM_IRQ_SP_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, fiq_sp) == BOOT_PARAM_FIQ_SP_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, abt_sp) == BOOT_PARAM_ABT_SP_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, udf_sp) == BOOT_PARAM_UDF_SP_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, svc_sp) == BOOT_PARAM_SVC_SP_OFFSET);
BUILD_ASSERT(offsetof(struct boot_params, sys_sp) == BOOT_PARAM_SYS_SP_OFFSET);
volatile struct boot_params arm_cpu_boot_params = {
.mpid = -1,
.irq_sp = (char *)(z_interrupt_stacks + CONFIG_ISR_STACK_SIZE),
.fiq_sp = (char *)(z_arm_fiq_stack + CONFIG_ARMV7_FIQ_STACK_SIZE),
.abt_sp = (char *)(z_arm_abort_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE),
.udf_sp = (char *)(z_arm_undef_stack + CONFIG_ARMV7_EXCEPTION_STACK_SIZE),
.svc_sp = (char *)(z_arm_svc_stack + CONFIG_ARMV7_SVC_STACK_SIZE),
.sys_sp = (char *)(z_arm_sys_stack + CONFIG_ARMV7_SYS_STACK_SIZE),
};
static const uint32_t cpu_node_list[] = {
DT_FOREACH_CHILD_STATUS_OKAY_SEP(DT_PATH(cpus), DT_REG_ADDR, (,))};
/* cpu_map saves the maping of core id and mpid */
static uint32_t cpu_map[CONFIG_MP_MAX_NUM_CPUS] = {
[0 ... (CONFIG_MP_MAX_NUM_CPUS - 1)] = INV_MPID
};
#ifdef CONFIG_ARM_MPU
extern void z_arm_mpu_init(void);
extern void z_arm_configure_static_mpu_regions(void);
#elif defined(CONFIG_ARM_AARCH32_MMU)
extern int z_arm_mmu_init(void);
#endif
/* Called from Zephyr initialization */
void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz, arch_cpustart_t fn, void *arg)
{
int cpu_count, i, j;
uint32_t cpu_mpid = 0;
uint32_t master_core_mpid;
/* Now it is on master core */
__ASSERT(arch_curr_cpu()->id == 0, "");
master_core_mpid = MPIDR_TO_CORE(GET_MPIDR());
cpu_count = ARRAY_SIZE(cpu_node_list);
__ASSERT(cpu_count == CONFIG_MP_MAX_NUM_CPUS,
"The count of CPU Cores nodes in dts is not equal to CONFIG_MP_MAX_NUM_CPUS\n");
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;
}
/* Pass stack address to secondary core */
arm_cpu_boot_params.irq_sp = Z_KERNEL_STACK_BUFFER(stack) + sz;
arm_cpu_boot_params.fiq_sp = Z_KERNEL_STACK_BUFFER(z_arm_fiq_stack[cpu_num])
+ CONFIG_ARMV7_FIQ_STACK_SIZE;
arm_cpu_boot_params.abt_sp = Z_KERNEL_STACK_BUFFER(z_arm_abort_stack[cpu_num])
+ CONFIG_ARMV7_EXCEPTION_STACK_SIZE;
arm_cpu_boot_params.udf_sp = Z_KERNEL_STACK_BUFFER(z_arm_undef_stack[cpu_num])
+ CONFIG_ARMV7_EXCEPTION_STACK_SIZE;
arm_cpu_boot_params.svc_sp = Z_KERNEL_STACK_BUFFER(z_arm_svc_stack[cpu_num])
+ CONFIG_ARMV7_SVC_STACK_SIZE;
arm_cpu_boot_params.sys_sp = Z_KERNEL_STACK_BUFFER(z_arm_sys_stack[cpu_num])
+ CONFIG_ARMV7_SYS_STACK_SIZE;
arm_cpu_boot_params.fn = fn;
arm_cpu_boot_params.arg = arg;
arm_cpu_boot_params.cpu_num = cpu_num;
/* store mpid last as this is our synchronization point */
arm_cpu_boot_params.mpid = cpu_mpid;
barrier_dsync_fence_full();
sys_cache_data_invd_range(
(void *)&arm_cpu_boot_params,
sizeof(arm_cpu_boot_params));
/*! TODO: Support PSCI
* \todo Support PSCI
*/
/* Wait secondary cores up, see z_arm64_secondary_start */
while (arm_cpu_boot_params.fn) {
wfe();
}
cpu_map[cpu_num] = cpu_mpid;
printk("Secondary CPU core %d (MPID:%#x) is up\n", cpu_num, cpu_mpid);
}
/* the C entry of secondary cores */
void z_arm_secondary_start(void)
{
int cpu_num = arm_cpu_boot_params.cpu_num;
arch_cpustart_t fn;
void *arg;
__ASSERT(arm_cpu_boot_params.mpid == MPIDR_TO_CORE(GET_MPIDR()), "");
/* Initialize tpidrro_el0 with our struct _cpu instance address */
write_tpidruro((uintptr_t)&_kernel.cpus[cpu_num]);
#ifdef CONFIG_ARM_MPU
/*! TODO: Unify mpu and mmu initialization function
* \todo Unify mpu and mmu initialization function
*/
z_arm_mpu_init();
z_arm_configure_static_mpu_regions();
#elif defined(CONFIG_ARM_AARCH32_MMU)
z_arm_mmu_init();
#endif
#ifdef CONFIG_SMP
arm_gic_secondary_init();
irq_enable(SGI_SCHED_IPI);
/*! TODO: FPU irq
* \todo FPU irq
*/
#endif
fn = arm_cpu_boot_params.fn;
arg = arm_cpu_boot_params.arg;
barrier_dsync_fence_full();
/*
* Secondary core clears .fn to announce its presence.
* Primary core is polling for this. We no longer own
* arm_cpu_boot_params afterwards.
*/
arm_cpu_boot_params.fn = NULL;
barrier_dsync_fence_full();
sev();
fn(arg);
}
#ifdef CONFIG_SMP
static void broadcast_ipi(unsigned int ipi)
{
uint32_t mpidr = MPIDR_TO_CORE(GET_MPIDR());
/*
* Send SGI to all cores except itself
*/
unsigned int num_cpus = arch_num_cpus();
for (int i = 0; i < num_cpus; i++) {
uint32_t target_mpidr = cpu_map[i];
uint8_t aff0;
if (mpidr == target_mpidr || mpidr == INV_MPID) {
continue;
}
aff0 = MPIDR_AFFLVL(target_mpidr, 0);
gic_raise_sgi(ipi, (uint64_t)target_mpidr, 1 << aff0);
}
}
void sched_ipi_handler(const void *unused)
{
ARG_UNUSED(unused);
z_sched_ipi();
}
/* arch implementation of sched_ipi */
void arch_sched_ipi(void)
{
broadcast_ipi(SGI_SCHED_IPI);
}
static int arm_smp_init(void)
{
cpu_map[0] = MPIDR_TO_CORE(GET_MPIDR());
/*
* SGI0 is use for sched ipi, this might be changed to use Kconfig
* option
*/
IRQ_CONNECT(SGI_SCHED_IPI, IRQ_DEFAULT_PRIORITY, sched_ipi_handler, NULL, 0);
irq_enable(SGI_SCHED_IPI);
return 0;
}
SYS_INIT(arm_smp_init, PRE_KERNEL_2, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);
#endif

16
arch/arm/core/cortex_a_r/stacks.c
View file

@ -4,16 +4,22 @@
* SPDX-License-Identifier: Apache-2.0
*/
#include "zephyr/kernel/thread_stack.h"
#include <zephyr/kernel.h>
#include <cortex_a_r/stack.h>
#include <string.h>
#include <kernel_internal.h>
K_KERNEL_STACK_DEFINE(z_arm_fiq_stack, CONFIG_ARMV7_FIQ_STACK_SIZE);
K_KERNEL_STACK_DEFINE(z_arm_abort_stack, CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_DEFINE(z_arm_undef_stack, CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_DEFINE(z_arm_svc_stack, CONFIG_ARMV7_SVC_STACK_SIZE);
K_KERNEL_STACK_DEFINE(z_arm_sys_stack, CONFIG_ARMV7_SYS_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DEFINE(z_arm_fiq_stack, CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_FIQ_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DEFINE(z_arm_abort_stack, CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DEFINE(z_arm_undef_stack, CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_EXCEPTION_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DEFINE(z_arm_svc_stack, CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_SVC_STACK_SIZE);
K_KERNEL_STACK_ARRAY_DEFINE(z_arm_sys_stack, CONFIG_MP_MAX_NUM_CPUS,
CONFIG_ARMV7_SYS_STACK_SIZE);
#if defined(CONFIG_INIT_STACKS)
void z_arm_init_stacks(void)

3
include/zephyr/arch/arm/cortex_a_r/lib_helpers.h
View file

@ -99,5 +99,8 @@ MAKE_REG_HELPER(ICC_IGRPEN1_EL1, 0, 12, 12, 7);
#define write_sysreg(val, reg) write_##reg(val)
#define read_sysreg(reg) read_##reg()
#define sev() __asm__ volatile("sev" : : : "memory")
#define wfe() __asm__ volatile("wfe" : : : "memory")
#endif /* !_ASMLANGUAGE */
#endif /* ZEPHYR_INCLUDE_ARCH_ARM_AARCH32_CORTEX_A_R_LIB_HELPERS_H_ */