From f8b5fd2a9a1f37338d35d30019f45f83ca5d7903 Mon Sep 17 00:00:00 2001 From: zhangyuan21 Date: Mon, 24 Apr 2023 05:27:18 +0800 Subject: [PATCH] arch/arm64: send sgi with correct aff and target list armv8r and armv8a have different process affinity, and sgi affinity needs to be able to adapt all of them. Signed-off-by: zhangyuan21 --- arch/arm64/include/fvp-v8r/chip.h | 2 ++ arch/arm64/include/qemu/chip.h | 2 ++ arch/arm64/src/common/arm64_arch.h | 11 ++++++- arch/arm64/src/common/arm64_boot.c | 8 +++++ arch/arm64/src/common/arm64_cpupause.c | 3 +- arch/arm64/src/common/arm64_gic.h | 3 +- arch/arm64/src/common/arm64_gicv3.c | 42 +++++++++++++++++++++++--- 7 files changed, 62 insertions(+), 9 deletions(-) diff --git a/arch/arm64/include/fvp-v8r/chip.h b/arch/arm64/include/fvp-v8r/chip.h index 665b1ebaf91..a6dcffa355e 100644 --- a/arch/arm64/include/fvp-v8r/chip.h +++ b/arch/arm64/include/fvp-v8r/chip.h @@ -69,6 +69,8 @@ #define CONFIG_LOAD_BASE 0x00000000 +#define MPID_TO_CLUSTER_ID(mpid) ((mpid) & ~0xff) + #endif #endif /* __ARCH_ARM64_INCLUDE_FVP_V8R_CHIP_H */ diff --git a/arch/arm64/include/qemu/chip.h b/arch/arm64/include/qemu/chip.h index f854f888bcc..3bec3877754 100644 --- a/arch/arm64/include/qemu/chip.h +++ b/arch/arm64/include/qemu/chip.h @@ -59,6 +59,8 @@ #define CONFIG_LOAD_BASE 0x40280000 +#define MPID_TO_CLUSTER_ID(mpid) ((mpid) & ~0xff) + #endif #endif /* __ARCH_ARM64_INCLUDE_QEMU_CHIP_H */ diff --git a/arch/arm64/src/common/arm64_arch.h b/arch/arm64/src/common/arm64_arch.h index dd30b3f1850..2a9fef9561b 100644 --- a/arch/arm64/src/common/arm64_arch.h +++ b/arch/arm64/src/common/arm64_arch.h @@ -150,6 +150,7 @@ /* MPIDR_EL1, Multiprocessor Affinity Register */ #define MPIDR_AFFLVL_MASK (0xff) +#define MPIDR_ID_MASK (0xff00ffffff) #define MPIDR_AFF0_SHIFT (0) #define MPIDR_AFF1_SHIFT (8) @@ -180,7 +181,13 @@ (((mpid) >> MPIDR_AFF ## aff_level ## _SHIFT) & MPIDR_AFFLVL_MASK) #define CORE_TO_MPID(core, aff_level) \ - (((core) << MPIDR_AFF ## aff_level ## _SHIFT)) + ({ \ + uint64_t __mpidr = GET_MPIDR(); \ + __mpidr &= ~(MPIDR_AFFLVL_MASK << MPIDR_AFF ## aff_level ## _SHIFT); \ + __mpidr |= (cpu << MPIDR_AFF ## aff_level ## _SHIFT); \ + __mpidr &= MPIDR_ID_MASK; \ + __mpidr; \ + }) /* System register interface to GICv3 */ @@ -561,6 +568,8 @@ void arm64_cpu_enable(void); #ifdef CONFIG_SMP uint64_t arm64_get_mpid(int cpu); +#else +# define arm64_get_mpid(cpu) GET_MPIDR() #endif /* CONFIG_SMP */ #endif /* __ASSEMBLY__ */ diff --git a/arch/arm64/src/common/arm64_boot.c b/arch/arm64/src/common/arm64_boot.c index 46f1a7017db..72b25698952 100644 --- a/arch/arm64/src/common/arm64_boot.c +++ b/arch/arm64/src/common/arm64_boot.c @@ -30,8 +30,16 @@ #include "arm64_internal.h" #include "arm64_arch.h" +/**************************************************************************** + * Public Data + ****************************************************************************/ + extern void *_vector_table[]; +/**************************************************************************** + * Private Data + ****************************************************************************/ + /**************************************************************************** * Public Functions ****************************************************************************/ diff --git a/arch/arm64/src/common/arm64_cpupause.c b/arch/arm64/src/common/arm64_cpupause.c index 27c892d7ce8..b8e85eed299 100644 --- a/arch/arm64/src/common/arm64_cpupause.c +++ b/arch/arm64/src/common/arm64_cpupause.c @@ -247,7 +247,6 @@ int arm64_pause_handler(int irq, void *context, void *arg) int up_cpu_pause(int cpu) { int ret; - uint64_t mpidr = GET_MPIDR(); DEBUGASSERT(cpu >= 0 && cpu < CONFIG_SMP_NCPUS && cpu != this_cpu()); @@ -273,7 +272,7 @@ int up_cpu_pause(int cpu) /* Execute SGI2 */ - ret = arm64_gic_raise_sgi(SGI_CPU_PAUSE, mpidr, (1 << cpu)); + ret = arm64_gic_raise_sgi(SGI_CPU_PAUSE, (1 << cpu)); if (ret < 0) { /* What happened? Unlock the g_cpu_wait spinlock */ diff --git a/arch/arm64/src/common/arm64_gic.h b/arch/arm64/src/common/arm64_gic.h index ac0dcb99d20..241e8392abb 100644 --- a/arch/arm64/src/common/arm64_gic.h +++ b/arch/arm64/src/common/arm64_gic.h @@ -307,8 +307,7 @@ int arm64_gic_irq_trigger(unsigned int intid, uint32_t flags); uint64_t * arm64_decodeirq(uint64_t *regs); -int arm64_gic_raise_sgi(unsigned int sgi_id, uint64_t target_aff, - uint16_t target_list); +int arm64_gic_raise_sgi(unsigned int sgi_id, uint16_t target_list); #ifdef CONFIG_SMP diff --git a/arch/arm64/src/common/arm64_gicv3.c b/arch/arm64/src/common/arm64_gicv3.c index 041d4dff85d..4f8e06ed106 100644 --- a/arch/arm64/src/common/arm64_gicv3.c +++ b/arch/arm64/src/common/arm64_gicv3.c @@ -303,8 +303,8 @@ void arm64_gic_eoi(unsigned int intid) write_sysreg(intid, ICC_EOIR1_EL1); } -int arm64_gic_raise_sgi(unsigned int sgi_id, uint64_t target_aff, - uint16_t target_list) +static int arm64_gic_send_sgi(unsigned int sgi_id, uint64_t target_aff, + uint16_t target_list) { uint32_t aff3; uint32_t aff2; @@ -329,6 +329,41 @@ int arm64_gic_raise_sgi(unsigned int sgi_id, uint64_t target_aff, return 0; } +int arm64_gic_raise_sgi(unsigned int sgi_id, uint16_t target_list) +{ + uint64_t pre_cluster_id = UINT64_MAX; + uint64_t curr_cluster_id; + uint64_t curr_mpidr; + uint16_t tlist = 0; + uint16_t cpu = 0; + uint16_t i; + + while ((i = ffs(target_list))) + { + cpu += (i - 1); + + target_list >>= i; + + curr_mpidr = arm64_get_mpid(cpu); + curr_cluster_id = MPID_TO_CLUSTER_ID(curr_mpidr); + + if (pre_cluster_id != UINT64_MAX && + pre_cluster_id != curr_cluster_id) + { + arm64_gic_send_sgi(sgi_id, pre_cluster_id, tlist); + } + + tlist |= 1 << (curr_mpidr & MPIDR_AFFLVL_MASK); + + cpu += i; + pre_cluster_id = curr_cluster_id; + } + + arm64_gic_send_sgi(sgi_id, pre_cluster_id, tlist); + + return 0; +} + /* Wake up GIC redistributor. * clear ProcessorSleep and wait till ChildAsleep is cleared. * ProcessSleep to be cleared only when ChildAsleep is set @@ -597,13 +632,12 @@ void up_affinity_irq(int irq, cpu_set_t cpuset) void up_trigger_irq(int irq, cpu_set_t cpuset) { - uint64_t mpidr = GET_MPIDR(); uint32_t mask = BIT(irq & (GIC_NUM_INTR_PER_REG - 1)); uint32_t idx = irq / GIC_NUM_INTR_PER_REG; if (GIC_IS_SGI(irq)) { - arm64_gic_raise_sgi(irq, mpidr, cpuset); + arm64_gic_raise_sgi(irq, cpuset); } else if (irq >= 0 && irq < NR_IRQS) {