Index: sys/interrupt.h
===================================================================
RCS file: /home/ncvs/src/sys/sys/interrupt.h,v
retrieving revision 1.30
diff -u -r1.30 interrupt.h
--- sys/interrupt.h	7 Jan 2005 02:29:23 -0000	1.30
+++ sys/interrupt.h	8 May 2005 21:38:06 -0000
@@ -64,10 +64,12 @@
 struct ithd {
 	struct	mtx it_lock;
 	struct	thread *it_td;		/* Interrupt process. */
+	struct	thread *it_preempted;	/* Thread we preempted. */
 	LIST_ENTRY(ithd) it_list;	/* All interrupt threads. */
 	TAILQ_HEAD(, intrhand) it_handlers; /* Interrupt handlers. */
 	struct	ithd *it_interrupted;	/* Who we interrupted. */
 	void	(*it_disable)(uintptr_t); /* Enable interrupt source. */
+	void	(*it_ack)(uintptr_t);	/* Acknowledge interrupt. */
 	void	(*it_enable)(uintptr_t); /* Disable interrupt source. */
 	void	*it_md;			/* Hook for MD interrupt code. */
 	int	it_flags;		/* Interrupt-specific flags. */
@@ -113,8 +115,8 @@
 void	db_dump_ithread(struct ithd *ithd, int handlers);
 #endif
 int	ithread_create(struct ithd **ithread, uintptr_t vector, int flags,
-	    void (*disable)(uintptr_t), void (*enable)(uintptr_t),
-	    const char *fmt, ...) __printflike(6, 7);
+	    void (*disable)(uintptr_t), void (*ack)(uintptr_t),
+	    void (*enable)(uintptr_t), const char *fmt, ...) __printflike(7, 8);
 int	ithread_destroy(struct ithd *ithread);
 u_char	ithread_priority(enum intr_type flags);
 int	ithread_add_handler(struct ithd *ithread, const char *name,
@@ -122,6 +124,7 @@
 	    void **cookiep);
 int	ithread_remove_handler(void *cookie);
 int	ithread_schedule(struct ithd *ithread);
+int	ithread_switch(struct thread *td, struct thread *newtd);
 int     swi_add(struct ithd **ithdp, const char *name,
 	    driver_intr_t handler, void *arg, int pri, enum intr_type flags,
 	    void **cookiep);
Index: i386/i386/intr_machdep.c
===================================================================
RCS file: /home/ncvs/src/sys/i386/i386/intr_machdep.c,v
retrieving revision 1.14
diff -u -r1.14 intr_machdep.c
--- i386/i386/intr_machdep.c	12 Apr 2005 23:18:54 -0000	1.14
+++ i386/i386/intr_machdep.c	8 May 2005 21:38:06 -0000
@@ -67,6 +67,16 @@
 static void	intrcnt_setname(const char *name, int index);
 static void	intrcnt_updatename(struct intsrc *is);
 static void	intrcnt_register(struct intsrc *is);
+static void	pic_ithread_disable(uintptr_t arg);
+
+static void
+pic_ithread_disable(uintptr_t arg)
+{
+	struct intsrc *isrc;
+
+	isrc = (struct intsrc *)arg;
+	isrc->is_pic->pic_disable_source(isrc, PIC_EOI);
+}
 
 /*
  * Register a new interrupt source with the global interrupt system.
@@ -82,7 +92,7 @@
 	if (interrupt_sources[vector] != NULL)
 		return (EEXIST);
 	error = ithread_create(&isrc->is_ithread, (uintptr_t)isrc, 0,
-	    (mask_fn)isrc->is_pic->pic_disable_source,
+	    pic_ithread_disable, (mask_fn)isrc->is_pic->pic_eoi_source,
 	    (mask_fn)isrc->is_pic->pic_enable_source, "irq%d:", vector);
 	if (error)
 		return (error);
@@ -210,10 +220,10 @@
 		 * For stray and threaded interrupts, we mask and EOI the
 		 * source.
 		 */
