MPIDR can be used to compare the GICR_TYPER register for redistributor base calculation. Logic is imported from kernel.
Signed-off-by: Lokesh Vutla <[email protected]> --- hypervisor/arch/arm/gic-v3.c | 13 ++++++++++--- hypervisor/arch/arm/include/asm/sysregs.h | 7 +++++++ hypervisor/arch/arm64/include/asm/sysregs.h | 10 ++++++++++ 3 files changed, 27 insertions(+), 3 deletions(-) diff --git a/hypervisor/arch/arm/gic-v3.c b/hypervisor/arch/arm/gic-v3.c index 3fcaf10..cfa641f 100644 --- a/hypervisor/arch/arm/gic-v3.c +++ b/hypervisor/arch/arm/gic-v3.c @@ -19,6 +19,7 @@ #include <asm/gic.h> #include <asm/irqchip.h> #include <asm/setup.h> +#include <asm/sysregs.h> #include <asm/traps.h> /* @@ -93,12 +94,18 @@ static int gic_cpu_init(struct per_cpu *cpu_data) unsigned long redist_addr = system_config->platform_info.arm.gicr_base; void *redist_base = gicr_base; unsigned long redist_size; - u64 typer; - u32 pidr; + u64 typer, mpidr; + u32 pidr, aff; u32 cell_icc_ctlr, cell_icc_pmr, cell_icc_igrpen1; u32 ich_vtr; u32 ich_vmcr; + mpidr = cpu_data->mpidr; + aff = (MPIDR_AFFINITY_LEVEL(mpidr, 3) << 24 | + MPIDR_AFFINITY_LEVEL(mpidr, 2) << 16 | + MPIDR_AFFINITY_LEVEL(mpidr, 1) << 8 | + MPIDR_AFFINITY_LEVEL(mpidr, 0)); + /* Find redistributor */ do { pidr = mmio_read32(redist_base + GICR_PIDR2); @@ -109,7 +116,7 @@ static int gic_cpu_init(struct per_cpu *cpu_data) redist_size = gic_version == 4 ? 0x40000 : 0x20000; typer = mmio_read64(redist_base + GICR_TYPER); - if ((typer >> 32) == cpu_data->cpu_id) { + if ((typer >> 32) == aff) { cpu_data->gicr.base = redist_base; cpu_data->gicr.phys_addr = redist_addr; break; diff --git a/hypervisor/arch/arm/include/asm/sysregs.h b/hypervisor/arch/arm/include/asm/sysregs.h index d6e2bb7..922d97a 100644 --- a/hypervisor/arch/arm/include/asm/sysregs.h +++ b/hypervisor/arch/arm/include/asm/sysregs.h @@ -84,6 +84,13 @@ #define SCTLR_C_AND_M_SET(sctlr) \ (((sctlr) & (SCTLR_C_BIT | SCTLR_M_BIT)) == (SCTLR_C_BIT | SCTLR_M_BIT)) +#define MPIDR_LEVEL_BITS 8 +#define MPIDR_LEVEL_MASK ((1 << MPIDR_LEVEL_BITS) - 1) +#define MPIDR_LEVEL_SHIFT(level) (MPIDR_LEVEL_BITS * (level)) + +#define MPIDR_AFFINITY_LEVEL(mpidr, level) \ + (((mpidr) >> (MPIDR_LEVEL_BITS * (level))) & MPIDR_LEVEL_MASK) + /* Bits to wipe on cell reset */ #define SCTLR_MASK (SCTLR_M_BIT | SCTLR_A_BIT | SCTLR_C_BIT \ | SCTLR_I_BIT | SCTLR_V_BIT | SCTLR_WXN_BIT \ diff --git a/hypervisor/arch/arm64/include/asm/sysregs.h b/hypervisor/arch/arm64/include/asm/sysregs.h index 923055f..4451c77 100644 --- a/hypervisor/arch/arm64/include/asm/sysregs.h +++ b/hypervisor/arch/arm64/include/asm/sysregs.h @@ -34,6 +34,16 @@ #define MPIDR_U_BIT (1 << 30) #define MPIDR_MP_BIT (1 << 31) +#define MPIDR_LEVEL_BITS_SHIFT 3 +#define MPIDR_LEVEL_BITS (1 << MPIDR_LEVEL_BITS_SHIFT) +#define MPIDR_LEVEL_MASK ((1 << MPIDR_LEVEL_BITS) - 1) + +#define MPIDR_LEVEL_SHIFT(level) \ + (((1 << (level)) >> 1) << MPIDR_LEVEL_BITS_SHIFT) + +#define MPIDR_AFFINITY_LEVEL(mpidr, level) \ + (((mpidr) >> MPIDR_LEVEL_SHIFT(level)) & MPIDR_LEVEL_MASK) + #define SCTLR_M_BIT (1 << 0) #define SCTLR_A_BIT (1 << 1) #define SCTLR_C_BIT (1 << 2) -- 2.13.0 -- You received this message because you are subscribed to the Google Groups "Jailhouse" group. To unsubscribe from this group and stop receiving emails from it, send an email to [email protected]. For more options, visit https://groups.google.com/d/optout.
