From 3d81d7f23fa349d7fdf8a8a8d9eae73e14e6e60f Mon Sep 17 00:00:00 2001 From: Huifeng Zhang Date: Tue, 5 Jul 2022 14:13:23 +0800 Subject: [PATCH] arch: arm64: fix the wrong way to send ipi interrupt On GICv3, when we send an IPI interrupt, aff3, aff2 and aff1 should be assigned a value corespond to a PE for which interrupt will be generated. target_list only corresponds to aff0. On real hardware, aff3, aff2, aff1 and aff0 should be treated as a whole to determine a PE. Signed-off-by: Huifeng Zhang --- arch/arm64/core/smp.c | 24 ++++++++++--------- include/zephyr/arch/arm64/cpu.h | 5 ++-- .../testsuite/include/zephyr/interrupt_util.h | 5 +++- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/arch/arm64/core/smp.c b/arch/arm64/core/smp.c index ecbd8f6ccc3..9184766e80d 100644 --- a/arch/arm64/core/smp.c +++ b/arch/arm64/core/smp.c @@ -47,7 +47,6 @@ volatile struct boot_params __aligned(L1_CACHE_BYTES) arm64_cpu_boot_params = { static const uint64_t cpu_node_list[] = { DT_FOREACH_CHILD_STATUS_OKAY_SEP(DT_PATH(cpus), DT_REG_ADDR, (,)) }; -static uint16_t target_list_mask; extern void z_arm64_mm_init(bool is_primary_core); @@ -106,8 +105,6 @@ void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz, while (arm64_cpu_boot_params.fn) { wfe(); } - /* Set secondary cores bit mask */ - target_list_mask |= 1 << MPIDR_TO_CORE(cpu_mpid); printk("Secondary CPU core %d (MPID:%#llx) is up\n", cpu_num, cpu_mpid); } @@ -157,14 +154,21 @@ void z_arm64_secondary_start(void) static void broadcast_ipi(unsigned int ipi) { - const uint64_t mpidr = GET_MPIDR(); + uint64_t mpidr = MPIDR_TO_CORE(GET_MPIDR()); /* * Send SGI to all cores except itself - * Note: Assume only one Cluster now. */ - gic_raise_sgi(ipi, mpidr, target_list_mask & - ~(1 << MPIDR_TO_CORE(mpidr))); + for (int i = 0; i < CONFIG_MP_NUM_CPUS; i++) { + uint64_t target_mpidr = cpu_node_list[i]; + uint8_t aff0 = MPIDR_AFFLVL(target_mpidr, 0); + + if (mpidr == target_mpidr) { + continue; + } + + gic_raise_sgi(ipi, target_mpidr, 1 << aff0); + } } void sched_ipi_handler(const void *unused) @@ -211,8 +215,9 @@ void flush_fpu_ipi_handler(const void *unused) void z_arm64_flush_fpu_ipi(unsigned int cpu) { const uint64_t mpidr = cpu_node_list[cpu]; + uint8_t aff0 = MPIDR_AFFLVL(mpidr, 0); - gic_raise_sgi(SGI_FPU_IPI, mpidr, 1 << MPIDR_TO_CORE(mpidr)); + gic_raise_sgi(SGI_FPU_IPI, mpidr, 1 << aff0); } #endif @@ -220,9 +225,6 @@ static int arm64_smp_init(const struct device *dev) { ARG_UNUSED(dev); - /* Seting the primary core bit mask */ - target_list_mask |= 1 << MPIDR_TO_CORE(GET_MPIDR()); - /* * SGI0 is use for sched ipi, this might be changed to use Kconfig * option diff --git a/include/zephyr/arch/arm64/cpu.h b/include/zephyr/arch/arm64/cpu.h index fa532fc645b..95c9a651c1a 100644 --- a/include/zephyr/arch/arm64/cpu.h +++ b/include/zephyr/arch/arm64/cpu.h @@ -73,12 +73,13 @@ #define MPIDR_AFF2_SHIFT (16) #define MPIDR_AFF3_SHIFT (32) +#define MPIDR_AFF_MASK (GENMASK(23, 0) | GENMASK(39, 32)) + #define MPIDR_AFFLVL(mpidr, aff_level) \ (((mpidr) >> MPIDR_AFF##aff_level##_SHIFT) & MPIDR_AFFLVL_MASK) #define GET_MPIDR() read_sysreg(mpidr_el1) -#define MPIDR_TO_CORE(mpidr) MPIDR_AFFLVL(mpidr, 0) -#define IS_PRIMARY_CORE() (!MPIDR_TO_CORE(GET_MPIDR())) +#define MPIDR_TO_CORE(mpidr) (mpidr & MPIDR_AFF_MASK) #define MODE_EL_SHIFT (0x2) #define MODE_EL_MASK (0x3) diff --git a/subsys/testsuite/include/zephyr/interrupt_util.h b/subsys/testsuite/include/zephyr/interrupt_util.h index 6721b98cb04..68f2632235f 100644 --- a/subsys/testsuite/include/zephyr/interrupt_util.h +++ b/subsys/testsuite/include/zephyr/interrupt_util.h @@ -89,7 +89,10 @@ static inline void trigger_irq(int irq) sys_write32(GICD_SGIR_TGTFILT_REQONLY | GICD_SGIR_SGIINTID(irq), GICD_SGIR); #else - gic_raise_sgi(irq, GET_MPIDR(), BIT(MPIDR_TO_CORE(GET_MPIDR()))); + uint64_t mpidr = GET_MPIDR(); + uint8_t aff0 = MPIDR_AFFLVL(mpidr, 0); + + gic_raise_sgi(irq, mpidr, BIT(aff0)); #endif }