17 files changed, 170 insertions, 411 deletions

diff --git a/sys/amd64/amd64/apic_vector.S b/sys/amd64/amd64/apic_vector.S
index 11d9797e7948..1f80224628b4 100644
--- a/sys/amd64/amd64/apic_vector.S
+++ b/sys/amd64/amd64/apic_vector.S
@@ -15,6 +15,23 @@
 /* make an index into the IO APIC from the IRQ# */
 #define REDTBL_IDX(irq_num)	(0x10 + ((irq_num) * 2))
 
+/*
+ * 
+ */
+#define PUSH_FRAME							\
+	pushl	$0 ;		/* dummy error code */			\
+	pushl	$0 ;		/* dummy trap type */			\
+	pushal ;							\
+	pushl	%ds ;		/* save data and extra segments ... */	\
+	pushl	%es ;							\
+	pushl	%fs
+
+#define POP_FRAME							\
+	popl	%fs ;							\
+	popl	%es ;							\
+	popl	%ds ;							\
+	popal ;								\
+	addl	$4+4,%esp
 
 /*
  * Macros for interrupt entry, call to handler, and exit.
@@ -24,18 +41,14 @@
 	.text ;								\
 	SUPERALIGN_TEXT ;						\
 IDTVEC(vec_name) ;							\
-	pushl	%eax ;		/* save only call-used registers */	\
-	pushl	%ecx ;							\
-	pushl	%edx ;							\
-	pushl	%ds ;							\
-	MAYBE_PUSHL_ES ;						\
-	pushl	%fs ;							\
+	PUSH_FRAME ;							\
 	movl	$KDSEL,%eax ;						\
 	mov	%ax,%ds ;						\
-	MAYBE_MOVW_AX_ES ;						\
+	mov	%ax,%es ;						\
 	movl	$KPSEL,%eax ;						\
 	mov	%ax,%fs ;						\
-	FAKE_MCOUNT((5+ACTUALLY_PUSHED)*4(%esp)) ;			\
+	FAKE_MCOUNT(13*4(%esp)) ;					\
+	incb	_intr_nesting_level ;					\
 	pushl	_intr_unit + (irq_num) * 4 ;				\
 	call	*_intr_handler + (irq_num) * 4 ; /* do the work ASAP */ \
 	addl	$4, %esp ;						\
@@ -46,31 +59,7 @@ IDTVEC(vec_name) ;							\
 	lock ; 								\
 	incl	(%eax) ;						\
 	MEXITCOUNT ;							\
-	popl	%fs ;							\
-	MAYBE_POPL_ES ;							\
-	popl	%ds ;							\
-	popl	%edx ;							\
-	popl	%ecx ;							\
-	popl	%eax ;							\
-	iret
-
-/*
- * 
- */
-#define PUSH_FRAME							\
-	pushl	$0 ;		/* dummy error code */			\
-	pushl	$0 ;		/* dummy trap type */			\
-	pushal ;							\
-	pushl	%ds ;		/* save data and extra segments ... */	\
-	pushl	%es ;							\
-	pushl	%fs
-
-#define POP_FRAME							\
-	popl	%fs ;							\
-	popl	%es ;							\
-	popl	%ds ;							\
-	popal ;								\
-	addl	$4+4,%esp
+	jmp	doreti_next
 
 #define IOAPICADDR(irq_num) CNAME(int_to_apicintpin) + 16 * (irq_num) + 8
 #define REDIRIDX(irq_num) CNAME(int_to_apicintpin) + 16 * (irq_num) + 12
diff --git a/sys/amd64/amd64/cpu_switch.S b/sys/amd64/amd64/cpu_switch.S
index ac71f0c43be9..32dbe56e686a 100644
--- a/sys/amd64/amd64/cpu_switch.S
+++ b/sys/amd64/amd64/cpu_switch.S
@@ -82,7 +82,6 @@ ENTRY(cpu_switch)
 	
 	/* switch to new process. first, save context as needed */
 	movl	_curproc,%ecx
-	movl	%ecx,_prevproc
 
 	/* if no process to save, don't bother */
 	testl	%ecx,%ecx
diff --git a/sys/amd64/amd64/genassym.c b/sys/amd64/amd64/genassym.c
index 78c607591875..0566dd7e0f45 100644
--- a/sys/amd64/amd64/genassym.c
+++ b/sys/amd64/amd64/genassym.c
@@ -173,7 +173,6 @@ ASSYM(BI_ESYMTAB, offsetof(struct bootinfo, bi_esymtab));
 ASSYM(BI_KERNEND, offsetof(struct bootinfo, bi_kernend));
 ASSYM(GD_SIZEOF, sizeof(struct globaldata));
 ASSYM(GD_CURPROC, offsetof(struct globaldata, gd_curproc));
-ASSYM(GD_PREVPROC, offsetof(struct globaldata, gd_prevproc));
 ASSYM(GD_NPXPROC, offsetof(struct globaldata, gd_npxproc));
 ASSYM(GD_IDLEPROC, offsetof(struct globaldata, gd_idleproc));
 ASSYM(GD_CURPCB, offsetof(struct globaldata, gd_curpcb));
diff --git a/sys/amd64/amd64/machdep.c b/sys/amd64/amd64/machdep.c
index 355a0a953184..705501062ac8 100644
--- a/sys/amd64/amd64/machdep.c
+++ b/sys/amd64/amd64/machdep.c
@@ -1883,7 +1883,6 @@ init386(first)
 
 	/* setup curproc so that mutexes work */
 	PCPU_SET(curproc, &proc0);
-	PCPU_SET(prevproc, &proc0);
 
 	/* make ldt memory segments */
 	/*
@@ -1945,6 +1944,11 @@ init386(first)
 	lidt(&r_idt);
 
 	/*
+	 * We need this mutex before the console probe.
+	 */
+	mtx_init(&clock_lock, "clk interrupt lock", MTX_SPIN);
+
+	/*
 	 * Initialize the console before we print anything out.
 	 */
 	cninit();
@@ -1955,8 +1959,7 @@ init386(first)
 #endif
 
 	/*
-	 * Giant is used early for at least debugger traps, unexpected traps,
-	 * and vm86bios initialization.
+	 * Giant is used early for at least debugger traps and unexpected traps.
 	 */
 	mtx_init(&Giant, "Giant", MTX_DEF);
 
diff --git a/sys/amd64/amd64/mp_machdep.c b/sys/amd64/amd64/mp_machdep.c
index ea9aee8ddbcd..5ef95b3607a7 100644
--- a/sys/amd64/amd64/mp_machdep.c
+++ b/sys/amd64/amd64/mp_machdep.c
@@ -1900,11 +1900,6 @@ struct simplelock	mcount_lock;
 struct simplelock	com_lock;
 #endif /* USE_COMLOCK */
 
