 @@ -1,14 +1,14 @@
-/* $NetBSD: gicv3.c,v 1.13 2018/11/23 11:49:04 jmcneill Exp $ */
+/* $NetBSD: gicv3.c,v 1.14 2019/06/12 10:02:17 jmcneill Exp $ */
 /*-
  * Copyright (c) 2018 Jared McNeill <jmcneill@invisible.ca>
  * 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.
 @@ -21,27 +21,27 @@
  * 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 "opt_multiprocessor.h"
 #define	_INTR_PRIVATE
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: gicv3.c,v 1.13 2018/11/23 11:49:04 jmcneill Exp $");
+__KERNEL_RCSID(0, "$NetBSD: gicv3.c,v 1.14 2019/06/12 10:02:17 jmcneill Exp $");
 #include <sys/param.h>
 #include <sys/kernel.h>
 #include <sys/bus.h>
 #include <sys/device.h>
 #include <sys/intr.h>
 #include <sys/systm.h>
 #include <sys/cpu.h>
 #include <arm/locore.h>
 #include <arm/armreg.h>
 #include <arm/cortex/gicv3.h>
 @@ -145,27 +145,27 @@ gicv3_block_irqs(struct pic_softc *pic,
+			;
+	}
+}
 static void
 gicv3_establish_irq(struct pic_softc *pic, struct intrsource *is)
+{
 	struct gicv3_softc * const sc = PICTOSOFTC(pic);
 	const u_int group = is->is_irq / 32;
 	uint32_t ipriority, icfg;
 	uint64_t irouter;
 	u_int n;
-	const u_int ipriority_val = 0x80 | IPL_TO_PRIORITY(is->is_ipl);
+	const u_int ipriority_val = 0x80 | (IPL_TO_PRIORITY(is->is_ipl) >> 1);
 	const u_int ipriority_shift = (is->is_irq & 0x3) * 8;
 	const u_int icfg_shift = (is->is_irq & 0xf) * 2;
 	if (group == 0) {
 		/* SGIs and PPIs are always MP-safe */
 		is->is_mpsafe = true;
 		/* Update interrupt configuration and priority on all redistributors */
 		for (n = 0; n < sc->sc_bsh_r_count; n++) {
 			icfg = gicr_read_4(sc, n, GICR_ICFGRn(is->is_irq / 16));
 			if (is->is_type == IST_LEVEL)
 				icfg &= ~(0x2 << icfg_shift);
 			if (is->is_type == IST_EDGE)
 @@ -196,27 +196,27 @@ gicv3_establish_irq(struct pic_softc *pi
 		gicd_write_4(sc, GICD_ICFGRn(is->is_irq / 16), icfg);
 		/* Update interrupt priority */
 		ipriority = gicd_read_4(sc, GICD_IPRIORITYRn(is->is_irq / 4));
 		ipriority &= ~(0xff << ipriority_shift);
 		ipriority |= (ipriority_val << ipriority_shift);
 		gicd_write_4(sc, GICD_IPRIORITYRn(is->is_irq / 4), ipriority);
+	}
+}
 static void
 gicv3_set_priority(struct pic_softc *pic, int ipl)
+{
-	icc_pmr_write(IPL_TO_PRIORITY(ipl) << 1);
+	icc_pmr_write(IPL_TO_PRIORITY(ipl));
+}
 static void
 gicv3_dist_enable(struct gicv3_softc *sc)
+{
 	uint32_t gicd_ctrl;
 	u_int n;
 	/* Disable the distributor */
 	gicd_write_4(sc, GICD_CTRL, 0);
 	/* Wait for register write to complete */
 	while (gicd_read_4(sc, GICD_CTRL) & GICD_CTRL_RWP)
 @@ -261,27 +261,27 @@ gicv3_redist_enable(struct gicv3_softc *
 	/* Wait for register write to complete */
 	while (gicr_read_4(sc, ci->ci_gic_redist, GICR_CTLR) & GICR_CTLR_RWP)
+		;
 	/* Set default priorities */
 	for (n = 0; n < 32; n += 4) {
 		uint32_t priority = 0;
 		size_t byte_shift = 0;
 		for (o = 0; o < 4; o++, byte_shift += 8) {
 			struct intrsource * const is = sc->sc_pic.pic_sources[n + o];
 			if (is == NULL)
 				priority |= 0xff << byte_shift;
 			else {
-				const u_int ipriority_val = 0x80 | IPL_TO_PRIORITY(is->is_ipl);
+				const u_int ipriority_val = 0x80 | (IPL_TO_PRIORITY(is->is_ipl) >> 1);
 				priority |= ipriority_val << byte_shift;
+			}
+		}
 		gicr_write_4(sc, ci->ci_gic_redist, GICR_IPRIORITYRn(n / 4), priority);
+	}
 	/* Set all interrupts to G1NS */
 	gicr_write_4(sc, ci->ci_gic_redist, GICR_IGROUPR0, ~0);
 	gicr_write_4(sc, ci->ci_gic_redist, GICR_IGRPMODR0, 0);
 	/* Restore PPI configs */
 	for (n = 0, icfg = 0; n < 16; n++) {
 		struct intrsource * const is = sc->sc_pic.pic_sources[16 + n];
 @@ -535,27 +535,27 @@ gicv3_lpi_block_irqs(struct pic_softc *p
 	while ((bit = ffs(mask)) != 0) {
 		sc->sc_lpiconf.base[irqbase + bit - 1] &= ~GIC_LPICONF_Enable;
 		mask &= ~__BIT(bit - 1);
+	}
 	bus_dmamap_sync(sc->sc_dmat, sc->sc_lpiconf.map, irqbase, 32, BUS_DMASYNC_PREWRITE);
+}
 static void
 gicv3_lpi_establish_irq(struct pic_softc *pic, struct intrsource *is)
+{
 	struct gicv3_softc * const sc = LPITOSOFTC(pic);
-	sc->sc_lpiconf.base[is->is_irq] = 0x80 | IPL_TO_PRIORITY(is->is_ipl) | GIC_LPICONF_Res1;
+	sc->sc_lpiconf.base[is->is_irq] = 0x80 | (IPL_TO_PRIORITY(is->is_ipl) >> 1) | GIC_LPICONF_Res1;
 	bus_dmamap_sync(sc->sc_dmat, sc->sc_lpiconf.map, is->is_irq, 1, BUS_DMASYNC_PREWRITE);
+}
 static void
 gicv3_lpi_cpu_init(struct pic_softc *pic, struct cpu_info *ci)
+{
 	struct gicv3_softc * const sc = LPITOSOFTC(pic);
 	struct gicv3_lpi_callback *cb;
 	uint32_t ctlr;
 	/* If physical LPIs are not supported on this redistributor, just return. */
 	const uint64_t typer = gicr_read_8(sc, ci->ci_gic_redist, GICR_TYPER);