diff --git a/drivers/interrupt_controller/intc_gicv3.c b/drivers/interrupt_controller/intc_gicv3.c index f2d03365485..addedb48535 100644 --- a/drivers/interrupt_controller/intc_gicv3.c +++ b/drivers/interrupt_controller/intc_gicv3.c @@ -477,12 +477,55 @@ static void gicv3_dist_init(void) #endif } +static uint64_t arm_gic_mpidr_to_affinity(uint64_t mpidr) +{ + uint64_t aff3, aff2, aff1, aff0; + +#if defined(CONFIG_ARM) + /* There is no Aff3 in AArch32 MPIDR */ + aff3 = 0; +#else + aff3 = MPIDR_AFFLVL(mpidr, 3); +#endif + + aff2 = MPIDR_AFFLVL(mpidr, 2); + aff1 = MPIDR_AFFLVL(mpidr, 1); + aff0 = MPIDR_AFFLVL(mpidr, 0); + + return (aff3 << 24 | aff2 << 16 | aff1 << 8 | aff0); +} + +static mem_addr_t arm_gic_iterate_rdists(void) +{ + uint64_t aff = arm_gic_mpidr_to_affinity(GET_MPIDR()); + + for (mem_addr_t rdist_addr = GIC_RDIST_BASE; + rdist_addr < GIC_RDIST_BASE + GIC_RDIST_SIZE; + rdist_addr += 0x20000) { + uint64_t val = sys_read64(rdist_addr + GICR_TYPER); + + if (GICR_TYPER_AFFINITY_VALUE_GET(val) == aff) { + return rdist_addr; + } + + if (GICR_TYPER_LAST_GET(val) == 1) { + return (mem_addr_t)NULL; + } + } + + return (mem_addr_t)NULL; +} + static void __arm_gic_init(void) { uint8_t cpu; + mem_addr_t gic_rd_base; cpu = arch_curr_cpu()->id; - gic_rdists[cpu] = GIC_RDIST_BASE + MPIDR_TO_CORE(GET_MPIDR()) * 0x20000; + gic_rd_base = arm_gic_iterate_rdists(); + __ASSERT(gic_rd_base != (mem_addr_t)NULL, ""); + + gic_rdists[cpu] = gic_rd_base; #ifdef CONFIG_GIC_V3_ITS /* Enable LPIs in Redistributor */ diff --git a/drivers/interrupt_controller/intc_gicv3_priv.h b/drivers/interrupt_controller/intc_gicv3_priv.h index e7d1704307a..e0347ed3576 100644 --- a/drivers/interrupt_controller/intc_gicv3_priv.h +++ b/drivers/interrupt_controller/intc_gicv3_priv.h @@ -30,6 +30,7 @@ */ #define GIC_RDIST_BASE DT_REG_ADDR_BY_IDX(DT_INST(0, arm_gic), 1) +#define GIC_RDIST_SIZE DT_REG_SIZE_BY_IDX(DT_INST(0, arm_gic), 1) /* SGI base is at 64K offset from Redistributor */ #define GICR_SGI_BASE_OFF 0x10000 @@ -62,6 +63,12 @@ #define GICR_CTLR_RWP 3 /* GICR_TYPER */ +#define GICR_TYPER_AFFINITY_VALUE_SHIFT 32 +#define GICR_TYPER_AFFINITY_VALUE_MASK 0xFFFFFFFFUL +#define GICR_TYPER_AFFINITY_VALUE_GET(_val) MASK_GET(_val, GICR_TYPER_AFFINITY_VALUE) +#define GICR_TYPER_LAST_SHIFT 4 +#define GICR_TYPER_LAST_MASK 0x10UL +#define GICR_TYPER_LAST_GET(_val) MASK_GET(_val, GICR_TYPER_LAST) #define GICR_TYPER_PROCESSOR_NUMBER_SHIFT 8 #define GICR_TYPER_PROCESSOR_NUMBER_MASK 0xFFFFUL #define GICR_TYPER_PROCESSOR_NUMBER_GET(_val) MASK_GET(_val, GICR_TYPER_PROCESSOR_NUMBER)