-#ifdef USE_CLOCKLOCK
-/* lock regions around the clock hardware */
-struct simplelock	clock_lock;
-#endif /* USE_CLOCKLOCK */
-
 /* lock around the MP rendezvous */
 static struct simplelock smp_rv_lock;
 
@@ -1930,9 +1925,6 @@ init_locks(void)
 #ifdef USE_COMLOCK
 	s_lock_init((struct simplelock*)&com_lock);
 #endif /* USE_COMLOCK */
-#ifdef USE_CLOCKLOCK
-	s_lock_init((struct simplelock*)&clock_lock);
-#endif /* USE_CLOCKLOCK */
 
 	s_lock_init(&ap_boot_lock);
 }
@@ -2425,7 +2417,6 @@ ap_init(void)
 	 * something unique to lock with.
 	 */
 	PCPU_SET(curproc,idleproc);
-	PCPU_SET(prevproc,idleproc);
 
 	microuptime(&switchtime);
 	switchticks = ticks;
diff --git a/sys/amd64/amd64/mptable.c b/sys/amd64/amd64/mptable.c
index ea9aee8ddbcd..5ef95b3607a7 100644
--- a/sys/amd64/amd64/mptable.c
+++ b/sys/amd64/amd64/mptable.c
@@ -1900,11 +1900,6 @@ struct simplelock	mcount_lock;
 struct simplelock	com_lock;
 #endif /* USE_COMLOCK */
 
-#ifdef USE_CLOCKLOCK
-/* lock regions around the clock hardware */
-struct simplelock	clock_lock;
-#endif /* USE_CLOCKLOCK */
-
 /* lock around the MP rendezvous */
 static struct simplelock smp_rv_lock;
 
@@ -1930,9 +1925,6 @@ init_locks(void)
 #ifdef USE_COMLOCK
 	s_lock_init((struct simplelock*)&com_lock);
 #endif /* USE_COMLOCK */
-#ifdef USE_CLOCKLOCK
-	s_lock_init((struct simplelock*)&clock_lock);
-#endif /* USE_CLOCKLOCK */
 
 	s_lock_init(&ap_boot_lock);
 }
@@ -2425,7 +2417,6 @@ ap_init(void)
 	 * something unique to lock with.
 	 */
 	PCPU_SET(curproc,idleproc);
-	PCPU_SET(prevproc,idleproc);
 
 	microuptime(&switchtime);
 	switchticks = ticks;
diff --git a/sys/amd64/amd64/swtch.s b/sys/amd64/amd64/swtch.s
index ac71f0c43be9..32dbe56e686a 100644
--- a/sys/amd64/amd64/swtch.s
+++ b/sys/amd64/amd64/swtch.s
@@ -82,7 +82,6 @@ ENTRY(cpu_switch)
 	
 	/* switch to new process. first, save context as needed */
 	movl	_curproc,%ecx
-	movl	%ecx,_prevproc
 
 	/* if no process to save, don't bother */
 	testl	%ecx,%ecx
diff --git a/sys/amd64/amd64/trap.c b/sys/amd64/amd64/trap.c
index 1cbfcfad4ed7..b9ebb600cb05 100644
--- a/sys/amd64/amd64/trap.c
+++ b/sys/amd64/amd64/trap.c
@@ -1244,6 +1244,18 @@ ast(frame)
 		addupc_task(p, p->p_stats->p_prof.pr_addr,
 			    p->p_stats->p_prof.pr_ticks);
 	}
+	if (p->p_flag & P_ALRMPEND) {
+		if (!mtx_owned(&Giant))
+			mtx_enter(&Giant, MTX_DEF);
+		p->p_flag &= ~P_ALRMPEND;
+		psignal(p, SIGVTALRM);
+	}
+	if (p->p_flag & P_PROFPEND) {
+		if (!mtx_owned(&Giant))
+			mtx_enter(&Giant, MTX_DEF);
+		p->p_flag &= ~P_PROFPEND;
+		psignal(p, SIGPROF);
+	}
 	if (userret(p, &frame, sticks, mtx_owned(&Giant)) != 0)
 		mtx_exit(&Giant, MTX_DEF);
 }
diff --git a/sys/amd64/amd64/tsc.c b/sys/amd64/amd64/tsc.c
index 0e630d9b2d7b..a164288dd17e 100644
--- a/sys/amd64/amd64/tsc.c
+++ b/sys/amd64/amd64/tsc.c
@@ -72,6 +72,7 @@
 #include <machine/ipl.h>
 #include <machine/limits.h>
 #include <machine/md_var.h>
+#include <machine/mutex.h>
 #include <machine/psl.h>
 #ifdef APIC_IO
 #include <machine/segments.h>
@@ -139,6 +140,7 @@ int	timer0_max_count;
 u_int	tsc_freq;
 int	tsc_is_broken;
 int	wall_cmos_clock;	/* wall CMOS clock assumed if != 0 */
+struct	mtx clock_lock;
 
 static	int	beeping = 0;
 static	const u_char daysinmonth[] = {31,28,31,30,31,30,31,31,30,31,30,31};