-		isrc->is_pic->pic_disable_source(isrc, PIC_EOI);
-		if (ih == NULL)
+		if (ih == NULL) {
+			isrc->is_pic->pic_disable_source(isrc, PIC_EOI);
 			error = EINVAL;
-		else
+		} else
 			error = ithread_schedule(it);
 	}
 	if (error == EINVAL) {
Index: kern/kern_intr.c
===================================================================
RCS file: /home/ncvs/src/sys/kern/kern_intr.c,v
retrieving revision 1.123
diff -u -r1.123 kern_intr.c
--- kern/kern_intr.c	12 Apr 2005 23:18:54 -0000	1.123
+++ kern/kern_intr.c	8 May 2005 22:14:25 -0000
@@ -53,6 +53,7 @@
 #include <machine/cpu.h>
 #include <machine/md_var.h>
 #include <machine/stdarg.h>
+#include <sys/kdb.h>
 #ifdef DDB
 #include <ddb/ddb.h>
 #include <ddb/db_sym.h>
@@ -176,7 +177,8 @@
 
 int
 ithread_create(struct ithd **ithread, uintptr_t vector, int flags,
-    void (*disable)(uintptr_t), void (*enable)(uintptr_t), const char *fmt, ...)
+    void (*disable)(uintptr_t), void (*ack)(uintptr_t),
+    void (*enable)(uintptr_t), const char *fmt, ...)
 {
 	struct ithd *ithd;
 	struct thread *td;
@@ -191,6 +193,7 @@
 	ithd = malloc(sizeof(struct ithd), M_ITHREAD, M_WAITOK | M_ZERO);
 	ithd->it_vector = vector;
 	ithd->it_disable = disable;
+	ithd->it_ack = ack;
 	ithd->it_enable = enable;
 	ithd->it_flags = flags;
 	TAILQ_INIT(&ithd->it_handlers);
@@ -366,16 +369,31 @@
 int
 ithread_schedule(struct ithd *ithread)
 {
+	static struct ithd *lastithd;
+	static int ithdcount;
 	struct int_entropy entropy;
 	struct thread *td;
 	struct thread *ctd;
 	struct proc *p;
 
+	if (lastithd != ithread)
+		ithdcount = 0;
+	else
+		ithdcount++;
+	lastithd = ithread;
+#if 0
+	if (ithdcount > 100)
+		panic("ithd schedule count exceeds 100 %s\n", ithread->it_name);
+#endif
+
 	/*
 	 * If no ithread or no handlers, then we have a stray interrupt.
 	 */
-	if ((ithread == NULL) || TAILQ_EMPTY(&ithread->it_handlers))
+	if ((ithread == NULL) || TAILQ_EMPTY(&ithread->it_handlers)) {
+		if (ithread && ithread->it_disable)
+			ithread->it_disable(ithread->it_vector);
 		return (EINVAL);
+	}
 
 	ctd = curthread;
 	td = ithread->it_td;
@@ -407,8 +425,32 @@
 	if (TD_AWAITING_INTR(td)) {
 		CTR2(KTR_INTR, "%s: setrunqueue %d", __func__, p->p_pid);
 		TD_CLR_IWAIT(td);
+#if 1
+		if (ithread->it_preempted)
+			kdb_backtrace();
+		if (!cold && td->td_priority < ctd->td_priority &&
+		    ctd->td_critnest <= 1 && ithread->it_preempted == NULL) {
+			ithread->it_preempted = ctd;
+			TD_SET_RUNNING(td);
+			TD_SET_CAN_RUN(ctd);
+			cpu_switch(ctd, td);
+			TD_SET_RUNNING(ctd);
+			sched_lock.mtx_lock = (uintptr_t)ctd;
+		} else {
+			if (ithread->it_disable != NULL)
+				ithread->it_disable(ithread->it_vector);
+			setrunqueue(td, SRQ_INTR);
+		}
+#else
+		if (ithread->it_disable != NULL)
+			ithread->it_disable(ithread->it_vector);
 		setrunqueue(td, SRQ_INTR);
+#endif
 	} else {
+		if (ithread->it_preempted && ithread->it_disable)
+			panic("eh?");
+		if (ithread->it_disable)
+			ithread->it_disable(ithread->it_vector);
 		CTR4(KTR_INTR, "%s: pid %d: it_need %d, state %d",
 		    __func__, p->p_pid, ithread->it_need, td->td_state);
 	}
@@ -433,7 +475,7 @@
 		if ((ithd->it_flags & IT_SOFT) == 0)
 			return(EINVAL);
 	} else {
-		error = ithread_create(&ithd, pri, IT_SOFT, NULL, NULL,
+		error = ithread_create(&ithd, pri, IT_SOFT, NULL, NULL, NULL,
 		    "swi%d:", pri);
 		if (error)
 			return (error);
@@ -503,6 +545,7 @@
 		/*
 		 * If we are an orphaned thread, then just die.
 		 */
+#if 0
 		if (ithd->it_flags & IT_DEAD) {
 			CTR3(KTR_INTR, "%s: pid %d: (%s) exiting", __func__,
 			    p->p_pid, p->p_comm);
@@ -511,7 +554,7 @@
 			free(ithd, M_ITHREAD);
 			kthread_exit(0);
 		}
-
+#endif
 		CTR4(KTR_INTR, "%s: pid %d: (%s) need=%d", __func__,
 		     p->p_pid, p->p_comm, ithd->it_need);
 		while (ithd->it_need) {
@@ -574,8 +617,8 @@
 				tsleep(&count, td->td_priority, "istorm", 1);
 			else
 				count++;
-
-			if (ithd->it_enable != NULL)
+			if (ithd->it_preempted == NULL &&
+			    ithd->it_enable != NULL)
 				ithd->it_enable(ithd->it_vector);
 		}
 		WITNESS_WARN(WARN_PANIC, NULL, "suspending ithread");
@@ -592,13 +635,44 @@
 			count = 0;
 			storming = 0;
 			CTR2(KTR_INTR, "%s: pid %d: done", __func__, p->p_pid);
-			mi_switch(SW_VOL, NULL);
+			if (ithd->it_preempted) {
+				struct thread *ctd;
+
+				if (ithd->it_ack)
+					ithd->it_ack(ithd->it_vector);
+				ctd = ithd->it_preempted;
+				ithd->it_preempted = NULL;
+				cpu_switch(td, ctd);
+				sched_lock.mtx_lock = (uintptr_t)td;
+			} else
+				mi_switch(SW_VOL, NULL);
 			CTR2(KTR_INTR, "%s: pid %d: resumed", __func__, p->p_pid);
 		}
 		mtx_unlock_spin(&sched_lock);
 	}
 }
 
