Index: head/sys/arm64/arm64/gic_v3.c =================================================================== --- head/sys/arm64/arm64/gic_v3.c +++ head/sys/arm64/arm64/gic_v3.c @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -61,6 +62,10 @@ static void gic_v3_eoi(device_t, u_int); static void gic_v3_mask_irq(device_t, u_int); static void gic_v3_unmask_irq(device_t, u_int); +#ifdef SMP +static void gic_v3_init_secondary(device_t); +static void gic_v3_ipi_send(device_t, cpuset_t, u_int); +#endif static device_method_t gic_v3_methods[] = { /* Device interface */ @@ -71,7 +76,10 @@ DEVMETHOD(pic_eoi, gic_v3_eoi), DEVMETHOD(pic_mask, gic_v3_mask_irq), DEVMETHOD(pic_unmask, gic_v3_unmask_irq), - +#ifdef SMP + DEVMETHOD(pic_init_secondary, gic_v3_init_secondary), + DEVMETHOD(pic_ipi_send, gic_v3_ipi_send), +#endif /* End */ DEVMETHOD_END }; @@ -95,6 +103,7 @@ /* Helper routines starting with gic_v3_ */ static int gic_v3_dist_init(struct gic_v3_softc *); +static int gic_v3_redist_alloc(struct gic_v3_softc *); static int gic_v3_redist_find(struct gic_v3_softc *); static int gic_v3_redist_init(struct gic_v3_softc *); static int gic_v3_cpu_init(struct gic_v3_softc *); @@ -105,11 +114,21 @@ /* Primary CPU initialization sequence */ static gic_v3_initseq_t gic_v3_primary_init[] = { gic_v3_dist_init, + gic_v3_redist_alloc, gic_v3_redist_init, gic_v3_cpu_init, NULL }; +#ifdef SMP +/* Secondary CPU initialization sequence */ +static gic_v3_initseq_t gic_v3_secondary_init[] = { + gic_v3_redist_init, + gic_v3_cpu_init, + NULL +}; +#endif + /* * Device interface. */ @@ -213,7 +232,7 @@ for (rid = 0; rid < (sc->gic_redists.nregions + 1); rid++) bus_release_resource(dev, SYS_RES_MEMORY, rid, sc->gic_res[rid]); - for (i = 0; i < MAXCPU; i++) + for (i = 0; i < mp_ncpus; i++) free(sc->gic_redists.pcpu[i], M_GIC_V3); free(sc->gic_res, M_GIC_V3); @@ -258,13 +277,9 @@ } if (active_irq <= GIC_LAST_SGI) { - /* - * TODO: Implement proper SGI handling. - * Mask it if such is received for some reason. - */ - device_printf(dev, - "Received unsupported interrupt type: SGI\n"); - PIC_MASK(dev, active_irq); + gic_icc_write(EOIR1, (uint64_t)active_irq); + arm_dispatch_intr(active_irq, frame); + continue; } } } @@ -283,7 +298,7 @@ sc = device_get_softc(dev); - if (irq >= GIC_FIRST_PPI && irq <= GIC_LAST_PPI) { /* PPIs in corresponding Re-Distributor */ + if (irq <= GIC_LAST_PPI) { /* SGIs and PPIs in corresponding Re-Distributor */ gic_r_write(sc, 4, GICR_SGI_BASE_SIZE + GICD_ICENABLER(irq), GICD_I_MASK(irq)); gic_v3_wait_for_rwp(sc, REDIST); @@ -303,7 +318,7 @@ sc = device_get_softc(dev); - if (irq >= GIC_FIRST_PPI && irq <= GIC_LAST_PPI) { /* PPIs in corresponding Re-Distributor */ + if (irq <= GIC_LAST_PPI) { /* SGIs and PPIs in corresponding Re-Distributor */ gic_r_write(sc, 4, GICR_SGI_BASE_SIZE + GICD_ISENABLER(irq), GICD_I_MASK(irq)); gic_v3_wait_for_rwp(sc, REDIST); @@ -316,6 +331,101 @@ panic("%s: Unsupported IRQ number %u", __func__, irq); } +#ifdef SMP +static void +gic_v3_init_secondary(device_t dev) +{ + struct gic_v3_softc *sc; + gic_v3_initseq_t *init_func; + int err; + + sc = device_get_softc(dev); + + /* Train init sequence for boot CPU */ + for (init_func = gic_v3_secondary_init; *init_func != NULL; init_func++) { + err = (*init_func)(sc); + if (err != 0) { + device_printf(dev, + "Could not initialize GIC for CPU%u\n", + PCPU_GET(cpuid)); + return; + } + } + + /* + * Try to initialize ITS. + * If there is no driver attached this routine will fail but that + * does not mean failure here as only LPIs will not be functional + * on the current CPU. + */ + if (its_init_cpu(NULL) != 0) { + device_printf(dev, + "Could not initialize ITS for CPU%u. " + "No LPIs will arrive on this CPU\n", + PCPU_GET(cpuid)); + } + + /* + * ARM64TODO: Unmask timer PPIs. To be removed when appropriate + * mechanism is implemented. + * Activate the timer interrupts: virtual (27), secure (29), + * and non-secure (30). Use hardcoded values here as there + * should be no defines for them. + */ + gic_v3_unmask_irq(dev, 27); + gic_v3_unmask_irq(dev, 29); + gic_v3_unmask_irq(dev, 30); +} + +static void +gic_v3_ipi_send(device_t dev, cpuset_t cpuset, u_int ipi) +{ + u_int cpu; + uint64_t aff, tlist; + uint64_t val; + uint64_t aff_mask; + + /* Set affinity mask to match level 3, 2 and 1 */ + aff_mask = CPU_AFF1_MASK | CPU_AFF2_MASK | CPU_AFF3_MASK; + + /* Iterate through all CPUs in set */ + while (!CPU_EMPTY(&cpuset)) { + aff = tlist = 0; + for (cpu = 0; cpu < mp_ncpus; cpu++) { + /* Compose target list for single AFF3:AFF2:AFF1 set */ + if (CPU_ISSET(cpu, &cpuset)) { + if (!tlist) { + /* + * Save affinity of the first CPU to + * send IPI to for later comparison. + */ + aff = CPU_AFFINITY(cpu); + tlist |= (1UL << CPU_AFF0(aff)); + CPU_CLR(cpu, &cpuset); + } + /* Check for same Affinity level 3, 2 and 1 */ + if ((aff & aff_mask) == (CPU_AFFINITY(cpu) & aff_mask)) { + tlist |= (1UL << CPU_AFF0(CPU_AFFINITY(cpu))); + /* Clear CPU in cpuset from target list */ + CPU_CLR(cpu, &cpuset); + } + } + } + if (tlist) { + KASSERT((tlist & ~GICI_SGI_TLIST_MASK) == 0, + ("Target list too long for GICv3 IPI")); + /* Send SGI to CPUs in target list */ + val = tlist; + val |= (uint64_t)CPU_AFF3(aff) << GICI_SGI_AFF3_SHIFT; + val |= (uint64_t)CPU_AFF2(aff) << GICI_SGI_AFF2_SHIFT; + val |= (uint64_t)CPU_AFF1(aff) << GICI_SGI_AFF1_SHIFT; + val |= (uint64_t)(ipi & GICI_SGI_IPI_MASK) << GICI_SGI_IPI_SHIFT; + gic_icc_write(SGI1R, val); + } + } +} +#endif + /* * Helper routines */ @@ -463,6 +573,22 @@ /* Re-Distributor */ static int +gic_v3_redist_alloc(struct gic_v3_softc *sc) +{ + u_int cpuid; + + /* Allocate struct resource for all CPU's Re-Distributor registers */ + for (cpuid = 0; cpuid < mp_ncpus; cpuid++) + if (CPU_ISSET(cpuid, &all_cpus) != 0) + sc->gic_redists.pcpu[cpuid] = + malloc(sizeof(*sc->gic_redists.pcpu[0]), + M_GIC_V3, M_WAITOK); + else + sc->gic_redists.pcpu[cpuid] = NULL; + return (0); +} + +static int gic_v3_redist_find(struct gic_v3_softc *sc) { struct resource r_res; @@ -475,10 +601,6 @@ cpuid = PCPU_GET(cpuid); - /* Allocate struct resource for this CPU's Re-Distributor registers */ - sc->gic_redists.pcpu[cpuid] = - malloc(sizeof(*sc->gic_redists.pcpu[0]), M_GIC_V3, M_WAITOK); - aff = CPU_AFFINITY(cpuid); /* Affinity in format for comparison with typer */ aff = (CPU_AFF3(aff) << 24) | (CPU_AFF2(aff) << 16) | @@ -502,7 +624,6 @@ default: device_printf(sc->dev, "No Re-Distributor found for CPU%u\n", cpuid); - free(sc->gic_redists.pcpu[cpuid], M_GIC_V3); return (ENODEV); } @@ -531,7 +652,6 @@ } while ((typer & GICR_TYPER_LAST) == 0); } - free(sc->gic_redists.pcpu[cpuid], M_GIC_V3); device_printf(sc->dev, "No Re-Distributor found for CPU%u\n", cpuid); return (ENXIO); } Index: head/sys/arm64/arm64/gic_v3_its.c =================================================================== --- head/sys/arm64/arm64/gic_v3_its.c +++ head/sys/arm64/arm64/gic_v3_its.c @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -88,7 +89,6 @@ static int its_alloc_tables(struct gic_v3_its_softc *); static void its_free_tables(struct gic_v3_its_softc *); static void its_init_commandq(struct gic_v3_its_softc *); -static int its_init_cpu(struct gic_v3_its_softc *); static void its_init_cpu_collection(struct gic_v3_its_softc *); static uint32_t its_get_devid(device_t); @@ -105,8 +105,8 @@ static void lpi_init_conftable(struct gic_v3_its_softc *); static void lpi_bitmap_init(struct gic_v3_its_softc *); -static void lpi_init_cpu(struct gic_v3_its_softc *); static int lpi_config_cpu(struct gic_v3_its_softc *); +static void lpi_alloc_cpu_pendtables(struct gic_v3_its_softc *); const char *its_ptab_cache[] = { [GITS_BASER_CACHE_NCNB] = "(NC,NB)", @@ -223,8 +223,12 @@ } /* 3. Allocate collections. One per-CPU */ - sc->its_cols = malloc(sizeof(*sc->its_cols) * MAXCPU, - M_GIC_V3_ITS, (M_WAITOK | M_ZERO)); + for (int cpu = 0; cpu < mp_ncpus; cpu++) + if (CPU_ISSET(cpu, &all_cpus) != 0) + sc->its_cols[cpu] = malloc(sizeof(*sc->its_cols[0]), + M_GIC_V3_ITS, (M_WAITOK | M_ZERO)); + else + sc->its_cols[cpu] = NULL; /* 4. Enable ITS in GITS_CTLR */ gits_tmp = gic_its_read(sc, 4, GITS_CTLR); @@ -236,10 +240,13 @@ /* 6. LPIs bitmap init */ lpi_bitmap_init(sc); - /* 7. CPU init */ + /* 7. Allocate pending tables for all CPUs */ + lpi_alloc_cpu_pendtables(sc); + + /* 8. CPU init */ (void)its_init_cpu(sc); - /* 8. Init ITS devices list */ + /* 9. Init ITS devices list */ TAILQ_INIT(&sc->its_dev_list); arm_register_msi_pic(dev); @@ -280,7 +287,8 @@ /* ITTs */ its_free_tables(sc); /* Collections */ - free(sc->its_cols, M_GIC_V3_ITS); + for (cpuid = 0; cpuid < mp_ncpus; cpuid++) + free(sc->its_cols[cpuid], M_GIC_V3_ITS); /* LPI config table */ parent = device_get_parent(sc->dev); gic_sc = device_get_softc(parent); @@ -288,10 +296,13 @@ contigfree((void *)gic_sc->gic_redists.lpis.conf_base, LPI_CONFTAB_SIZE, M_GIC_V3_ITS); } - if ((void *)gic_sc->gic_redists.lpis.pend_base[cpuid] != NULL) { - contigfree((void *)gic_sc->gic_redists.lpis.pend_base[cpuid], - roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K), M_GIC_V3_ITS); - } + for (cpuid = 0; cpuid < mp_ncpus; cpuid++) + if ((void *)gic_sc->gic_redists.lpis.pend_base[cpuid] != NULL) { + contigfree( + (void *)gic_sc->gic_redists.lpis.pend_base[cpuid], + roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K), + M_GIC_V3_ITS); + } /* Resource... */ bus_release_resource(dev, SYS_RES_MEMORY, rid, sc->its_res); @@ -525,13 +536,31 @@ gic_its_write(sc, 8, GITS_CWRITER, 0x0); } -static int +int its_init_cpu(struct gic_v3_its_softc *sc) { device_t parent; struct gic_v3_softc *gic_sc; /* + * NULL in place of the softc pointer means that + * this function was called during GICv3 secondary initialization. + */ + if (sc == NULL) { + if (device_is_attached(its_sc->dev)) { + /* + * XXX ARM64TODO: This is part of the workaround that + * saves ITS software context for further use in + * mask/unmask and here. This should be removed as soon + * as the upper layer is capable of passing the ITS + * context to this function. + */ + sc = its_sc; + } else + return (ENXIO); + } + + /* * Check for LPIs support on this Re-Distributor. */ parent = device_get_parent(sc->dev); @@ -544,8 +573,8 @@ return (ENXIO); } - /* Initialize LPIs for this CPU */ - lpi_init_cpu(sc); + /* Configure LPIs for this CPU */ + lpi_config_cpu(sc); /* Initialize collections */ its_init_cpu_collection(sc); @@ -582,11 +611,12 @@ target = GICR_TYPER_CPUNUM(typer); } - sc->its_cols[cpuid].col_target = target; - sc->its_cols[cpuid].col_id = cpuid; + sc->its_cols[cpuid]->col_target = target; + sc->its_cols[cpuid]->col_id = cpuid; + + its_cmd_mapc(sc, sc->its_cols[cpuid], 1); + its_cmd_invall(sc, sc->its_cols[cpuid]); - its_cmd_mapc(sc, &sc->its_cols[cpuid], 1); - its_cmd_invall(sc, &sc->its_cols[cpuid]); } static void @@ -633,7 +663,7 @@ } static void -lpi_init_cpu(struct gic_v3_its_softc *sc) +lpi_alloc_cpu_pendtables(struct gic_v3_its_softc *sc) { device_t parent; struct gic_v3_softc *gic_sc; @@ -647,25 +677,31 @@ * LPI Pending Table settings. * This has to be done for each Re-Distributor, hence for each CPU. */ - cpuid = PCPU_GET(cpuid); + for (cpuid = 0; cpuid < mp_ncpus; cpuid++) { - pend_base = (vm_offset_t)contigmalloc( - roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K), M_GIC_V3_ITS, - (M_WAITOK | M_ZERO), 0, ~0UL, PAGE_SIZE_64K, 0); - - /* Clean D-cache so that ITS can see zeroed pages */ - cpu_dcache_wb_range((vm_offset_t)pend_base, - roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K)); + /* Limit allocation to active CPUs only */ + if (CPU_ISSET(cpuid, &all_cpus) == 0) + continue; - if (bootverbose) { - device_printf(sc->dev, - "LPI Pending Table for CPU%u at PA: 0x%lx\n", - cpuid, vtophys(pend_base)); - } + pend_base = (vm_offset_t)contigmalloc( + roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K), M_GIC_V3_ITS, + (M_WAITOK | M_ZERO), 0, ~0UL, PAGE_SIZE_64K, 0); + + /* Clean D-cache so that ITS can see zeroed pages */ + cpu_dcache_wb_range((vm_offset_t)pend_base, + roundup2(LPI_PENDTAB_SIZE, PAGE_SIZE_64K)); - gic_sc->gic_redists.lpis.pend_base[cpuid] = pend_base; + if (bootverbose) { + device_printf(sc->dev, + "LPI Pending Table for CPU%u at PA: 0x%lx\n", + cpuid, vtophys(pend_base)); + } - lpi_config_cpu(sc); + gic_sc->gic_redists.lpis.pend_base[cpuid] = pend_base; + } + + /* Ensure visibility of pend_base addresses on other CPUs */ + wmb(); } static int @@ -683,6 +719,9 @@ gic_sc = device_get_softc(parent); cpuid = PCPU_GET(cpuid); + /* Ensure data observability on a current CPU */ + rmb(); + conf_base = gic_sc->gic_redists.lpis.conf_base; pend_base = gic_sc->gic_redists.lpis.pend_base[cpuid]; @@ -1379,7 +1418,7 @@ * to be bound to the CPU that performs the configuration. */ cpuid = PCPU_GET(cpuid); - newdev->col = &sc->its_cols[cpuid]; + newdev->col = sc->its_cols[cpuid]; TAILQ_INSERT_TAIL(&sc->its_dev_list, newdev, entry); Index: head/sys/arm64/arm64/gic_v3_reg.h =================================================================== --- head/sys/arm64/arm64/gic_v3_reg.h +++ head/sys/arm64/arm64/gic_v3_reg.h @@ -356,6 +356,12 @@ /* * CPU interface */ +#define GICI_SGI_TLIST_MASK (0xffffUL) +#define GICI_SGI_AFF1_SHIFT (16UL) +#define GICI_SGI_AFF2_SHIFT (32UL) +#define GICI_SGI_AFF3_SHIFT (48UL) +#define GICI_SGI_IPI_MASK (0xfUL) +#define GICI_SGI_IPI_SHIFT (24UL) /* * Registers list (ICC_xyz_EL1): Index: head/sys/arm64/arm64/gic_v3_var.h =================================================================== --- head/sys/arm64/arm64/gic_v3_var.h +++ head/sys/arm64/arm64/gic_v3_var.h @@ -221,7 +221,7 @@ struct its_cmd * its_cmdq_base; /* ITS command queue base */ struct its_cmd * its_cmdq_write; /* ITS command queue write ptr */ struct its_ptab its_ptabs[GITS_BASER_NUM];/* ITS private tables */ - struct its_col * its_cols; /* Per-CPU collections */ + struct its_col * its_cols[MAXCPU];/* Per-CPU collections */ uint64_t its_flags; @@ -253,6 +253,8 @@ int gic_v3_its_alloc_msi(device_t, device_t, int, int *); int gic_v3_its_map_msix(device_t, device_t, int, uint64_t *, uint32_t *); +int its_init_cpu(struct gic_v3_its_softc *); + void lpi_unmask_irq(device_t, uint32_t); void lpi_mask_irq(device_t, uint32_t); /* Index: head/sys/arm64/include/cpu.h =================================================================== --- head/sys/arm64/include/cpu.h +++ head/sys/arm64/include/cpu.h @@ -57,7 +57,12 @@ #define CPU_AFF1(mpidr) (u_int)(((mpidr) >> 8) & 0xff) #define CPU_AFF2(mpidr) (u_int)(((mpidr) >> 16) & 0xff) #define CPU_AFF3(mpidr) (u_int)(((mpidr) >> 32) & 0xff) -#define CPU_AFF_MASK 0xff00ffffffUL /* Mask affinity fields in MPIDR_EL1 */ +#define CPU_AFF0_MASK 0xffUL +#define CPU_AFF1_MASK 0xff00UL +#define CPU_AFF2_MASK 0xff0000UL +#define CPU_AFF3_MASK 0xff00000000UL +#define CPU_AFF_MASK (CPU_AFF0_MASK | CPU_AFF1_MASK | \ + CPU_AFF2_MASK| CPU_AFF3_MASK) /* Mask affinity fields in MPIDR_EL1 */ #ifdef _KERNEL