[src/trunk]: src/sys/arch/arm/cortex Support pic_set_affinity and pic_get_aff...

[jmcneill <jmcneill%NetBSD.org@localhost>]

details:   https://anonhg.NetBSD.org/src/rev/34fa689c4c30
branches:  trunk
changeset: 320662:34fa689c4c30
user:      jmcneill <jmcneill%NetBSD.org@localhost>
date:      Sun Jul 15 16:04:07 2018 +0000

description:
Support pic_set_affinity and pic_get_affinity

diffstat:

 sys/arch/arm/cortex/gic.c |  69 ++++++++++++++++++++++++++++++++++++++++++++--
 1 files changed, 66 insertions(+), 3 deletions(-)

diffs (118 lines):

diff -r 06fe85982ef8 -r 34fa689c4c30 sys/arch/arm/cortex/gic.c
--- a/sys/arch/arm/cortex/gic.c Sun Jul 15 16:03:24 2018 +0000
+++ b/sys/arch/arm/cortex/gic.c Sun Jul 15 16:04:07 2018 +0000
@@ -1,4 +1,4 @@
-/*     $NetBSD: gic.c,v 1.34 2018/04/28 18:26:53 jakllsch Exp $        */
+/*     $NetBSD: gic.c,v 1.35 2018/07/15 16:04:07 jmcneill Exp $        */
 /*-
  * Copyright (c) 2012 The NetBSD Foundation, Inc.
  * All rights reserved.
@@ -34,7 +34,7 @@
 #define _INTR_PRIVATE
 
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: gic.c,v 1.34 2018/04/28 18:26:53 jakllsch Exp $");
+__KERNEL_RCSID(0, "$NetBSD: gic.c,v 1.35 2018/07/15 16:04:07 jmcneill Exp $");
 
 #include <sys/param.h>
 #include <sys/bus.h>
@@ -75,6 +75,8 @@
 #ifdef MULTIPROCESSOR
 static void armgic_cpu_init(struct pic_softc *, struct cpu_info *);
 static void armgic_ipi_send(struct pic_softc *, const kcpuset_t *, u_long);
+static void armgic_get_affinity(struct pic_softc *, size_t, kcpuset_t *);
+static int armgic_set_affinity(struct pic_softc *, size_t, const kcpuset_t *);
 #endif
 
 static const struct pic_ops armgic_picops = {
@@ -88,6 +90,8 @@
 #ifdef MULTIPROCESSOR
        .pic_cpu_init = armgic_cpu_init,
        .pic_ipi_send = armgic_ipi_send,
+       .pic_get_affinity = armgic_get_affinity,
+       .pic_set_affinity = armgic_set_affinity,
 #endif
 };
 
@@ -104,6 +108,7 @@
        uint32_t sc_gic_valid_lines[1024/32];
        uint32_t sc_enabled_local;
 #ifdef MULTIPROCESSOR
+       uint32_t sc_target[MAXCPUS];
        uint32_t sc_mptargets;
 #endif
        uint32_t sc_bptargets;
@@ -222,6 +227,63 @@
        gicc_write(sc, GICC_PMR, priority);
 }
 
+#ifdef MULTIPROCESSOR
+static void
+armgic_get_affinity(struct pic_softc *pic, size_t irq, kcpuset_t *affinity)
+{
+       struct armgic_softc * const sc = PICTOSOFTC(pic);
+       const size_t group = irq / 32;
+       int n;
+
+       kcpuset_zero(affinity);
+       if (group == 0) {
+               /* All CPUs are targets for group 0 (SGI/PPI) */
+               for (n = 0; n < MAXCPUS; n++) {
+                       if (sc->sc_target[n] != 0)
+                               kcpuset_set(affinity, n);
+               }
+       } else {
+               /* Find distributor targets (SPI) */
+               const u_int byte_shift = 8 * (irq & 3);
+               const bus_size_t targets_reg = GICD_ITARGETSRn(irq / 4);
+               const uint32_t targets = gicd_read(sc, targets_reg);
+               const uint32_t targets_val = (targets >> byte_shift) & 0xff;
+
+               for (n = 0; n < MAXCPUS; n++) {
+                       if (sc->sc_target[n] & targets_val)
+                               kcpuset_set(affinity, n);
+               }
+       }
+}
+
+static int
+armgic_set_affinity(struct pic_softc *pic, size_t irq,
+    const kcpuset_t *affinity)
+{
+       struct armgic_softc * const sc = PICTOSOFTC(pic);
+       const size_t group = irq / 32;
+       if (group == 0)
+               return EINVAL;
+
+       const u_int byte_shift = 8 * (irq & 3);
+       const bus_size_t targets_reg = GICD_ITARGETSRn(irq / 4);
+       uint32_t targets_val = 0;
+       int n;
+
+       for (n = 0; n < MAXCPUS; n++) {
+               if (kcpuset_isset(affinity, n))
+                       targets_val |= sc->sc_target[n];
+       }
+
+       uint32_t targets = gicd_read(sc, targets_reg);
+       targets &= ~(0xff << byte_shift);
+       targets |= (targets_val << byte_shift);
+       gicd_write(sc, targets_reg, targets);
+
+       return 0;
+}
+#endif
+
 #ifdef __HAVE_PIC_FAST_SOFTINTS
 void
 softint_init_md(lwp_t *l, u_int level, uintptr_t *machdep_p)
@@ -451,7 +513,8 @@
 armgic_cpu_init(struct pic_softc *pic, struct cpu_info *ci)
 {
        struct armgic_softc * const sc = PICTOSOFTC(pic);
-       sc->sc_mptargets |= gicd_find_targets(sc);
+       sc->sc_target[cpu_index(ci)] = gicd_find_targets(sc);
+       sc->sc_mptargets |= sc->sc_target[cpu_index(ci)];
        KASSERTMSG(ci->ci_cpl == IPL_HIGH, "ipl %d not IPL_HIGH", ci->ci_cpl);
        armgic_cpu_init_priorities(sc);
        if (!CPU_IS_PRIMARY(ci)) {