Index: projects/arm_intrng/sys/arm/arm/gic.c =================================================================== --- projects/arm_intrng/sys/arm/arm/gic.c (revision 276185) +++ projects/arm_intrng/sys/arm/arm/gic.c (revision 276186) @@ -1,560 +1,564 @@ /*- * Copyright (c) 2011 The FreeBSD Foundation * All rights reserved. * * Developed by Damjan Marion * * Based on OMAP4 GIC code by Ben Gray * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * 3. The name of the company nor the name of the author may be used to * endorse or promote products derived from this software without specific * prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. */ #include __FBSDID("$FreeBSD$"); #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef ARM_INTRNG #include "pic_if.h" #endif /* We are using GICv2 register naming */ /* Distributor Registers */ #define GICD_CTLR 0x000 /* v1 ICDDCR */ #define GICD_TYPER 0x004 /* v1 ICDICTR */ #define GICD_IIDR 0x008 /* v1 ICDIIDR */ #define GICD_IGROUPR(n) (0x0080 + ((n) * 4)) /* v1 ICDISER */ #define GICD_ISENABLER(n) (0x0100 + ((n) * 4)) /* v1 ICDISER */ #define GICD_ICENABLER(n) (0x0180 + ((n) * 4)) /* v1 ICDICER */ #define GICD_ISPENDR(n) (0x0200 + ((n) * 4)) /* v1 ICDISPR */ #define GICD_ICPENDR(n) (0x0280 + ((n) * 4)) /* v1 ICDICPR */ #define GICD_ICACTIVER(n) (0x0380 + ((n) * 4)) /* v1 ICDABR */ #define GICD_IPRIORITYR(n) (0x0400 + ((n) * 4)) /* v1 ICDIPR */ #define GICD_ITARGETSR(n) (0x0800 + ((n) * 4)) /* v1 ICDIPTR */ #define GICD_ICFGR(n) (0x0C00 + ((n) * 4)) /* v1 ICDICFR */ #define GICD_SGIR(n) (0x0F00 + ((n) * 4)) /* v1 ICDSGIR */ /* CPU Registers */ #define GICC_CTLR 0x0000 /* v1 ICCICR */ #define GICC_PMR 0x0004 /* v1 ICCPMR */ #define GICC_BPR 0x0008 /* v1 ICCBPR */ #define GICC_IAR 0x000C /* v1 ICCIAR */ #define GICC_EOIR 0x0010 /* v1 ICCEOIR */ #define GICC_RPR 0x0014 /* v1 ICCRPR */ #define GICC_HPPIR 0x0018 /* v1 ICCHPIR */ #define GICC_ABPR 0x001C /* v1 ICCABPR */ #define GICC_IIDR 0x00FC /* v1 ICCIIDR*/ #define GIC_FIRST_IPI 0 /* Irqs 0-15 are SGIs/IPIs. */ #define GIC_LAST_IPI 15 #define GIC_FIRST_PPI 16 /* Irqs 16-31 are private (per */ #define GIC_LAST_PPI 31 /* core) peripheral interrupts. */ #define GIC_FIRST_SPI 32 /* Irqs 32+ are shared peripherals. */ /* First bit is a polarity bit (0 - low, 1 - high) */ #define GICD_ICFGR_POL_LOW (0 << 0) #define GICD_ICFGR_POL_HIGH (1 << 0) #define GICD_ICFGR_POL_MASK 0x1 /* Second bit is a trigger bit (0 - level, 1 - edge) */ #define GICD_ICFGR_TRIG_LVL (0 << 1) #define GICD_ICFGR_TRIG_EDGE (1 << 1) #define GICD_ICFGR_TRIG_MASK 0x2 struct arm_gic_softc { device_t gic_dev; struct resource * gic_res[3]; bus_space_tag_t gic_c_bst; bus_space_tag_t gic_d_bst; bus_space_handle_t gic_c_bsh; bus_space_handle_t gic_d_bsh; void * gic_intrhand; uint8_t ver; struct mtx mutex; uint32_t nirqs; }; static struct resource_spec arm_gic_spec[] = { { SYS_RES_MEMORY, 0, RF_ACTIVE }, /* Distributor registers */ { SYS_RES_MEMORY, 1, RF_ACTIVE }, /* CPU Interrupt Intf. registers */ { SYS_RES_IRQ, 0, RF_ACTIVE | RF_OPTIONAL }, /* Parent interrupt */ { -1, 0 } }; static struct arm_gic_softc *arm_gic_sc = NULL; static int arm_gic_probe(device_t); static int arm_gic_attach(device_t); static void arm_gic_init_secondary(device_t); static int arm_gic_intr(void *); static int arm_gic_config(device_t, int, enum intr_trigger, enum intr_polarity); static void arm_gic_eoi(device_t, int); static void arm_gic_mask(device_t, int); static void arm_gic_unmask(device_t, int); static void arm_gic_ipi_send(device_t, cpuset_t, int); +static int arm_gic_ipi_read(device_t dev, int i); static void arm_gic_ipi_clear(device_t, int); #define gic_c_read_4(_sc, _reg) \ bus_space_read_4((_sc)->gic_c_bst, (_sc)->gic_c_bsh, (_reg)) #define gic_c_write_4(_sc, _reg, _val) \ bus_space_write_4((_sc)->gic_c_bst, (_sc)->gic_c_bsh, (_reg), (_val)) #define gic_d_read_4(_sc, _reg) \ bus_space_read_4((_sc)->gic_d_bst, (_sc)->gic_d_bsh, (_reg)) #define gic_d_write_4(_sc, _reg, _val) \ bus_space_write_4((_sc)->gic_d_bst, (_sc)->gic_d_bsh, (_reg), (_val)) static struct ofw_compat_data compat_data[] = { {"arm,gic", true}, /* Non-standard, used in FreeBSD dts. */ {"arm,gic-400", true}, {"arm,cortex-a15-gic", true}, {"arm,cortex-a9-gic", true}, {"arm,cortex-a7-gic", true}, {"arm,arm11mp-gic", true}, {"brcm,brahma-b15-gic", true}, {NULL, false} }; static int arm_gic_probe(device_t dev) { if (!ofw_bus_status_okay(dev)) return (ENXIO); if (!ofw_bus_search_compatible(dev, compat_data)->ocd_data) return (ENXIO); device_set_desc(dev, "ARM Generic Interrupt Controller"); return (BUS_PROBE_DEFAULT); } static void arm_gic_init_secondary(device_t dev) { struct arm_gic_softc *sc = device_get_softc(dev); int i; for (i = 0; i < sc->nirqs; i += 4) gic_d_write_4(sc, GICD_IPRIORITYR(i >> 2), 0); /* Set all the interrupts to be in Group 0 (secure) */ for (i = 0; i < sc->nirqs; i += 32) { gic_d_write_4(sc, GICD_IGROUPR(i >> 5), 0); } /* Enable CPU interface */ gic_c_write_4(sc, GICC_CTLR, 1); /* Set priority mask register. */ gic_c_write_4(sc, GICC_PMR, 0xff); /* Enable interrupt distribution */ gic_d_write_4(sc, GICD_CTLR, 0x01); /* * Activate the timer interrupts: virtual, secure, and non-secure. */ gic_d_write_4(sc, GICD_ISENABLER(27 >> 5), (1UL << (27 & 0x1F))); gic_d_write_4(sc, GICD_ISENABLER(29 >> 5), (1UL << (29 & 0x1F))); gic_d_write_4(sc, GICD_ISENABLER(30 >> 5), (1UL << (30 & 0x1F))); } int gic_decode_fdt(uint32_t iparent, uint32_t *intr, int *interrupt, int *trig, int *pol) { static u_int num_intr_cells; if (num_intr_cells == 0) { if (OF_searchencprop(OF_node_from_xref(iparent), "#interrupt-cells", &num_intr_cells, sizeof(num_intr_cells)) == -1) { num_intr_cells = 1; } } if (num_intr_cells == 1) { *interrupt = fdt32_to_cpu(intr[0]); *trig = INTR_TRIGGER_CONFORM; *pol = INTR_POLARITY_CONFORM; } else { if (intr[0] == 0) *interrupt = fdt32_to_cpu(intr[1]) + GIC_FIRST_SPI; else *interrupt = fdt32_to_cpu(intr[1]) + GIC_FIRST_PPI; /* * In intr[2], bits[3:0] are trigger type and level flags. * 1 = low-to-high edge triggered * 2 = high-to-low edge triggered * 4 = active high level-sensitive * 8 = active low level-sensitive * The hardware only supports active-high-level or rising-edge. */ if (intr[2] & 0x0a) { printf("unsupported trigger/polarity configuration " "0x%2x\n", intr[2] & 0x0f); return (ENOTSUP); } *pol = INTR_POLARITY_CONFORM; if (intr[2] & 0x01) *trig = INTR_TRIGGER_EDGE; else *trig = INTR_TRIGGER_LEVEL; } return (0); } static int arm_gic_attach(device_t dev) { struct arm_gic_softc *sc; int i; uint32_t icciidr; if (arm_gic_sc) return (ENXIO); sc = device_get_softc(dev); if (bus_alloc_resources(dev, arm_gic_spec, sc->gic_res)) { device_printf(dev, "could not allocate resources\n"); return (ENXIO); } sc->gic_dev = dev; arm_gic_sc = sc; /* Initialize mutex */ mtx_init(&sc->mutex, "GIC lock", "", MTX_SPIN); /* Distributor Interface */ sc->gic_d_bst = rman_get_bustag(sc->gic_res[0]); sc->gic_d_bsh = rman_get_bushandle(sc->gic_res[0]); /* CPU Interface */ sc->gic_c_bst = rman_get_bustag(sc->gic_res[1]); sc->gic_c_bsh = rman_get_bushandle(sc->gic_res[1]); +#ifdef ARM_INTRNG if (bus_setup_intr(dev, sc->gic_res[2], INTR_TYPE_MISC | INTR_CONTROLLER, arm_gic_intr, NULL, sc, &sc->gic_intrhand)) { device_printf(dev, "could not setup interrupt handler\n"); bus_release_resources(dev, arm_gic_spec, sc->gic_res); return (ENXIO); } arm_register_pic(dev, PIC_FEATURE_IPI); + for (int i = 0; i < ARM_IPI_COUNT; i++) arm_ipi_map_irq(dev, i, i); +#endif /* Disable interrupt forwarding to the CPU interface */ gic_d_write_4(sc, GICD_CTLR, 0x00); /* Get the number of interrupts */ sc->nirqs = gic_d_read_4(sc, GICD_TYPER); sc->nirqs = 32 * ((sc->nirqs & 0x1f) + 1); icciidr = gic_c_read_4(sc, GICC_IIDR); device_printf(dev,"pn 0x%x, arch 0x%x, rev 0x%x, implementer 0x%x irqs %u\n", icciidr>>20, (icciidr>>16) & 0xF, (icciidr>>12) & 0xf, (icciidr & 0xfff), sc->nirqs); /* Set all global interrupts to be level triggered, active low. */ for (i = 32; i < sc->nirqs; i += 16) { gic_d_write_4(sc, GICD_ICFGR(i >> 4), 0x00000000); } /* Disable all interrupts. */ for (i = 32; i < sc->nirqs; i += 32) { gic_d_write_4(sc, GICD_ICENABLER(i >> 5), 0xFFFFFFFF); } for (i = 0; i < sc->nirqs; i += 4) { gic_d_write_4(sc, GICD_IPRIORITYR(i >> 2), 0); gic_d_write_4(sc, GICD_ITARGETSR(i >> 2), 1 << 0 | 1 << 8 | 1 << 16 | 1 << 24); } /* Set all the interrupts to be in Group 0 (secure) */ for (i = 0; i < sc->nirqs; i += 32) { gic_d_write_4(sc, GICD_IGROUPR(i >> 5), 0); } /* Enable CPU interface */ gic_c_write_4(sc, GICC_CTLR, 1); /* Set priority mask register. */ gic_c_write_4(sc, GICC_PMR, 0xff); /* Enable interrupt distribution */ gic_d_write_4(sc, GICD_CTLR, 0x01); return (0); } static int arm_gic_intr(void *arg) { struct arm_gic_softc *sc = (struct arm_gic_softc *)arg; uint32_t active_irq, last_irq = 0; active_irq = gic_c_read_4(sc, GICC_IAR); /* * Immediatly EOIR the SGIs, because doing so requires the other * bits (ie CPU number), not just the IRQ number, and we do not * have this information later. */ if ((active_irq & 0x3ff) <= GIC_LAST_IPI) gic_c_write_4(sc, GICC_EOIR, active_irq); active_irq &= 0x3FF; if (active_irq == 0x3FF) { if (last_irq == -1) printf("Spurious interrupt detected\n"); return (FILTER_HANDLED); } //gic_c_write_4(sc, GICC_EOIR, active_irq); arm_dispatch_irq(sc->gic_dev, NULL, active_irq); return (FILTER_HANDLED); } static int arm_gic_config(device_t dev, int irq, enum intr_trigger trig, enum intr_polarity pol) { struct arm_gic_softc *sc = device_get_softc(dev); uint32_t reg; uint32_t mask; /* Function is public-accessible, so validate input arguments */ if ((irq < 0) || (irq >= sc->nirqs)) goto invalid_args; if ((trig != INTR_TRIGGER_EDGE) && (trig != INTR_TRIGGER_LEVEL) && (trig != INTR_TRIGGER_CONFORM)) goto invalid_args; if ((pol != INTR_POLARITY_HIGH) && (pol != INTR_POLARITY_LOW) && (pol != INTR_POLARITY_CONFORM)) goto invalid_args; mtx_lock_spin(&sc->mutex); reg = gic_d_read_4(sc, GICD_ICFGR(irq >> 4)); mask = (reg >> 2*(irq % 16)) & 0x3; if (pol == INTR_POLARITY_LOW) { mask &= ~GICD_ICFGR_POL_MASK; mask |= GICD_ICFGR_POL_LOW; } else if (pol == INTR_POLARITY_HIGH) { mask &= ~GICD_ICFGR_POL_MASK; mask |= GICD_ICFGR_POL_HIGH; } if (trig == INTR_TRIGGER_LEVEL) { mask &= ~GICD_ICFGR_TRIG_MASK; mask |= GICD_ICFGR_TRIG_LVL; } else if (trig == INTR_TRIGGER_EDGE) { mask &= ~GICD_ICFGR_TRIG_MASK; mask |= GICD_ICFGR_TRIG_EDGE; } /* Set mask */ reg = reg & ~(0x3 << 2*(irq % 16)); reg = reg | (mask << 2*(irq % 16)); gic_d_write_4(sc, GICD_ICFGR(irq >> 4), reg); mtx_unlock_spin(&sc->mutex); return (0); invalid_args: device_printf(dev, "gic_config_irg, invalid parameters\n"); return (EINVAL); } static void arm_gic_eoi(device_t dev, int irq) { struct arm_gic_softc *sc = device_get_softc(dev); if (irq > GIC_LAST_IPI) arm_irq_memory_barrier(irq); gic_c_write_4(sc, GICC_EOIR, irq); } static void arm_gic_mask(device_t dev, int irq) { struct arm_gic_softc *sc = device_get_softc(dev); gic_d_write_4(sc, GICD_ICENABLER(irq >> 5), (1UL << (irq & 0x1F))); gic_c_write_4(sc, GICC_EOIR, irq); } static void arm_gic_unmask(device_t dev, int irq) { struct arm_gic_softc *sc = device_get_softc(dev); if (irq > GIC_LAST_IPI) arm_irq_memory_barrier(irq); gic_d_write_4(sc, GICD_ISENABLER(irq >> 5), (1UL << (irq & 0x1F))); } static void arm_gic_ipi_send(device_t dev, cpuset_t cpus, int ipi) { struct arm_gic_softc *sc = device_get_softc(dev); uint32_t val = 0, i; for (i = 0; i < MAXCPU; i++) if (CPU_ISSET(i, &cpus)) val |= 1 << (16 + i); gic_d_write_4(sc, GICD_SGIR(0), val | ipi); } static int arm_gic_ipi_read(device_t dev, int i) { if (i != -1) { /* * The intr code will automagically give the frame pointer * if the interrupt argument is 0. */ if ((unsigned int)i > 16) return (0); return (i); } return (0x3ff); } static void arm_gic_ipi_clear(device_t dev, int ipi) { /* no-op */ } #ifndef ARM_INTRNG void arm_mask_irq(uintptr_t nb) { arm_gic_mask(arm_gic_sc->gic_dev, nb); } void arm_unmask_irq(uintptr_t nb) { arm_gic_unmask(arm_gic_sc->gic_dev, nb); } void gic_init_secondary(void) { arm_gic_init_secondary(arm_gic_sc->gic_dev); } void pic_ipi_send(cpuset_t cpus, u_int ipi) { arm_gic_ipi_send(arm_gic_sc->gic_dev, cpus, ipi); } int pic_ipi_read(int i) { - return (arm_gic_pid_ipi_read(arm_gic_sc->gic_dev, i); + return (arm_gic_ipi_read(arm_gic_sc->gic_dev, i)); } void pic_ipi_clear(int ipi) { arm_gic_ipi_clear(arm_gic_sc->gic_dev, ipi); } #endif static device_method_t arm_gic_methods[] = { /* Device interface */ DEVMETHOD(device_probe, arm_gic_probe), DEVMETHOD(device_attach, arm_gic_attach), #ifdef ARM_INTRNG /* Interrupt controller interface */ DEVMETHOD(pic_config, arm_gic_config), DEVMETHOD(pic_mask, arm_gic_mask), DEVMETHOD(pic_unmask, arm_gic_unmask), DEVMETHOD(pic_eoi, arm_gic_eoi), DEVMETHOD(pic_init_secondary, arm_gic_init_secondary), DEVMETHOD(pic_ipi_send, arm_gic_ipi_send), DEVMETHOD(pic_ipi_clear, arm_gic_ipi_clear), DEVMETHOD(pic_ipi_read, arm_gic_ipi_read), #endif { 0, 0 } }; static driver_t arm_gic_driver = { "gic", arm_gic_methods, sizeof(struct arm_gic_softc), }; static devclass_t arm_gic_devclass; EARLY_DRIVER_MODULE(gic, simplebus, arm_gic_driver, arm_gic_devclass, 0, 0, BUS_PASS_INTERRUPT + BUS_PASS_ORDER_MIDDLE); EARLY_DRIVER_MODULE(gic, ofwbus, arm_gic_driver, arm_gic_devclass, 0, 0, BUS_PASS_INTERRUPT + BUS_PASS_ORDER_MIDDLE); Index: projects/arm_intrng/sys/arm/arm/mp_machdep.c =================================================================== --- projects/arm_intrng/sys/arm/arm/mp_machdep.c (revision 276185) +++ projects/arm_intrng/sys/arm/arm/mp_machdep.c (revision 276186) @@ -1,441 +1,445 @@ /*- * Copyright (c) 2011 Semihalf. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * 1. Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright * notice, this list of conditions and the following disclaimer in the * documentation and/or other materials provided with the distribution. * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF * SUCH DAMAGE. */ #include __FBSDID("$FreeBSD$"); #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef VFP #include #endif #ifdef CPU_MV_PJ4B #include #include #endif #include "opt_smp.h" void *temp_pagetable; extern struct pcpu __pcpu[]; /* used to hold the AP's until we are ready to release them */ struct mtx ap_boot_mtx; struct pcb stoppcbs[MAXCPU]; /* # of Applications processors */ volatile int mp_naps; /* Set to 1 once we're ready to let the APs out of the pen. */ volatile int aps_ready = 0; static int ipi_handler(void *arg); void set_stackptrs(int cpu); /* Temporary variables for init_secondary() */ void *dpcpu[MAXCPU - 1]; /* Determine if we running MP machine */ int cpu_mp_probe(void) { CPU_SETOF(0, &all_cpus); return (platform_mp_probe()); } /* Start Application Processor via platform specific function */ static int check_ap(void) { uint32_t ms; for (ms = 0; ms < 2000; ++ms) { if ((mp_naps + 1) == mp_ncpus) return (0); /* success */ else DELAY(1000); } return (-2); } extern unsigned char _end[]; /* Initialize and fire up non-boot processors */ void cpu_mp_start(void) { int error, i; vm_offset_t temp_pagetable_va; vm_paddr_t addr, addr_end; mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN); /* Reserve memory for application processors */ for(i = 0; i < (mp_ncpus - 1); i++) dpcpu[i] = (void *)kmem_malloc(kernel_arena, DPCPU_SIZE, M_WAITOK | M_ZERO); temp_pagetable_va = (vm_offset_t)contigmalloc(L1_TABLE_SIZE, M_TEMP, 0, 0x0, 0xffffffff, L1_TABLE_SIZE, 0); addr = arm_physmem_kernaddr; addr_end = (vm_offset_t)&_end - KERNVIRTADDR + arm_physmem_kernaddr; addr_end &= ~L1_S_OFFSET; addr_end += L1_S_SIZE; bzero((void *)temp_pagetable_va, L1_TABLE_SIZE); for (addr = arm_physmem_kernaddr; addr <= addr_end; addr += L1_S_SIZE) { ((int *)(temp_pagetable_va))[addr >> L1_S_SHIFT] = L1_TYPE_S|L1_SHARED|L1_S_C|L1_S_B|L1_S_AP(AP_KRW)|L1_S_DOM(PMAP_DOMAIN_KERNEL)|addr; ((int *)(temp_pagetable_va))[(addr - arm_physmem_kernaddr + KERNVIRTADDR) >> L1_S_SHIFT] = L1_TYPE_S|L1_SHARED|L1_S_C|L1_S_B|L1_S_AP(AP_KRW)|L1_S_DOM(PMAP_DOMAIN_KERNEL)|addr; } #if defined(CPU_MV_PJ4B) /* Add ARMADAXP registers required for snoop filter initialization */ ((int *)(temp_pagetable_va))[MV_BASE >> L1_S_SHIFT] = L1_TYPE_S|L1_SHARED|L1_S_B|L1_S_AP(AP_KRW)|fdt_immr_pa; #endif temp_pagetable = (void*)(vtophys(temp_pagetable_va)); cpu_idcache_wbinv_all(); cpu_l2cache_wbinv_all(); /* Initialize boot code and start up processors */ platform_mp_start_ap(); /* Check if ap's started properly */ error = check_ap(); if (error) printf("WARNING: Some AP's failed to start\n"); else for (i = 1; i < mp_ncpus; i++) CPU_SET(i, &all_cpus); contigfree((void *)temp_pagetable_va, L1_TABLE_SIZE, M_TEMP); } /* Introduce rest of cores to the world */ void cpu_mp_announce(void) { } extern vm_paddr_t pmap_pa; void init_secondary(int cpu) { struct pcpu *pc; uint32_t loop_counter; int start = 0, end = 0; cpu_idcache_inv_all(); cpu_setup(NULL); setttb(pmap_pa); cpu_tlb_flushID(); pc = &__pcpu[cpu]; /* * pcpu_init() updates queue, so it should not be executed in parallel * on several cores */ while(mp_naps < (cpu - 1)) ; pcpu_init(pc, cpu, sizeof(struct pcpu)); dpcpu_init(dpcpu[cpu - 1], cpu); /* Provide stack pointers for other processor modes. */ set_stackptrs(cpu); /* Signal our startup to BSP */ atomic_add_rel_32(&mp_naps, 1); /* Spin until the BSP releases the APs */ while (!aps_ready) ; /* Initialize curthread */ KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread")); pc->pc_curthread = pc->pc_idlethread; pc->pc_curpcb = pc->pc_idlethread->td_pcb; set_curthread(pc->pc_idlethread); #ifdef VFP pc->pc_cpu = cpu; vfp_init(); #endif mtx_lock_spin(&ap_boot_mtx); atomic_add_rel_32(&smp_cpus, 1); if (smp_cpus == mp_ncpus) { /* enable IPI's, tlb shootdown, freezes etc */ atomic_store_rel_int(&smp_started, 1); } mtx_unlock_spin(&ap_boot_mtx); /* Enable ipi */ #ifdef IPI_IRQ_START start = IPI_IRQ_START; #ifdef IPI_IRQ_END end = IPI_IRQ_END; #else end = IPI_IRQ_START; #endif #endif for (int i = 0; i < ARM_IPI_COUNT; i++) +#ifdef ARM_INTRNG arm_unmask_ipi(i); +#else + arm_unmask_irq(i); +#endif enable_interrupts(PSR_I); loop_counter = 0; while (smp_started == 0) { DELAY(100); loop_counter++; if (loop_counter == 1000) CTR0(KTR_SMP, "AP still wait for smp_started"); } /* Start per-CPU event timers. */ cpu_initclocks_ap(); CTR0(KTR_SMP, "go into scheduler"); platform_mp_init_secondary(); /* Enter the scheduler */ sched_throw(NULL); panic("scheduler returned us to %s", __func__); /* NOTREACHED */ } static int ipi_handler(void *arg) { u_int cpu, ipi; cpu = PCPU_GET(cpuid); ipi = pic_ipi_read((int)arg); while ((ipi != 0x3ff)) { switch (ipi) { case IPI_RENDEZVOUS: CTR0(KTR_SMP, "IPI_RENDEZVOUS"); smp_rendezvous_action(); break; case IPI_AST: CTR0(KTR_SMP, "IPI_AST"); break; case IPI_STOP: /* * IPI_STOP_HARD is mapped to IPI_STOP so it is not * necessary to add it in the switch. */ CTR0(KTR_SMP, "IPI_STOP or IPI_STOP_HARD"); savectx(&stoppcbs[cpu]); /* * CPUs are stopped when entering the debugger and at * system shutdown, both events which can precede a * panic dump. For the dump to be correct, all caches * must be flushed and invalidated, but on ARM there's * no way to broadcast a wbinv_all to other cores. * Instead, we have each core do the local wbinv_all as * part of stopping the core. The core requesting the * stop will do the l2 cache flush after all other cores * have done their l1 flushes and stopped. */ cpu_idcache_wbinv_all(); /* Indicate we are stopped */ CPU_SET_ATOMIC(cpu, &stopped_cpus); /* Wait for restart */ while (!CPU_ISSET(cpu, &started_cpus)) cpu_spinwait(); CPU_CLR_ATOMIC(cpu, &started_cpus); CPU_CLR_ATOMIC(cpu, &stopped_cpus); CTR0(KTR_SMP, "IPI_STOP (restart)"); break; case IPI_PREEMPT: CTR1(KTR_SMP, "%s: IPI_PREEMPT", __func__); sched_preempt(curthread); break; case IPI_HARDCLOCK: CTR1(KTR_SMP, "%s: IPI_HARDCLOCK", __func__); hardclockintr(); break; case IPI_TLB: CTR1(KTR_SMP, "%s: IPI_TLB", __func__); cpufuncs.cf_tlb_flushID(); break; default: panic("Unknown IPI 0x%0x on cpu %d", ipi, curcpu); } pic_ipi_clear(ipi); ipi = pic_ipi_read(-1); } return (FILTER_HANDLED); } static void release_aps(void *dummy __unused) { uint32_t loop_counter; int start = 0, end = 0; if (mp_ncpus == 1) return; #ifdef IPI_IRQ_START start = IPI_IRQ_START; #ifdef IPI_IRQ_END end = IPI_IRQ_END; #else end = IPI_IRQ_START; #endif #endif for (int i = 0; i < ARM_IPI_COUNT; i++) { /* * IPI handler */ /* * Use 0xdeadbeef as the argument value for irq 0, * if we used 0, the intr code will give the trap frame * pointer instead. */ #ifdef ARM_INTRNG arm_setup_irqhandler(NULL, ipi_handler, NULL, (void *)i, i, INTR_TYPE_MISC | INTR_EXCL | INTR_IPI, NULL); arm_unmask_ipi(i); #else arm_setup_irqhandler("ipi", ipi_handler, NULL, (void *)i, i, INTR_TYPE_MISC | INTR_EXCL, NULL); /* Enable ipi */ arm_unmask_irq(i); #endif } atomic_store_rel_int(&aps_ready, 1); printf("Release APs\n"); for (loop_counter = 0; loop_counter < 2000; loop_counter++) { if (smp_started) return; DELAY(1000); } printf("AP's not started\n"); } SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL); struct cpu_group * cpu_topo(void) { return (smp_topo_1level(CG_SHARE_L2, mp_ncpus, 0)); } void cpu_mp_setmaxid(void) { platform_mp_setmaxid(); } /* Sending IPI */ void ipi_all_but_self(u_int ipi) { cpuset_t other_cpus; other_cpus = all_cpus; CPU_CLR(PCPU_GET(cpuid), &other_cpus); CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi); platform_ipi_send(other_cpus, ipi); } void ipi_cpu(int cpu, u_int ipi) { cpuset_t cpus; CPU_ZERO(&cpus); CPU_SET(cpu, &cpus); CTR3(KTR_SMP, "%s: cpu: %d, ipi: %x", __func__, cpu, ipi); platform_ipi_send(cpus, ipi); } void ipi_selected(cpuset_t cpus, u_int ipi) { CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi); platform_ipi_send(cpus, ipi); } void tlb_broadcast(int ipi) { if (smp_started) ipi_all_but_self(ipi); }