@@ -198,12 +200,9 @@ SYSCTL_OPAQUE(_debug, OID_AUTO, i8254_timecounter, CTLFLAG_RD,
 static void
 clkintr(struct clockframe frame)
 {
-	int intrsave;
 
 	if (timecounter->tc_get_timecount == i8254_get_timecount) {
-		intrsave = save_intr();
-		disable_intr();
-		CLOCK_LOCK();
+		mtx_enter(&clock_lock, MTX_SPIN);
 		if (i8254_ticked)
 			i8254_ticked = 0;
 		else {
@@ -211,8 +210,7 @@ clkintr(struct clockframe frame)
 			i8254_lastcount = 0;
 		}
 		clkintr_pending = 0;
-		CLOCK_UNLOCK();
-		restore_intr(intrsave);
+		mtx_exit(&clock_lock, MTX_SPIN);
 	}
 	timer_func(&frame);
 	switch (timer0_state) {
@@ -231,17 +229,14 @@ clkintr(struct clockframe frame)
 		break;
 
 	case ACQUIRE_PENDING:
-		intrsave = save_intr();
-		disable_intr();
-		CLOCK_LOCK();
+		mtx_enter(&clock_lock, MTX_SPIN);
 		i8254_offset = i8254_get_timecount(NULL);
 		i8254_lastcount = 0;
 		timer0_max_count = TIMER_DIV(new_rate);
 		outb(TIMER_MODE, TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 		outb(TIMER_CNTR0, timer0_max_count & 0xff);
 		outb(TIMER_CNTR0, timer0_max_count >> 8);
-		CLOCK_UNLOCK();
-		restore_intr(intrsave);
+		mtx_exit(&clock_lock, MTX_SPIN);
 		timer_func = new_function;
 		timer0_state = ACQUIRED;
 		setdelayed();
@@ -250,9 +245,7 @@ clkintr(struct clockframe frame)
 	case RELEASE_PENDING:
 		if ((timer0_prescaler_count += timer0_max_count)
 		    >= hardclock_max_count) {
-			intrsave = save_intr();
-			disable_intr();
-			CLOCK_LOCK();
+			mtx_enter(&clock_lock, MTX_SPIN);
 			i8254_offset = i8254_get_timecount(NULL);
 			i8254_lastcount = 0;
 			timer0_max_count = hardclock_max_count;
@@ -260,8 +253,7 @@ clkintr(struct clockframe frame)
 			     TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 			outb(TIMER_CNTR0, timer0_max_count & 0xff);
 			outb(TIMER_CNTR0, timer0_max_count >> 8);
-			CLOCK_UNLOCK();
-			restore_intr(intrsave);
+			mtx_exit(&clock_lock, MTX_SPIN);
 			timer0_prescaler_count = 0;
 			timer_func = hardclock;
 			timer0_state = RELEASED;
@@ -408,11 +400,9 @@ DB_SHOW_COMMAND(rtc, rtc)
 static int
 getit(void)
 {
-	int high, low, intrsave;
+	int high, low;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 
 	/* Select timer0 and latch counter value. */
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_LATCH);
@@ -420,8 +410,7 @@ getit(void)
 	low = inb(TIMER_CNTR0);
 	high = inb(TIMER_CNTR0);
 
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	return ((high << 8) | low);
 }
 
@@ -527,7 +516,6 @@ sysbeepstop(void *chan)
 int
 sysbeep(int pitch, int period)
 {
-	int intrsave;
 	int x = splclock();
 
 	if (acquire_timer2(TIMER_SQWAVE|TIMER_16BIT))
@@ -536,13 +524,10 @@ sysbeep(int pitch, int period)
 			splx(x);
 			return (-1); /* XXX Should be EBUSY, but nobody cares anyway. */
 		}
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	outb(TIMER_CNTR2, pitch);
 	outb(TIMER_CNTR2, (pitch>>8));
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	if (!beeping) {
 		/* enable counter2 output to speaker */
 		outb(IO_PPI, inb(IO_PPI) | 3);
@@ -691,12 +676,9 @@ fail:
 static void
 set_timer_freq(u_int freq, int intr_freq)
 {
-	int intrsave;
 	int new_timer0_max_count;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	timer_freq = freq;
 	new_timer0_max_count = hardclock_max_count = TIMER_DIV(intr_freq);
 	if (new_timer0_max_count != timer0_max_count) {
@@ -705,8 +687,7 @@ set_timer_freq(u_int freq, int intr_freq)
 		outb(TIMER_CNTR0, timer0_max_count & 0xff);
 		outb(TIMER_CNTR0, timer0_max_count >> 8);
 	}
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 }
 
 /*
@@ -720,16 +701,12 @@ set_timer_freq(u_int freq, int intr_freq)
 void
 i8254_restore(void)
 {
-	int intrsave;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 	outb(TIMER_CNTR0, timer0_max_count & 0xff);
 	outb(TIMER_CNTR0, timer0_max_count >> 8);
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 }
 
 /*
@@ -989,8 +966,8 @@ cpu_initclocks()
 {
 	int diag;
 #ifdef APIC_IO
-	int apic_8254_trial, num_8254_ticks;
-	struct intrec *clkdesc, *rtcdesc;
+	int apic_8254_trial;
+	struct intrec *clkdesc;
 #endif /* APIC_IO */
 
 	if (statclock_disable) {
@@ -1023,6 +1000,11 @@ cpu_initclocks()
 		} else 
 			panic("APIC_IO: Cannot route 8254 interrupt to CPU");
 	}
+
+	clkdesc = inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr,
+			      NULL, PI_REALTIME, INTR_FAST);
+	INTREN(1 << apic_8254_intr);
+
 #else /* APIC_IO */
 
 	/*
@@ -1030,9 +1012,8 @@ cpu_initclocks()
 	 * couldn't find anything suitable in the BSD/OS code (grog,
 	 * 19 July 2000).
 	 */
-	/* Setup the PIC clk handler.  The APIC handler is setup later */
 	inthand_add("clk", 0, (driver_intr_t *)clkintr, NULL, PI_REALTIME,
-		    INTR_EXCL);
+		    INTR_FAST);
 	INTREN(IRQ0);
 
 #endif /* APIC_IO */
@@ -1042,18 +1023,8 @@ cpu_initclocks()
 	writertc(RTC_STATUSB, RTCSB_24HR);
 
 	/* Don't bother enabling the statistics clock. */
-	if (statclock_disable) {
-#ifdef APIC_IO
-		/*
-		 * XXX - if statclock is disabled, don't attempt the APIC
-		 * trial.  Not sure this is sane for APIC_IO.
-		 */
-		inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr,
-			    NULL, PI_REALTIME, INTR_EXCL);
-		INTREN(1 << apic_8254_intr);
-#endif /* APIC_IO */
+	if (statclock_disable)
 		return;
-	}
 	diag = rtcin(RTC_DIAG);
 	if (diag != 0)
 		printf("RTC BIOS diagnostic error %b\n", diag, RTCDG_BITS);
@@ -1061,44 +1032,34 @@ cpu_initclocks()
 #ifdef APIC_IO
 	if (isa_apic_irq(8) != 8)
 		panic("APIC RTC != 8");
+#endif /* APIC_IO */
 
-	if (apic_8254_trial) {
-		/*
-		 * XXX - We use fast interrupts for clk and rtc long enough to
-		 * perform the APIC probe and then revert to exclusive
-		 * interrupts.
-		 */
-		clkdesc = inthand_add("clk", apic_8254_intr,
-		    (driver_intr_t *)clkintr, NULL, PI_REALTIME, INTR_FAST);
-		INTREN(1 << apic_8254_intr);
+	inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL, PI_REALTIME,
+		    INTR_FAST);
 
-		rtcdesc = inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL,
-		    PI_REALTIME, INTR_FAST);	    /* XXX */
-		INTREN(APIC_IRQ8);
-		writertc(RTC_STATUSB, rtc_statusb);
+#ifdef APIC_IO
+	INTREN(APIC_IRQ8);
+#else
+	INTREN(IRQ8);
+#endif /* APIC_IO */
+
+	writertc(RTC_STATUSB, rtc_statusb);
+
+#ifdef APIC_IO
+	if (apic_8254_trial) {
 
 		printf("APIC_IO: Testing 8254 interrupt delivery\n");
 		while (read_intr_count(8) < 6)
 			;	/* nothing */
-		num_8254_ticks = read_intr_count(apic_8254_intr);
-
-		/* disable and remove our fake handlers */
-		INTRDIS(1 << apic_8254_intr);
-		inthand_remove(clkdesc);
-
-		writertc(RTC_STATUSA, rtc_statusa);
-		writertc(RTC_STATUSB, RTCSB_24HR);
-
-		INTRDIS(APIC_IRQ8);
-		inthand_remove(rtcdesc);
-
-		if (num_8254_ticks < 3) {
+		if (read_intr_count(apic_8254_intr) < 3) {
 			/* 
 			 * The MP table is broken.
 			 * The 8254 was not connected to the specified pin
 			 * on the IO APIC.
 			 * Workaround: Limited variant of mixed mode.
 			 */
+			INTRDIS(1 << apic_8254_intr);
+			inthand_remove(clkdesc);
 			printf("APIC_IO: Broken MP table detected: "
 			       "8254 is not connected to "
 			       "IOAPIC #%d intpin %d\n",
@@ -1117,27 +1078,13 @@ cpu_initclocks()
 			}
 			apic_8254_intr = apic_irq(0, 0);
 			setup_8254_mixed_mode();
+			inthand_add("clk", apic_8254_intr,
+				    (driver_intr_t *)clkintr, NULL,
+				    PI_REALTIME, INTR_FAST);
+			INTREN(1 << apic_8254_intr);
 		}
 		
 	}
-
-	/* Finally, setup the real clock handlers */
-	inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr, NULL,
-		    PI_REALTIME, INTR_EXCL);
-	INTREN(1 << apic_8254_intr);
-#endif
-
-	inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL, PI_REALTIME,
-		    INTR_EXCL);
-#ifdef APIC_IO
-	INTREN(APIC_IRQ8);
-#else
-	INTREN(IRQ8);
-#endif
-
-	writertc(RTC_STATUSB, rtc_statusb);
-
-#ifdef APIC_IO
 	if (apic_int_type(0, 0) != 3 ||
 	    int_to_apicintpin[apic_8254_intr].ioapic != 0 ||
 	    int_to_apicintpin[apic_8254_intr].int_pin != 0)
