Right now gicv3 driver assumes there is only one cluster and handles sgis based on this assumptions. This will fail for systems with more than 1 cluster. So add support for affinity based routing.
Signed-off-by: Lokesh Vutla <[email protected]> --- hypervisor/arch/arm-common/gic-v3.c | 66 ++++++++++++++++++++++++++++++------- 1 file changed, 54 insertions(+), 12 deletions(-) diff --git a/hypervisor/arch/arm-common/gic-v3.c b/hypervisor/arch/arm-common/gic-v3.c index 7e03b64..cb8166c 100644 --- a/hypervisor/arch/arm-common/gic-v3.c +++ b/hypervisor/arch/arm-common/gic-v3.c @@ -274,18 +274,36 @@ static int gic_cell_init(struct cell *cell) static int gic_send_sgi(struct sgi *sgi) { - u64 val; - u16 targets = sgi->targets; + u64 val, cpu; + unsigned long targets = 0, tar; + u64 mpidr, aff1 = 0, aff2 = 0, aff3 = 0; + struct cell *cell; if (!is_sgi(sgi->id)) return -EINVAL; - if (sgi->routing_mode == 2) - targets = 1 << phys_processor_id(); + tar = sgi->targets; + + for_each_cell(cell) { + for_each_cpu(cpu, cell->cpu_set) { + if (test_bit(cpu, &tar)) { + mpidr = per_cpu(cpu)->mpidr; + targets |= (1 << MPIDR_AFFINITY_LEVEL(mpidr, 0)); + aff1 = MPIDR_AFFINITY_LEVEL(mpidr, 1); + aff2 = MPIDR_AFFINITY_LEVEL(mpidr, 2); + aff3 = MPIDR_AFFINITY_LEVEL(mpidr, 3); + } + } + } + + if (sgi->routing_mode == 2) { + mpidr = phys_processor_id(); + targets = 1 << MPIDR_AFFINITY_LEVEL(mpidr, 0); + } - val = (u64)sgi->aff3 << ICC_SGIR_AFF3_SHIFT - | (u64)sgi->aff2 << ICC_SGIR_AFF2_SHIFT - | sgi->aff1 << ICC_SGIR_AFF1_SHIFT + val = aff3 << ICC_SGIR_AFF3_SHIFT + | aff2 << ICC_SGIR_AFF2_SHIFT + | aff1 << ICC_SGIR_AFF1_SHIFT | (targets & ICC_SGIR_TARGET_MASK) | (sgi->id & 0xf) << ICC_SGIR_IRQN_SHIFT; @@ -304,17 +322,41 @@ static int gic_send_sgi(struct sgi *sgi) return 0; } +#define SGIR_TO_AFFINITY(sgir, level) \ + (sgir >> ICC_SGIR_AFF## level ##_SHIFT & 0xff) + +#define SGIR_TO_MPIDR_AFFINITY(sgir, level) \ + (SGIR_TO_AFFINITY(sgir, level) << MPIDR_LEVEL_SHIFT(level)) + void gicv3_handle_sgir_write(u64 sgir) { struct sgi sgi; unsigned long routing_mode = !!(sgir & ICC_SGIR_ROUTING_BIT); + unsigned long targets = 0, tar; + struct cell *cell; + u64 cluster_id, cpu, clst; + u16 aff0; + + cluster_id = SGIR_TO_MPIDR_AFFINITY(sgir, 3) | + SGIR_TO_MPIDR_AFFINITY(sgir, 2) | + SGIR_TO_MPIDR_AFFINITY(sgir, 1); + tar = sgir & ICC_SGIR_TARGET_MASK; + + for_each_cell(cell) { + for_each_cpu(cpu, cell->cpu_set) { + clst = per_cpu(cpu)->mpidr & ~0xffUL; + aff0 = MPIDR_AFFINITY_LEVEL(per_cpu(cpu)->mpidr, 0); + if ((cluster_id == clst) && (test_bit(aff0, &tar))) { + targets |= (1 << cpu); + } + } + } - /* FIXME: clusters are not supported yet. */ - sgi.targets = sgir & ICC_SGIR_TARGET_MASK; + sgi.targets = targets; sgi.routing_mode = routing_mode; - sgi.aff1 = sgir >> ICC_SGIR_AFF1_SHIFT & 0xff; - sgi.aff2 = sgir >> ICC_SGIR_AFF2_SHIFT & 0xff; - sgi.aff3 = sgir >> ICC_SGIR_AFF3_SHIFT & 0xff; + sgi.aff1 = SGIR_TO_AFFINITY(sgir, 1); + sgi.aff2 = SGIR_TO_AFFINITY(sgir, 2); + sgi.aff3 = SGIR_TO_AFFINITY(sgir, 3); sgi.id = sgir >> ICC_SGIR_IRQN_SHIFT & 0xf; gic_handle_sgir_write(&sgi, true); -- 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.