+int
+ithread_switch(struct thread *td, struct thread *newtd)
+{
+	struct ithd *ithd;
+
+	ithd = td->td_ithd;
+        if (ithd->it_preempted == NULL)
+		return (0);
+	if (newtd)
+		panic("unexpected newtd");
+	if (ithd->it_disable != NULL)
+		ithd->it_disable(ithd->it_vector);
+	if (TD_IS_RUNNING(td))
+		setrunqueue(td, SRQ_OURSELF|SRQ_YIELDING);
+	newtd = ithd->it_preempted;
+	ithd->it_preempted = NULL;
+	cpu_switch(td, newtd);
+	sched_lock.mtx_lock = (uintptr_t)td;
+	return (1);
+}
+
 #ifdef DDB
 /*
  * Dump details about an interrupt handler
Index: kern/kern_switch.c
===================================================================
RCS file: /home/ncvs/src/sys/kern/kern_switch.c,v
retrieving revision 1.111
diff -u -r1.111 kern_switch.c
--- kern/kern_switch.c	8 Apr 2005 03:37:53 -0000	1.111
+++ kern/kern_switch.c	8 May 2005 22:10:32 -0000
@@ -600,7 +600,6 @@
 		}
 		
 		td->td_critnest = 0;
-
 #ifdef PREEMPTION
 		mtx_assert(&sched_lock, MA_NOTOWNED);
 		if (td->td_owepreempt) {
@@ -612,7 +611,6 @@
 		}
 	             
 #endif
-
 	} else {
 		td->td_critnest--;
 	}
Index: kern/kern_synch.c
===================================================================
RCS file: /home/ncvs/src/sys/kern/kern_synch.c,v
retrieving revision 1.269
diff -u -r1.269 kern_synch.c
--- kern/kern_synch.c	8 Apr 2005 03:37:53 -0000	1.269
+++ kern/kern_synch.c	8 May 2005 21:38:06 -0000
@@ -41,7 +41,9 @@
 
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/bus.h>
 #include <sys/condvar.h>
+#include <sys/interrupt.h>
 #include <sys/kdb.h>
 #include <sys/kernel.h>
 #include <sys/ktr.h>
@@ -293,6 +295,9 @@
 	    ("mi_switch: switch must be voluntary or involuntary"));
 	KASSERT(newtd != curthread, ("mi_switch: preempting back to ourself"));
 
+	if (td->td_ithd)
+		if (ithread_switch(td, newtd))
+			return;
 	if (flags & SW_VOL)
 		p->p_stats->p_ru.ru_nvcsw++;
 	else
Index: kern/sched_4bsd.c
===================================================================
RCS file: /home/ncvs/src/sys/kern/sched_4bsd.c,v
retrieving revision 1.74
diff -u -r1.74 sched_4bsd.c
--- kern/sched_4bsd.c	19 Apr 2005 04:01:24 -0000	1.74
+++ kern/sched_4bsd.c	8 May 2005 21:38:06 -0000
@@ -300,6 +300,7 @@
 	sched_tdcnt--;
 	CTR1(KTR_SCHED, "global load: %d", sched_tdcnt);
 }
+#include <sys/interrupt.h>
 /*
  * Arrange to reschedule if necessary, taking the priorities and
  * schedulers into account.
@@ -307,9 +308,13 @@
 static void
 maybe_resched(struct thread *td)
 {
+	struct thread *ctd;
 
+	ctd = curthread;
+	while (ctd->td_ithd && ctd->td_ithd->it_preempted)
+		ctd = ctd->td_ithd->it_preempted;
 	mtx_assert(&sched_lock, MA_OWNED);
-	if (td->td_priority < curthread->td_priority)
+	if (td->td_priority < ctd->td_priority)
 		curthread->td_flags |= TDF_NEEDRESCHED;
 }
 
Index: kern/subr_turnstile.c
===================================================================
RCS file: /home/ncvs/src/sys/kern/subr_turnstile.c,v
retrieving revision 1.152
diff -u -r1.152 subr_turnstile.c
--- kern/subr_turnstile.c	10 Feb 2005 12:02:37 -0000	1.152
+++ kern/subr_turnstile.c	8 May 2005 21:38:06 -0000
@@ -207,7 +207,7 @@
 		 * If lock holder is actually running or on the run queue
 		 * then we are done.
 		 */
-		if (TD_IS_RUNNING(td) || TD_ON_RUNQ(td)) {
+		if (TD_IS_RUNNING(td) || TD_ON_RUNQ(td) || TD_CAN_RUN(td)) {
 			MPASS(td->td_blocked == NULL);
 			return;
 		}