@@ -1242,12 +1189,11 @@ static unsigned
 i8254_get_timecount(struct timecounter *tc)
 {
 	u_int count;
-	int intrsave;
 	u_int high, low;
+	u_int eflags;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	eflags = read_eflags();
+	mtx_enter(&clock_lock, MTX_SPIN);
 
 	/* Select timer0 and latch counter value. */
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_LATCH);
@@ -1257,7 +1203,7 @@ i8254_get_timecount(struct timecounter *tc)
 	count = timer0_max_count - ((high << 8) | low);
 	if (count < i8254_lastcount ||
 	    (!i8254_ticked && (clkintr_pending ||
-	    ((count < 20 || (!(intrsave & PSL_I) && count < timer0_max_count / 2u)) &&
+	    ((count < 20 || (!(eflags & PSL_I) && count < timer0_max_count / 2u)) &&
 #ifdef APIC_IO
 #define	lapic_irr1	((volatile u_int *)&lapic)[0x210 / 4]	/* XXX XXX */
 	    /* XXX this assumes that apic_8254_intr is < 24. */
@@ -1271,8 +1217,7 @@ i8254_get_timecount(struct timecounter *tc)
 	}
 	i8254_lastcount = count;
 	count += i8254_offset;
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	return (count);
 }
 
diff --git a/sys/amd64/include/cpu.h b/sys/amd64/include/cpu.h
index 18822b87cc5b..d35b94077130 100644
--- a/sys/amd64/include/cpu.h
+++ b/sys/amd64/include/cpu.h
@@ -62,21 +62,6 @@
 	((ISPL((framep)->cf_cs) == SEL_UPL) || (framep->cf_eflags & PSL_VM))
 
 #define CLKF_INTR(framep)	(intr_nesting_level >= 2)
-#if 0
-/*
- * XXX splsoftclock() is very broken and barely worth fixing.  It doesn't
- * turn off the clock bit in imen or in the icu.  (This is not a serious
- * problem at 100 Hz but it is serious at 16000 Hz for pcaudio.  softclock()
- * can take more than 62.5 usec so clock interrupts are lost.)  It doesn't
- * check for pending interrupts being unmasked.  clkintr() and Xintr0()
- * assume that the ipl is high when hardclock() returns.  Our SWI_CLOCK
- * handling is efficient enough that little is gained by calling
- * softclock() directly.
- */
-#define	CLKF_BASEPRI(framep)	((framep)->cf_ppl == 0)
-#else
-#define	CLKF_BASEPRI(framep)	(0)
-#endif
 #define	CLKF_PC(framep)		((framep)->cf_eip)
 
 /*
diff --git a/sys/amd64/include/mptable.h b/sys/amd64/include/mptable.h
index ea9aee8ddbcd..5ef95b3607a7 100644
--- a/sys/amd64/include/mptable.h
+++ b/sys/amd64/include/mptable.h
@@ -1900,11 +1900,6 @@ struct simplelock	mcount_lock;
 struct simplelock	com_lock;
 #endif /* USE_COMLOCK */
 
-#ifdef USE_CLOCKLOCK
-/* lock regions around the clock hardware */
-struct simplelock	clock_lock;
-#endif /* USE_CLOCKLOCK */
-
 /* lock around the MP rendezvous */
 static struct simplelock smp_rv_lock;
 
@@ -1930,9 +1925,6 @@ init_locks(void)
 #ifdef USE_COMLOCK
 	s_lock_init((struct simplelock*)&com_lock);
 #endif /* USE_COMLOCK */
-#ifdef USE_CLOCKLOCK
-	s_lock_init((struct simplelock*)&clock_lock);
-#endif /* USE_CLOCKLOCK */
 
 	s_lock_init(&ap_boot_lock);
 }
@@ -2425,7 +2417,6 @@ ap_init(void)
 	 * something unique to lock with.
 	 */
 	PCPU_SET(curproc,idleproc);
-	PCPU_SET(prevproc,idleproc);
 
 	microuptime(&switchtime);
 	switchticks = ticks;
diff --git a/sys/amd64/include/mutex.h b/sys/amd64/include/mutex.h
index ca518d98fd44..881cbfae3c72 100644
--- a/sys/amd64/include/mutex.h
+++ b/sys/amd64/include/mutex.h
@@ -146,6 +146,7 @@ void	_mtx_exit(struct mtx *mtxp, int type, const char *file, int line);
 /* Global locks */
 extern struct mtx	sched_lock;
 extern struct mtx	Giant;
+extern struct mtx	clock_lock;
 
 /*
  * Used to replace return with an exit Giant and return.
diff --git a/sys/amd64/include/pcpu.h b/sys/amd64/include/pcpu.h
index ace3602a2844..27884997a535 100644
--- a/sys/amd64/include/pcpu.h
+++ b/sys/amd64/include/pcpu.h
@@ -55,7 +55,6 @@
 struct globaldata {
 	struct privatespace *gd_prvspace;	/* self-reference */
 	struct proc	*gd_curproc;
-	struct proc	*gd_prevproc;
 	struct proc	*gd_npxproc;
 	struct pcb	*gd_curpcb;
 	struct proc	*gd_idleproc;
diff --git a/sys/amd64/isa/atpic_vector.S b/sys/amd64/isa/atpic_vector.S
index 0a89492d8ba0..7644c03aeb3a 100644
--- a/sys/amd64/isa/atpic_vector.S
+++ b/sys/amd64/isa/atpic_vector.S
@@ -49,17 +49,18 @@
 	.text ; \
 	SUPERALIGN_TEXT ; \
 IDTVEC(vec_name) ; \
-	pushl	%eax ;		/* save only call-used registers */ \
-	pushl	%ecx ; \
-	pushl	%edx ; \
+	pushl	$0 ;		/* dummy error code */ \
+	pushl	$0 ;		/* dummy trap type */ \
+	pushal ; \
 	pushl	%ds ; \
+	pushl	%es ; \
 	pushl	%fs ; \
-	MAYBE_PUSHL_ES ; \
 	mov	$KDSEL,%ax ; \
 	mov	%ax,%ds ; \
+	mov	%ax,%es ; \
 	mov	%ax,%fs ; \
-	MAYBE_MOVW_AX_ES ; \
-	FAKE_MCOUNT((4+ACTUALLY_PUSHED)*4(%esp)) ; \
+	FAKE_MCOUNT((12+ACTUALLY_PUSHED)*4(%esp)) ; \
+	incb	_intr_nesting_level ; \
 	pushl	_intr_unit + (irq_num) * 4 ; \
 	call	*_intr_handler + (irq_num) * 4 ; /* do the work ASAP */ \
 	enable_icus ;		/* (re)enable ASAP (helps edge trigger?) */ \
@@ -67,19 +68,8 @@ IDTVEC(vec_name) ; \
 	incl	_cnt+V_INTR ;	/* book-keeping can wait */ \
 	movl	_intr_countp + (irq_num) * 4,%eax ; \
 	incl	(%eax) ; \
-/*	movl	_cpl,%eax ;	// are we unmasking pending SWIs? / \
-	notl	%eax ; \
-	andl	_spending,$SWI_MASK ; \
-	jne	2f ; 		// yes, maybe handle them */ \
-1: ; \
 	MEXITCOUNT ; \
-	MAYBE_POPL_ES ; \
-	popl	%fs ; \
-	popl	%ds ; \
-	popl	%edx ; \
-	popl	%ecx ; \
-	popl	%eax ; \
-	iret ; \
+	jmp	doreti_next
 
 #if 0
 ; \
@@ -141,7 +131,7 @@ IDTVEC(vec_name) ; \
 	movb	%al,_imen + IRQ_BYTE(irq_num) ; \
 	outb	%al,$icu+ICU_IMR_OFFSET ; \
 	enable_icus ; \
-	incb	_intr_nesting_level ; /* XXX do we need this? */ \
+	incb	_intr_nesting_level ; \
 __CONCAT(Xresume,irq_num): ; \
 	FAKE_MCOUNT(13*4(%esp)) ;	/* XXX late to avoid double count */ \
 	pushl	$irq_num; 	/* pass the IRQ */ \
@@ -153,26 +143,6 @@ __CONCAT(Xresume,irq_num): ; \
 	/* _doreti, but it's probably better to use less cache. */ \
 	jmp	doreti_next	/* and catch up inside doreti */
 
-/*
- * Reenable the interrupt mask after completing an interrupt.  Called
- * from ithd_loop.  There are two separate functions, one for each
- * ICU.
- */
-       .globl   setimask0, setimask1
-setimask0:
-	cli
-	movb	_imen,%al
-	outb	%al,$IO_ICU1 + ICU_IMR_OFFSET
-	sti
-	ret
-
-setimask1:
-	cli
-	movb	_imen + 1,%al
-	outb	%al,$IO_ICU2 + ICU_IMR_OFFSET
-	sti
-	ret
-
 MCOUNT_LABEL(bintr)
 	FAST_INTR(0,fastintr0, ENABLE_ICU1)
 	FAST_INTR(1,fastintr1, ENABLE_ICU1)
diff --git a/sys/amd64/isa/clock.c b/sys/amd64/isa/clock.c
index 0e630d9b2d7b..a164288dd17e 100644
--- a/sys/amd64/isa/clock.c
+++ b/sys/amd64/isa/clock.c
@@ -72,6 +72,7 @@
 #include <machine/ipl.h>
 #include <machine/limits.h>
 #include <machine/md_var.h>
+#include <machine/mutex.h>
 #include <machine/psl.h>
 #ifdef APIC_IO
 #include <machine/segments.h>
@@ -139,6 +140,7 @@ int	timer0_max_count;
 u_int	tsc_freq;
 int	tsc_is_broken;
 int	wall_cmos_clock;	/* wall CMOS clock assumed if != 0 */
+struct	mtx clock_lock;
 
 static	int	beeping = 0;
 static	const u_char daysinmonth[] = {31,28,31,30,31,30,31,31,30,31,30,31};
@@ -198,12 +200,9 @@ SYSCTL_OPAQUE(_debug, OID_AUTO, i8254_timecounter, CTLFLAG_RD,
 static void
 clkintr(struct clockframe frame)
 {
-	int intrsave;
 
 	if (timecounter->tc_get_timecount == i8254_get_timecount) {
-		intrsave = save_intr();
-		disable_intr();
-		CLOCK_LOCK();
+		mtx_enter(&clock_lock, MTX_SPIN);
 		if (i8254_ticked)
 			i8254_ticked = 0;
 		else {
@@ -211,8 +210,7 @@ clkintr(struct clockframe frame)
 			i8254_lastcount = 0;
 		}
 		clkintr_pending = 0;
-		CLOCK_UNLOCK();
-		restore_intr(intrsave);
+		mtx_exit(&clock_lock, MTX_SPIN);
 	}
 	timer_func(&frame);
 	switch (timer0_state) {
@@ -231,17 +229,14 @@ clkintr(struct clockframe frame)
 		break;
 
 	case ACQUIRE_PENDING:
-		intrsave = save_intr();
-		disable_intr();
-		CLOCK_LOCK();
+		mtx_enter(&clock_lock, MTX_SPIN);
 		i8254_offset = i8254_get_timecount(NULL);
 		i8254_lastcount = 0;
 		timer0_max_count = TIMER_DIV(new_rate);
 		outb(TIMER_MODE, TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 		outb(TIMER_CNTR0, timer0_max_count & 0xff);
 		outb(TIMER_CNTR0, timer0_max_count >> 8);
-		CLOCK_UNLOCK();
-		restore_intr(intrsave);
+		mtx_exit(&clock_lock, MTX_SPIN);
 		timer_func = new_function;
 		timer0_state = ACQUIRED;
 		setdelayed();
@@ -250,9 +245,7 @@ clkintr(struct clockframe frame)
 	case RELEASE_PENDING:
 		if ((timer0_prescaler_count += timer0_max_count)
 		    >= hardclock_max_count) {
-			intrsave = save_intr();
-			disable_intr();
-			CLOCK_LOCK();
+			mtx_enter(&clock_lock, MTX_SPIN);
 			i8254_offset = i8254_get_timecount(NULL);
 			i8254_lastcount = 0;
 			timer0_max_count = hardclock_max_count;
@@ -260,8 +253,7 @@ clkintr(struct clockframe frame)
 			     TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 			outb(TIMER_CNTR0, timer0_max_count & 0xff);
 			outb(TIMER_CNTR0, timer0_max_count >> 8);
-			CLOCK_UNLOCK();
-			restore_intr(intrsave);
+			mtx_exit(&clock_lock, MTX_SPIN);
 			timer0_prescaler_count = 0;
 			timer_func = hardclock;
 			timer0_state = RELEASED;
@@ -408,11 +400,9 @@ DB_SHOW_COMMAND(rtc, rtc)
 static int
 getit(void)
 {
-	int high, low, intrsave;
+	int high, low;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 
 	/* Select timer0 and latch counter value. */
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_LATCH);
@@ -420,8 +410,7 @@ getit(void)
 	low = inb(TIMER_CNTR0);
 	high = inb(TIMER_CNTR0);
 
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	return ((high << 8) | low);
 }
 
@@ -527,7 +516,6 @@ sysbeepstop(void *chan)
 int
 sysbeep(int pitch, int period)
 {
-	int intrsave;
 	int x = splclock();
 
 	if (acquire_timer2(TIMER_SQWAVE|TIMER_16BIT))
@@ -536,13 +524,10 @@ sysbeep(int pitch, int period)
 			splx(x);
 			return (-1); /* XXX Should be EBUSY, but nobody cares anyway. */
 		}
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	outb(TIMER_CNTR2, pitch);
 	outb(TIMER_CNTR2, (pitch>>8));
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	if (!beeping) {
 		/* enable counter2 output to speaker */
 		outb(IO_PPI, inb(IO_PPI) | 3);
@@ -691,12 +676,9 @@ fail:
 static void
 set_timer_freq(u_int freq, int intr_freq)
 {
-	int intrsave;
 	int new_timer0_max_count;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	timer_freq = freq;
 	new_timer0_max_count = hardclock_max_count = TIMER_DIV(intr_freq);
 	if (new_timer0_max_count != timer0_max_count) {
@@ -705,8 +687,7 @@ set_timer_freq(u_int freq, int intr_freq)
 		outb(TIMER_CNTR0, timer0_max_count & 0xff);
 		outb(TIMER_CNTR0, timer0_max_count >> 8);
 	}
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 }
 
 /*
@@ -720,16 +701,12 @@ set_timer_freq(u_int freq, int intr_freq)
 void
 i8254_restore(void)
 {
-	int intrsave;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	mtx_enter(&clock_lock, MTX_SPIN);
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_RATEGEN | TIMER_16BIT);
 	outb(TIMER_CNTR0, timer0_max_count & 0xff);
 	outb(TIMER_CNTR0, timer0_max_count >> 8);
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 }
 
 /*
@@ -989,8 +966,8 @@ cpu_initclocks()
 {
 	int diag;
 #ifdef APIC_IO
-	int apic_8254_trial, num_8254_ticks;
-	struct intrec *clkdesc, *rtcdesc;
+	int apic_8254_trial;
+	struct intrec *clkdesc;
 #endif /* APIC_IO */
 
 	if (statclock_disable) {
@@ -1023,6 +1000,11 @@ cpu_initclocks()
 		} else 
 			panic("APIC_IO: Cannot route 8254 interrupt to CPU");
 	}
+
+	clkdesc = inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr,
+			      NULL, PI_REALTIME, INTR_FAST);
+	INTREN(1 << apic_8254_intr);
+
 #else /* APIC_IO */
 
 	/*
@@ -1030,9 +1012,8 @@ cpu_initclocks()
 	 * couldn't find anything suitable in the BSD/OS code (grog,
 	 * 19 July 2000).
 	 */
-	/* Setup the PIC clk handler.  The APIC handler is setup later */
 	inthand_add("clk", 0, (driver_intr_t *)clkintr, NULL, PI_REALTIME,
-		    INTR_EXCL);
+		    INTR_FAST);
 	INTREN(IRQ0);
 
 #endif /* APIC_IO */
@@ -1042,18 +1023,8 @@ cpu_initclocks()
 	writertc(RTC_STATUSB, RTCSB_24HR);
 
 	/* Don't bother enabling the statistics clock. */
-	if (statclock_disable) {
-#ifdef APIC_IO
-		/*
-		 * XXX - if statclock is disabled, don't attempt the APIC
-		 * trial.  Not sure this is sane for APIC_IO.
-		 */
-		inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr,
-			    NULL, PI_REALTIME, INTR_EXCL);
-		INTREN(1 << apic_8254_intr);
-#endif /* APIC_IO */
+	if (statclock_disable)
 		return;
-	}
 	diag = rtcin(RTC_DIAG);
 	if (diag != 0)
 		printf("RTC BIOS diagnostic error %b\n", diag, RTCDG_BITS);
@@ -1061,44 +1032,34 @@ cpu_initclocks()
 #ifdef APIC_IO
 	if (isa_apic_irq(8) != 8)
 		panic("APIC RTC != 8");
+#endif /* APIC_IO */
 
-	if (apic_8254_trial) {
-		/*
-		 * XXX - We use fast interrupts for clk and rtc long enough to
-		 * perform the APIC probe and then revert to exclusive
-		 * interrupts.
-		 */
-		clkdesc = inthand_add("clk", apic_8254_intr,
-		    (driver_intr_t *)clkintr, NULL, PI_REALTIME, INTR_FAST);
-		INTREN(1 << apic_8254_intr);
+	inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL, PI_REALTIME,
+		    INTR_FAST);
 
-		rtcdesc = inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL,
-		    PI_REALTIME, INTR_FAST);	    /* XXX */
-		INTREN(APIC_IRQ8);
-		writertc(RTC_STATUSB, rtc_statusb);
+#ifdef APIC_IO
+	INTREN(APIC_IRQ8);
+#else
+	INTREN(IRQ8);
+#endif /* APIC_IO */
+
+	writertc(RTC_STATUSB, rtc_statusb);
+
+#ifdef APIC_IO
+	if (apic_8254_trial) {
 
 		printf("APIC_IO: Testing 8254 interrupt delivery\n");
 		while (read_intr_count(8) < 6)
 			;	/* nothing */
-		num_8254_ticks = read_intr_count(apic_8254_intr);
-
-		/* disable and remove our fake handlers */
-		INTRDIS(1 << apic_8254_intr);
-		inthand_remove(clkdesc);
-
-		writertc(RTC_STATUSA, rtc_statusa);
-		writertc(RTC_STATUSB, RTCSB_24HR);
-
-		INTRDIS(APIC_IRQ8);
-		inthand_remove(rtcdesc);
-
-		if (num_8254_ticks < 3) {
+		if (read_intr_count(apic_8254_intr) < 3) {
 			/* 
 			 * The MP table is broken.
 			 * The 8254 was not connected to the specified pin
 			 * on the IO APIC.
 			 * Workaround: Limited variant of mixed mode.
 			 */
+			INTRDIS(1 << apic_8254_intr);
+			inthand_remove(clkdesc);
 			printf("APIC_IO: Broken MP table detected: "
 			       "8254 is not connected to "
 			       "IOAPIC #%d intpin %d\n",
@@ -1117,27 +1078,13 @@ cpu_initclocks()
 			}
 			apic_8254_intr = apic_irq(0, 0);
 			setup_8254_mixed_mode();
+			inthand_add("clk", apic_8254_intr,
+				    (driver_intr_t *)clkintr, NULL,
+				    PI_REALTIME, INTR_FAST);
+			INTREN(1 << apic_8254_intr);
 		}
 		
 	}
-
-	/* Finally, setup the real clock handlers */
-	inthand_add("clk", apic_8254_intr, (driver_intr_t *)clkintr, NULL,
-		    PI_REALTIME, INTR_EXCL);
-	INTREN(1 << apic_8254_intr);
-#endif
-
-	inthand_add("rtc", 8, (driver_intr_t *)rtcintr, NULL, PI_REALTIME,
-		    INTR_EXCL);
-#ifdef APIC_IO
-	INTREN(APIC_IRQ8);
-#else
-	INTREN(IRQ8);
-#endif
-
-	writertc(RTC_STATUSB, rtc_statusb);
-
-#ifdef APIC_IO
 	if (apic_int_type(0, 0) != 3 ||
 	    int_to_apicintpin[apic_8254_intr].ioapic != 0 ||
 	    int_to_apicintpin[apic_8254_intr].int_pin != 0)
@@ -1242,12 +1189,11 @@ static unsigned
 i8254_get_timecount(struct timecounter *tc)
 {
 	u_int count;
-	int intrsave;
 	u_int high, low;
+	u_int eflags;
 
-	intrsave = save_intr();
-	disable_intr();
-	CLOCK_LOCK();
+	eflags = read_eflags();
+	mtx_enter(&clock_lock, MTX_SPIN);
 
 	/* Select timer0 and latch counter value. */
 	outb(TIMER_MODE, TIMER_SEL0 | TIMER_LATCH);
@@ -1257,7 +1203,7 @@ i8254_get_timecount(struct timecounter *tc)
 	count = timer0_max_count - ((high << 8) | low);
 	if (count < i8254_lastcount ||
 	    (!i8254_ticked && (clkintr_pending ||
-	    ((count < 20 || (!(intrsave & PSL_I) && count < timer0_max_count / 2u)) &&
+	    ((count < 20 || (!(eflags & PSL_I) && count < timer0_max_count / 2u)) &&
 #ifdef APIC_IO
 #define	lapic_irr1	((volatile u_int *)&lapic)[0x210 / 4]	/* XXX XXX */
 	    /* XXX this assumes that apic_8254_intr is < 24. */
@@ -1271,8 +1217,7 @@ i8254_get_timecount(struct timecounter *tc)
 	}
 	i8254_lastcount = count;
 	count += i8254_offset;
-	CLOCK_UNLOCK();
-	restore_intr(intrsave);
+	mtx_exit(&clock_lock, MTX_SPIN);
 	return (count);
 }
 
diff --git a/sys/amd64/isa/icu_vector.S b/sys/amd64/isa/icu_vector.S
index 0a89492d8ba0..7644c03aeb3a 100644
--- a/sys/amd64/isa/icu_vector.S
+++ b/sys/amd64/isa/icu_vector.S
@@ -49,17 +49,18 @@
 	.text ; \
 	SUPERALIGN_TEXT ; \
 IDTVEC(vec_name) ; \
-	pushl	%eax ;		/* save only call-used registers */ \
-	pushl	%ecx ; \
-	pushl	%edx ; \
+	pushl	$0 ;		/* dummy error code */ \
+	pushl	$0 ;		/* dummy trap type */ \
+	pushal ; \
 	pushl	%ds ; \
+	pushl	%es ; \
 	pushl	%fs ; \
-	MAYBE_PUSHL_ES ; \
 	mov	$KDSEL,%ax ; \
 	mov	%ax,%ds ; \
+	mov	%ax,%es ; \
 	mov	%ax,%fs ; \
-	MAYBE_MOVW_AX_ES ; \
-	FAKE_MCOUNT((4+ACTUALLY_PUSHED)*4(%esp)) ; \
+	FAKE_MCOUNT((12+ACTUALLY_PUSHED)*4(%esp)) ; \
+	incb	_intr_nesting_level ; \
 	pushl	_intr_unit + (irq_num) * 4 ; \
 	call	*_intr_handler + (irq_num) * 4 ; /* do the work ASAP */ \
 	enable_icus ;		/* (re)enable ASAP (helps edge trigger?) */ \
@@ -67,19 +68,8 @@ IDTVEC(vec_name) ; \
 	incl	_cnt+V_INTR ;	/* book-keeping can wait */ \
 	movl	_intr_countp + (irq_num) * 4,%eax ; \
 	incl	(%eax) ; \
-/*	movl	_cpl,%eax ;	// are we unmasking pending SWIs? / \
-	notl	%eax ; \
-	andl	_spending,$SWI_MASK ; \
-	jne	2f ; 		// yes, maybe handle them */ \
-1: ; \
 	MEXITCOUNT ; \
-	MAYBE_POPL_ES ; \
-	popl	%fs ; \
-	popl	%ds ; \
-	popl	%edx ; \
-	popl	%ecx ; \
-	popl	%eax ; \
-	iret ; \
+	jmp	doreti_next
 
 #if 0
 ; \
@@ -141,7 +131,7 @@ IDTVEC(vec_name) ; \
 	movb	%al,_imen + IRQ_BYTE(irq_num) ; \
 	outb	%al,$icu+ICU_IMR_OFFSET ; \
 	enable_icus ; \
-	incb	_intr_nesting_level ; /* XXX do we need this? */ \
+	incb	_intr_nesting_level ; \
 __CONCAT(Xresume,irq_num): ; \
 	FAKE_MCOUNT(13*4(%esp)) ;	/* XXX late to avoid double count */ \
 	pushl	$irq_num; 	/* pass the IRQ */ \
@@ -153,26 +143,6 @@ __CONCAT(Xresume,irq_num): ; \
 	/* _doreti, but it's probably better to use less cache. */ \
 	jmp	doreti_next	/* and catch up inside doreti */
 
-/*
- * Reenable the interrupt mask after completing an interrupt.  Called
- * from ithd_loop.  There are two separate functions, one for each
- * ICU.
- */
-       .globl   setimask0, setimask1
-setimask0:
-	cli
-	movb	_imen,%al
-	outb	%al,$IO_ICU1 + ICU_IMR_OFFSET
-	sti
-	ret
-
-setimask1:
-	cli
-	movb	_imen + 1,%al
-	outb	%al,$IO_ICU2 + ICU_IMR_OFFSET
-	sti
-	ret
-
 MCOUNT_LABEL(bintr)
 	FAST_INTR(0,fastintr0, ENABLE_ICU1)
 	FAST_INTR(1,fastintr1, ENABLE_ICU1)
diff --git a/sys/amd64/isa/icu_vector.s b/sys/amd64/isa/icu_vector.s
index 0a89492d8ba0..7644c03aeb3a 100644
--- a/sys/amd64/isa/icu_vector.s
+++ b/sys/amd64/isa/icu_vector.s
@@ -49,17 +49,18 @@
 	.text ; \
 	SUPERALIGN_TEXT ; \
 IDTVEC(vec_name) ; \
-	pushl	%eax ;		/* save only call-used registers */ \
-	pushl	%ecx ; \
-	pushl	%edx ; \
+	pushl	$0 ;		/* dummy error code */ \
+	pushl	$0 ;		/* dummy trap type */ \
+	pushal ; \
 	pushl	%ds ; \
+	pushl	%es ; \
 	pushl	%fs ; \
-	MAYBE_PUSHL_ES ; \
 	mov	$KDSEL,%ax ; \
 	mov	%ax,%ds ; \
+	mov	%ax,%es ; \
 	mov	%ax,%fs ; \
-	MAYBE_MOVW_AX_ES ; \
-	FAKE_MCOUNT((4+ACTUALLY_PUSHED)*4(%esp)) ; \
+	FAKE_MCOUNT((12+ACTUALLY_PUSHED)*4(%esp)) ; \
+	incb	_intr_nesting_level ; \
 	pushl	_intr_unit + (irq_num) * 4 ; \
 	call	*_intr_handler + (irq_num) * 4 ; /* do the work ASAP */ \
 	enable_icus ;		/* (re)enable ASAP (helps edge trigger?) */ \
@@ -67,19 +68,8 @@ IDTVEC(vec_name) ; \
 	incl	_cnt+V_INTR ;	/* book-keeping can wait */ \
 	movl	_intr_countp + (irq_num) * 4,%eax ; \
 	incl	(%eax) ; \
-/*	movl	_cpl,%eax ;	// are we unmasking pending SWIs? / \
-	notl	%eax ; \
-	andl	_spending,$SWI_MASK ; \
-	jne	2f ; 		// yes, maybe handle them */ \
-1: ; \
 	MEXITCOUNT ; \
-	MAYBE_POPL_ES ; \
-	popl	%fs ; \
-	popl	%ds ; \
-	popl	%edx ; \
-	popl	%ecx ; \
-	popl	%eax ; \
-	iret ; \
+	jmp	doreti_next
 
 #if 0
 ; \
@@ -141,7 +131,7 @@ IDTVEC(vec_name) ; \
 	movb	%al,_imen + IRQ_BYTE(irq_num) ; \
 	outb	%al,$icu+ICU_IMR_OFFSET ; \
 	enable_icus ; \
-	incb	_intr_nesting_level ; /* XXX do we need this? */ \
+	incb	_intr_nesting_level ; \
 __CONCAT(Xresume,irq_num): ; \
 	FAKE_MCOUNT(13*4(%esp)) ;	/* XXX late to avoid double count */ \
 	pushl	$irq_num; 	/* pass the IRQ */ \
@@ -153,26 +143,6 @@ __CONCAT(Xresume,irq_num): ; \
 	/* _doreti, but it's probably better to use less cache. */ \
 	jmp	doreti_next	/* and catch up inside doreti */
 
-/*
- * Reenable the interrupt mask after completing an interrupt.  Called
- * from ithd_loop.  There are two separate functions, one for each
- * ICU.
- */
-       .globl   setimask0, setimask1
-setimask0:
-	cli
-	movb	_imen,%al
-	outb	%al,$IO_ICU1 + ICU_IMR_OFFSET
-	sti
-	ret
-
-setimask1:
-	cli
-	movb	_imen + 1,%al
-	outb	%al,$IO_ICU2 + ICU_IMR_OFFSET
-	sti
-	ret
-
 MCOUNT_LABEL(bintr)
 	FAST_INTR(0,fastintr0, ENABLE_ICU1)
 	FAST_INTR(1,fastintr1, ENABLE_ICU1)