Index: sys/arch/arm/cortex/gic.c
===================================================================
RCS file: /cvsroot/src/sys/arch/arm/cortex/gic.c,v
retrieving revision 1.10
diff -u -p -r1.10 gic.c
--- sys/arch/arm/cortex/gic.c	19 May 2014 22:47:53 -0000	1.10
+++ sys/arch/arm/cortex/gic.c	27 Oct 2014 23:09:56 -0000
@@ -147,14 +147,12 @@ armgic_ipl_to_priority(int ipl)
 	    | ((IPL_HIGH - ipl) * GICC_PMR_NS_PRIORITIES / NIPL);
 }
 
-#if 0
 static inline int
 armgic_priority_to_ipl(uint32_t priority)
 {
 	return IPL_HIGH
 	    - (priority & ~GICC_PMR_NONSECURE) * NIPL / GICC_PMR_NS_PRIORITIES;
 }
-#endif
 
 static void
 armgic_unblock_irqs(struct pic_softc *pic, size_t irq_base, uint32_t irq_mask)
@@ -180,7 +178,7 @@ armgic_block_irqs(struct pic_softc *pic,
 	gicd_write(sc, GICD_ICENABLERn(group), irq_mask);
 }
 
-static uint32_t armgic_last_priority;
+/*static*/ uint32_t armgic_last_priority;
 
 static void
 armgic_set_priority(struct pic_softc *pic, int ipl)
@@ -192,6 +190,19 @@ armgic_set_priority(struct pic_softc *pi
 	armgic_last_priority = priority;
 }
 
+
+static int
+armgic_get_priority(struct pic_softc *pic)
+{
+       struct armgic_softc * const sc = PICTOSOFTC(pic);
+       uint32_t pmr_val;
+
+       pmr_val = gicc_read(sc, GICC_PMR);
+       return IPL_HIGH
+           - (((pmr_val & ~GICC_PMR_NONSECURE) * NIPL) / GICC_PMR_NS_PRIORITIES);
+}
+
+
 #ifdef __HAVE_PIC_FAST_SOFTINTS
 void
 softint_init_md(lwp_t *l, u_int level, uintptr_t *machdep_p)
@@ -220,6 +231,8 @@ armgic_irq_handler(void *tf)
 	struct cpu_info * const ci = curcpu();
 	struct armgic_softc * const sc = &armgic_softc;
 	const int old_ipl = ci->ci_cpl;
+	uint32_t pmr = gicc_read(sc, GICC_PMR);
+	const int cur_ipl = armgic_priority_to_ipl(pmr);
 #ifdef DIAGNOSTIC
 	const int old_mtx_count = ci->ci_mtx_count;
 	const int old_l_biglocks = ci->ci_curlwp->l_biglocks;
@@ -232,6 +245,8 @@ armgic_irq_handler(void *tf)
 
 	KASSERTMSG(old_ipl != IPL_HIGH, "old_ipl %d pmr %#x hppir %#x",
 	    old_ipl, gicc_read(sc, GICC_PMR), gicc_read(sc, GICC_HPPIR));
+	KASSERTMSG(cur_ipl == ci->ci_cpl, "ci->ci_cpl %d, cur_ipl %d",
+	    ci->ci_cpl, cur_ipl);
 #if 0
 	printf("%s(enter): %s: pmr=%u hppir=%u\n",
 	    __func__, ci->ci_data.cpu_name,
@@ -252,6 +267,7 @@ armgic_irq_handler(void *tf)
 				break;
 			//printf(".%u", irq);
 		}
+		KASSERT(ci->ci_cpl != IPL_HIGH);
 
 		//const uint32_t cpuid = __SHIFTOUT(iar, GICC_IAR_CPUID_MASK);
 		struct intrsource * const is = sc->sc_pic.pic_sources[irq];
@@ -269,14 +285,12 @@ armgic_irq_handler(void *tf)
 		 *
 		 * However, if are just raising ipl, we can just update ci_cpl.
 		 */
-#if 0
 		const int ipl = armgic_priority_to_ipl(gicc_read(sc, GICC_RPR));
-		KASSERTMSG(panicstr != NULL || ipl == is->is_ipl,
+		KASSERTMSG(panicstr != NULL || ipl <= is->is_ipl,
 		    "%s: irq %d: running ipl %d != source ipl %u", 
 		    ci->ci_data.cpu_name, irq, ipl, is->is_ipl);
-#else
-		const int ipl = is->is_ipl;
-#endif
+
+		KASSERT(is->is_ipl < ipl);
 		if (__predict_false(ipl < ci->ci_cpl)) {
 			//printf("<");
 			pic_do_pending_ints(I32_bit, ipl, tf);
@@ -296,8 +310,8 @@ armgic_irq_handler(void *tf)
 		gicc_write(sc, GICC_EOIR, iar);
 #ifdef DEBUG
 		n++;
-		KDASSERTMSG(n < 5, "%s: processed too many (%zu)",
-		    ci->ci_data.cpu_name, n);
+		KDASSERTMSG(n < 5, "%s: processed too many (%zu) irq %d ipl %d",
+		    ci->ci_data.cpu_name, n, irq, ipl);
 #endif
 	}