19 files changed, 708 insertions, 103 deletions

diff --git a/sys/amd64/amd64/apic_vector.S b/sys/amd64/amd64/apic_vector.S
index b3dd56a..edd8225 100644
--- a/sys/amd64/amd64/apic_vector.S
+++ b/sys/amd64/amd64/apic_vector.S
@@ -1,6 +1,6 @@
 /*
  *	from: vector.s, 386BSD 0.1 unknown origin
- *	$Id: apic_vector.s,v 1.28 1998/03/05 21:45:53 tegge Exp $
+ *	$Id: apic_vector.s,v 1.29 1998/04/22 22:49:27 tegge Exp $
  */
 
 
@@ -675,7 +675,15 @@ _Xcpuast:
 	movl	_cpuid, %eax
 	lock	
 	btrl	%eax, _checkstate_pending_ast
-
+	lock	
+	btrl	%eax, CNAME(resched_cpus)
+	jz	2f
+	movl	$1, CNAME(want_resched)
+	lock
+	incl	CNAME(want_resched_cnt)
+2:		
+	lock
+	incl	CNAME(cpuast_cnt)
 	MEXITCOUNT
 	jmp	_doreti
 1:
@@ -803,12 +811,27 @@ forward_irq:
 	SUPERALIGN_TEXT
 	.globl _Xcpustop
 _Xcpustop:
+	pushl	%ebp
+	movl	%esp, %ebp
 	pushl	%eax
+	pushl	%ecx
+	pushl	%edx
 	pushl	%ds			/* save current data segment */
+	pushl	%es		
 
 	movl	$KDSEL, %eax
 	movl	%ax, %ds		/* use KERNEL data segment */
 
+	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+
+	movl	_cpuid, %eax
+	imull	$PCB_SIZE, %eax
+	leal	CNAME(stoppcbs)(%eax), %eax
+	pushl	%eax
+	call	CNAME(savectx)		/* Save process context */
+	addl	$4, %esp
+	
+		
 	movl	_cpuid, %eax
 
 	lock
@@ -819,11 +842,26 @@ _Xcpustop:
 
 	lock
 	btrl	%eax, _started_cpus	/* started_cpus &= ~(1<<id) */
+	lock
+	btrl	%eax, _stopped_cpus	/* stopped_cpus &= ~(1<<id) */
 
-	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+	test	%eax, %eax
+	jnz	2f
 
+	movl	CNAME(cpustop_restartfunc), %eax
+	test	%eax, %eax
+	jz	2f
+	movl	$0, CNAME(cpustop_restartfunc)	/* One-shot */
+
+	call	%eax
+2:
+	popl	%es
 	popl	%ds			/* restore previous data segment */
+	popl	%edx
+	popl	%ecx
 	popl	%eax
+	movl	%ebp, %esp
+	popl	%ebp
 	iret
 
 
@@ -944,12 +982,25 @@ _checkstate_pending_ast:
 	.globl CNAME(forward_irq_misscnt)
 	.globl CNAME(forward_irq_toodeepcnt)
 	.globl CNAME(forward_irq_hitcnt)
+	.globl CNAME(resched_cpus)
+	.globl CNAME(want_resched_cnt)
+	.globl CNAME(cpuast_cnt)
+	.globl CNAME(cpustop_restartfunc)
 CNAME(forward_irq_misscnt):	
 	.long 0
 CNAME(forward_irq_hitcnt):	
 	.long 0
 CNAME(forward_irq_toodeepcnt):
 	.long 0
+CNAME(resched_cpus):
+	.long 0
+CNAME(want_resched_cnt):
+	.long 0
+CNAME(cpuast_cnt):
+	.long 0
+CNAME(cpustop_restartfunc):
+	.long 0
+		
 
 
 	.globl	_apic_pin_trigger
diff --git a/sys/amd64/amd64/genassym.c b/sys/amd64/amd64/genassym.c
index 89117f4..4e640ab 100644
--- a/sys/amd64/amd64/genassym.c
+++ b/sys/amd64/amd64/genassym.c
@@ -34,7 +34,7 @@
  * SUCH DAMAGE.
  *
  *	from: @(#)genassym.c	5.11 (Berkeley) 5/10/91
- *	$Id: genassym.c,v 1.54 1998/04/06 18:59:14 peter Exp $
+ *	$Id: genassym.c,v 1.55 1998/05/17 18:53:11 tegge Exp $
  */
 
 #include "opt_vm86.h"
@@ -159,6 +159,9 @@ main()
 	printf("#define\tPCB_SAVEFPU %p\n", &pcb->pcb_savefpu);
 	printf("#define\tPCB_SAVEFPU_SIZE %d\n", sizeof pcb->pcb_savefpu);
 	printf("#define\tPCB_ONFAULT %p\n", &pcb->pcb_onfault);
+#ifdef SMP
+	printf("#define\tPCB_SIZE %d\n", sizeof(struct pcb));
+#endif
 
 	printf("#define\tTF_ES %p\n", &tf->tf_es);
 	printf("#define\tTF_DS %p\n", &tf->tf_ds);
diff --git a/sys/amd64/amd64/mp_machdep.c b/sys/amd64/amd64/mp_machdep.c
index 7b58b87..c5e392a 100644
--- a/sys/amd64/amd64/mp_machdep.c
+++ b/sys/amd64/amd64/mp_machdep.c
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/amd64/amd64/mptable.c b/sys/amd64/amd64/mptable.c
index 7b58b87..c5e392a 100644
--- a/sys/amd64/amd64/mptable.c
+++ b/sys/amd64/amd64/mptable.c
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/amd64/amd64/vm_machdep.c b/sys/amd64/amd64/vm_machdep.c
index 78f16ae..7e3b32a 100644
--- a/sys/amd64/amd64/vm_machdep.c
+++ b/sys/amd64/amd64/vm_machdep.c
@@ -38,7 +38,7 @@
  *
  *	from: @(#)vm_machdep.c	7.3 (Berkeley) 5/13/91
  *	Utah $Hdr: vm_machdep.c 1.16.1.1 89/06/23$
- *	$Id: vm_machdep.c,v 1.105 1998/03/23 19:52:42 jlemon Exp $
+ *	$Id: vm_machdep.c,v 1.106 1998/05/16 14:44:11 kato Exp $
  */
 
 #include "npx.h"
@@ -87,6 +87,13 @@
 #include <i386/isa/isa.h>
 #endif
 
+static void	cpu_reset_real __P((void));
+#ifdef SMP
+static void	cpu_reset_proxy __P((void));
+static u_int	cpu_reset_proxyid;
+static volatile u_int	cpu_reset_proxy_active;
+#endif
+
 #ifdef BOUNCE_BUFFERS
 static vm_offset_t
 		vm_bounce_kva __P((int size, int waitok));
@@ -838,9 +845,89 @@ vunmapbuf(bp)
 /*
  * Force reset the processor by invalidating the entire address space!
  */
+
+#ifdef SMP
+static void
+cpu_reset_proxy()
+{
+	u_int saved_mp_lock;
+
+	cpu_reset_proxy_active = 1;
+	while (cpu_reset_proxy_active == 1)
+		;	 /* Wait for other cpu to disable interupts */
+	saved_mp_lock = mp_lock;
+	mp_lock = 1;
+	printf("cpu_reset_proxy: Grabbed mp lock for BSP\n");
+	cpu_reset_proxy_active = 3;
+	while (cpu_reset_proxy_active == 3)
+		;	/* Wait for other cpu to enable interrupts */
+	stop_cpus((1<<cpu_reset_proxyid));
+	printf("cpu_reset_proxy: Stopped CPU %d\n", cpu_reset_proxyid);
+	DELAY(1000000);
+	cpu_reset_real();
+}
+#endif
+
 void
 cpu_reset()
 {
+#ifdef SMP
+	if (smp_active == 0) {
+		cpu_reset_real();
+		/* NOTREACHED */
+	} else {
+
+		u_int map;
+		int cnt;
+		printf("cpu_reset called on cpu#%d\n",cpuid);
+
+		map = other_cpus & ~ stopped_cpus;
+
+		if (map != 0) {
+			printf("cpu_reset: Stopping other CPUs\n");
+			stop_cpus(map);		/* Stop all other CPUs */
+		}
+
+		if (cpuid == 0) {
+			DELAY(1000000);
+			cpu_reset_real();
+			/* NOTREACHED */
+		} else {
+			/* We are not BSP (CPU #0) */
+
+			cpu_reset_proxyid = cpuid;
+			cpustop_restartfunc = cpu_reset_proxy;
+			printf("cpu_reset: Restarting BSP\n");
+			started_cpus = (1<<0);		/* Restart CPU #0 */
+
+			cnt = 0;
+			while (cpu_reset_proxy_active == 0 && cnt < 10000000)
+				cnt++;	/* Wait for BSP to announce restart */
+			if (cpu_reset_proxy_active == 0)
+				printf("cpu_reset: Failed to restart BSP\n");
+			__asm __volatile("cli" : : : "memory");
+			cpu_reset_proxy_active = 2;
+			cnt = 0;
+			while (cpu_reset_proxy_active == 2 && cnt < 10000000)
+				cnt++;	/* Do nothing */
+			if (cpu_reset_proxy_active == 2) {
+				printf("cpu_reset: BSP did not grab mp lock\n");
+				cpu_reset_real();	/* XXX: Bogus ? */
+			}
+			cpu_reset_proxy_active = 4;
+			__asm __volatile("sti" : : : "memory");
+			while (1);
+			/* NOTREACHED */
+		}
+	}
+#else
+	cpu_reset_real();
+#endif
+}
+
+static void
+cpu_reset_real()
+{
 
 #ifdef PC98
 	/*
diff --git a/sys/amd64/include/mptable.h b/sys/amd64/include/mptable.h
index 7b58b87..c5e392a 100644
--- a/sys/amd64/include/mptable.h
+++ b/sys/amd64/include/mptable.h
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/amd64/include/smp.h b/sys/amd64/include/smp.h
index 283c925..f258630 100644
--- a/sys/amd64/include/smp.h
+++ b/sys/amd64/include/smp.h
@@ -6,7 +6,7 @@
  * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
  * ----------------------------------------------------------------------------
  *
- * $Id: smp.h,v 1.41 1998/04/01 20:38:28 tegge Exp $
+ * $Id: smp.h,v 1.42 1998/04/01 21:07:36 tegge Exp $
  *
  */
 
@@ -80,6 +80,8 @@ extern volatile u_int		started_cpus;
 
 extern volatile u_int		checkstate_probed_cpus;
 extern volatile u_int		checkstate_need_ast;
+extern volatile u_int		resched_cpus;
+extern void (*cpustop_restartfunc) __P((void));
 
 /* functions in apic_ipl.s */
 void	apic_eoi		__P((void));
@@ -103,6 +105,7 @@ extern int			io_num_to_apic_id[];
 extern int			apic_id_to_logical[];
 extern u_int			all_cpus;
 extern u_char			SMP_ioapic[];
+extern struct pcb		stoppcbs[];
 
 /* functions in mp_machdep.c */
 u_int	mp_bootaddress		__P((u_int));
@@ -131,6 +134,7 @@ void	forward_statclock	__P((int pscnt));
 void	forward_hardclock	__P((int pscnt));
 #endif /* BETTER_CLOCK */
 void	forward_signal		__P((struct proc *));
+void	forward_roundrobin	__P((void));
 #ifdef	APIC_INTR_REORDER
 void	set_lapic_isrloc	__P((int, int));
 #endif /* APIC_INTR_REORDER */
diff --git a/sys/i386/i386/apic_vector.s b/sys/i386/i386/apic_vector.s
index b3dd56a..edd8225 100644
--- a/sys/i386/i386/apic_vector.s
+++ b/sys/i386/i386/apic_vector.s
@@ -1,6 +1,6 @@
 /*
  *	from: vector.s, 386BSD 0.1 unknown origin
- *	$Id: apic_vector.s,v 1.28 1998/03/05 21:45:53 tegge Exp $
+ *	$Id: apic_vector.s,v 1.29 1998/04/22 22:49:27 tegge Exp $
  */
 
 
@@ -675,7 +675,15 @@ _Xcpuast:
 	movl	_cpuid, %eax
 	lock	
 	btrl	%eax, _checkstate_pending_ast
-
+	lock	
+	btrl	%eax, CNAME(resched_cpus)
+	jz	2f
+	movl	$1, CNAME(want_resched)
+	lock
+	incl	CNAME(want_resched_cnt)
+2:		
+	lock
+	incl	CNAME(cpuast_cnt)
 	MEXITCOUNT
 	jmp	_doreti
 1:
@@ -803,12 +811,27 @@ forward_irq:
 	SUPERALIGN_TEXT
 	.globl _Xcpustop
 _Xcpustop:
+	pushl	%ebp
+	movl	%esp, %ebp
 	pushl	%eax
+	pushl	%ecx
+	pushl	%edx
 	pushl	%ds			/* save current data segment */
+	pushl	%es		
 
 	movl	$KDSEL, %eax
 	movl	%ax, %ds		/* use KERNEL data segment */
 
+	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+
+	movl	_cpuid, %eax
+	imull	$PCB_SIZE, %eax
+	leal	CNAME(stoppcbs)(%eax), %eax
+	pushl	%eax
+	call	CNAME(savectx)		/* Save process context */
+	addl	$4, %esp
+	
+		
 	movl	_cpuid, %eax
 
 	lock
@@ -819,11 +842,26 @@ _Xcpustop:
 
 	lock
 	btrl	%eax, _started_cpus	/* started_cpus &= ~(1<<id) */
+	lock
+	btrl	%eax, _stopped_cpus	/* stopped_cpus &= ~(1<<id) */
 
-	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+	test	%eax, %eax
+	jnz	2f
 
+	movl	CNAME(cpustop_restartfunc), %eax
+	test	%eax, %eax
+	jz	2f
+	movl	$0, CNAME(cpustop_restartfunc)	/* One-shot */
+
+	call	%eax
+2:
+	popl	%es
 	popl	%ds			/* restore previous data segment */
+	popl	%edx
+	popl	%ecx
 	popl	%eax
+	movl	%ebp, %esp
+	popl	%ebp
 	iret
 
 
@@ -944,12 +982,25 @@ _checkstate_pending_ast:
 	.globl CNAME(forward_irq_misscnt)
 	.globl CNAME(forward_irq_toodeepcnt)
 	.globl CNAME(forward_irq_hitcnt)
+	.globl CNAME(resched_cpus)
+	.globl CNAME(want_resched_cnt)
+	.globl CNAME(cpuast_cnt)
+	.globl CNAME(cpustop_restartfunc)
 CNAME(forward_irq_misscnt):	
 	.long 0
 CNAME(forward_irq_hitcnt):	
 	.long 0
 CNAME(forward_irq_toodeepcnt):
 	.long 0
+CNAME(resched_cpus):
+	.long 0
+CNAME(want_resched_cnt):
+	.long 0
+CNAME(cpuast_cnt):
+	.long 0
+CNAME(cpustop_restartfunc):
+	.long 0
+		
 
 
 	.globl	_apic_pin_trigger
diff --git a/sys/i386/i386/genassym.c b/sys/i386/i386/genassym.c
index 89117f4..4e640ab 100644
--- a/sys/i386/i386/genassym.c
+++ b/sys/i386/i386/genassym.c
@@ -34,7 +34,7 @@
  * SUCH DAMAGE.
  *
  *	from: @(#)genassym.c	5.11 (Berkeley) 5/10/91
- *	$Id: genassym.c,v 1.54 1998/04/06 18:59:14 peter Exp $
+ *	$Id: genassym.c,v 1.55 1998/05/17 18:53:11 tegge Exp $
  */
 
 #include "opt_vm86.h"
@@ -159,6 +159,9 @@ main()
 	printf("#define\tPCB_SAVEFPU %p\n", &pcb->pcb_savefpu);
 	printf("#define\tPCB_SAVEFPU_SIZE %d\n", sizeof pcb->pcb_savefpu);
 	printf("#define\tPCB_ONFAULT %p\n", &pcb->pcb_onfault);
+#ifdef SMP
+	printf("#define\tPCB_SIZE %d\n", sizeof(struct pcb));
+#endif
 
 	printf("#define\tTF_ES %p\n", &tf->tf_es);
 	printf("#define\tTF_DS %p\n", &tf->tf_ds);
diff --git a/sys/i386/i386/mp_machdep.c b/sys/i386/i386/mp_machdep.c
index 7b58b87..c5e392a 100644
--- a/sys/i386/i386/mp_machdep.c
+++ b/sys/i386/i386/mp_machdep.c
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/i386/i386/mptable.c b/sys/i386/i386/mptable.c
index 7b58b87..c5e392a 100644
--- a/sys/i386/i386/mptable.c
+++ b/sys/i386/i386/mptable.c
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/i386/i386/vm_machdep.c b/sys/i386/i386/vm_machdep.c
index 78f16ae..7e3b32a 100644
--- a/sys/i386/i386/vm_machdep.c
+++ b/sys/i386/i386/vm_machdep.c
@@ -38,7 +38,7 @@
  *
  *	from: @(#)vm_machdep.c	7.3 (Berkeley) 5/13/91
  *	Utah $Hdr: vm_machdep.c 1.16.1.1 89/06/23$
- *	$Id: vm_machdep.c,v 1.105 1998/03/23 19:52:42 jlemon Exp $
+ *	$Id: vm_machdep.c,v 1.106 1998/05/16 14:44:11 kato Exp $
  */
 
 #include "npx.h"
@@ -87,6 +87,13 @@
 #include <i386/isa/isa.h>
 #endif
 
+static void	cpu_reset_real __P((void));
+#ifdef SMP
+static void	cpu_reset_proxy __P((void));
+static u_int	cpu_reset_proxyid;
+static volatile u_int	cpu_reset_proxy_active;
+#endif
+
 #ifdef BOUNCE_BUFFERS
 static vm_offset_t
 		vm_bounce_kva __P((int size, int waitok));
@@ -838,9 +845,89 @@ vunmapbuf(bp)
 /*
  * Force reset the processor by invalidating the entire address space!
  */
+
+#ifdef SMP
+static void
+cpu_reset_proxy()
+{
+	u_int saved_mp_lock;
+
+	cpu_reset_proxy_active = 1;
+	while (cpu_reset_proxy_active == 1)
+		;	 /* Wait for other cpu to disable interupts */
+	saved_mp_lock = mp_lock;
+	mp_lock = 1;
+	printf("cpu_reset_proxy: Grabbed mp lock for BSP\n");
+	cpu_reset_proxy_active = 3;
+	while (cpu_reset_proxy_active == 3)
+		;	/* Wait for other cpu to enable interrupts */
+	stop_cpus((1<<cpu_reset_proxyid));
+	printf("cpu_reset_proxy: Stopped CPU %d\n", cpu_reset_proxyid);
+	DELAY(1000000);
+	cpu_reset_real();
+}
+#endif
+
 void
 cpu_reset()
 {
+#ifdef SMP
+	if (smp_active == 0) {
+		cpu_reset_real();
+		/* NOTREACHED */
+	} else {
+
+		u_int map;
+		int cnt;
+		printf("cpu_reset called on cpu#%d\n",cpuid);
+
+		map = other_cpus & ~ stopped_cpus;
+
+		if (map != 0) {
+			printf("cpu_reset: Stopping other CPUs\n");
+			stop_cpus(map);		/* Stop all other CPUs */
+		}
+
+		if (cpuid == 0) {
+			DELAY(1000000);
+			cpu_reset_real();
+			/* NOTREACHED */
+		} else {
+			/* We are not BSP (CPU #0) */
+
+			cpu_reset_proxyid = cpuid;
+			cpustop_restartfunc = cpu_reset_proxy;
+			printf("cpu_reset: Restarting BSP\n");
+			started_cpus = (1<<0);		/* Restart CPU #0 */
+
+			cnt = 0;
+			while (cpu_reset_proxy_active == 0 && cnt < 10000000)
+				cnt++;	/* Wait for BSP to announce restart */
+			if (cpu_reset_proxy_active == 0)
+				printf("cpu_reset: Failed to restart BSP\n");
+			__asm __volatile("cli" : : : "memory");
+			cpu_reset_proxy_active = 2;
+			cnt = 0;
+			while (cpu_reset_proxy_active == 2 && cnt < 10000000)
+				cnt++;	/* Do nothing */
+			if (cpu_reset_proxy_active == 2) {
+				printf("cpu_reset: BSP did not grab mp lock\n");
+				cpu_reset_real();	/* XXX: Bogus ? */
+			}
+			cpu_reset_proxy_active = 4;
+			__asm __volatile("sti" : : : "memory");
+			while (1);
+			/* NOTREACHED */
+		}
+	}
+#else
+	cpu_reset_real();
+#endif
+}
+
+static void
+cpu_reset_real()
+{
 
 #ifdef PC98
 	/*
diff --git a/sys/i386/include/mptable.h b/sys/i386/include/mptable.h
index 7b58b87..c5e392a 100644
--- a/sys/i386/include/mptable.h
+++ b/sys/i386/include/mptable.h
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/i386/include/smp.h b/sys/i386/include/smp.h
index 283c925..f258630 100644
--- a/sys/i386/include/smp.h
+++ b/sys/i386/include/smp.h
@@ -6,7 +6,7 @@
  * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
  * ----------------------------------------------------------------------------
  *
- * $Id: smp.h,v 1.41 1998/04/01 20:38:28 tegge Exp $
+ * $Id: smp.h,v 1.42 1998/04/01 21:07:36 tegge Exp $
  *
  */
 
@@ -80,6 +80,8 @@ extern volatile u_int		started_cpus;
 
 extern volatile u_int		checkstate_probed_cpus;
 extern volatile u_int		checkstate_need_ast;
+extern volatile u_int		resched_cpus;
+extern void (*cpustop_restartfunc) __P((void));
 
 /* functions in apic_ipl.s */
 void	apic_eoi		__P((void));
@@ -103,6 +105,7 @@ extern int			io_num_to_apic_id[];
 extern int			apic_id_to_logical[];
 extern u_int			all_cpus;
 extern u_char			SMP_ioapic[];
+extern struct pcb		stoppcbs[];
 
 /* functions in mp_machdep.c */
 u_int	mp_bootaddress		__P((u_int));
@@ -131,6 +134,7 @@ void	forward_statclock	__P((int pscnt));
 void	forward_hardclock	__P((int pscnt));
 #endif /* BETTER_CLOCK */
 void	forward_signal		__P((struct proc *));
+void	forward_roundrobin	__P((void));
 #ifdef	APIC_INTR_REORDER
 void	set_lapic_isrloc	__P((int, int));
 #endif /* APIC_INTR_REORDER */
diff --git a/sys/i386/isa/apic_vector.s b/sys/i386/isa/apic_vector.s
index b3dd56a..edd8225 100644
--- a/sys/i386/isa/apic_vector.s
+++ b/sys/i386/isa/apic_vector.s
@@ -1,6 +1,6 @@
 /*
  *	from: vector.s, 386BSD 0.1 unknown origin
- *	$Id: apic_vector.s,v 1.28 1998/03/05 21:45:53 tegge Exp $
+ *	$Id: apic_vector.s,v 1.29 1998/04/22 22:49:27 tegge Exp $
  */
 
 
@@ -675,7 +675,15 @@ _Xcpuast:
 	movl	_cpuid, %eax
 	lock	
 	btrl	%eax, _checkstate_pending_ast
-
+	lock	
+	btrl	%eax, CNAME(resched_cpus)
+	jz	2f
+	movl	$1, CNAME(want_resched)
+	lock
+	incl	CNAME(want_resched_cnt)
+2:		
+	lock
+	incl	CNAME(cpuast_cnt)
 	MEXITCOUNT
 	jmp	_doreti
 1:
@@ -803,12 +811,27 @@ forward_irq:
 	SUPERALIGN_TEXT
 	.globl _Xcpustop
 _Xcpustop:
+	pushl	%ebp
+	movl	%esp, %ebp
 	pushl	%eax
+	pushl	%ecx
+	pushl	%edx
 	pushl	%ds			/* save current data segment */
+	pushl	%es		
 
 	movl	$KDSEL, %eax
 	movl	%ax, %ds		/* use KERNEL data segment */
 
+	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+
+	movl	_cpuid, %eax
+	imull	$PCB_SIZE, %eax
+	leal	CNAME(stoppcbs)(%eax), %eax
+	pushl	%eax
+	call	CNAME(savectx)		/* Save process context */
+	addl	$4, %esp
+	
+		
 	movl	_cpuid, %eax
 
 	lock
@@ -819,11 +842,26 @@ _Xcpustop:
 
 	lock
 	btrl	%eax, _started_cpus	/* started_cpus &= ~(1<<id) */
+	lock
+	btrl	%eax, _stopped_cpus	/* stopped_cpus &= ~(1<<id) */
 
-	movl	$0, lapic_eoi		/* End Of Interrupt to APIC */
+	test	%eax, %eax
+	jnz	2f
 
+	movl	CNAME(cpustop_restartfunc), %eax
+	test	%eax, %eax
+	jz	2f
+	movl	$0, CNAME(cpustop_restartfunc)	/* One-shot */
+
+	call	%eax
+2:
+	popl	%es
 	popl	%ds			/* restore previous data segment */
+	popl	%edx
+	popl	%ecx
 	popl	%eax
+	movl	%ebp, %esp
+	popl	%ebp
 	iret
 
 
@@ -944,12 +982,25 @@ _checkstate_pending_ast:
 	.globl CNAME(forward_irq_misscnt)
 	.globl CNAME(forward_irq_toodeepcnt)
 	.globl CNAME(forward_irq_hitcnt)
+	.globl CNAME(resched_cpus)
+	.globl CNAME(want_resched_cnt)
+	.globl CNAME(cpuast_cnt)
+	.globl CNAME(cpustop_restartfunc)
 CNAME(forward_irq_misscnt):	
 	.long 0
 CNAME(forward_irq_hitcnt):	
 	.long 0
 CNAME(forward_irq_toodeepcnt):
 	.long 0
+CNAME(resched_cpus):
+	.long 0
+CNAME(want_resched_cnt):
+	.long 0
+CNAME(cpuast_cnt):
+	.long 0
+CNAME(cpustop_restartfunc):
+	.long 0
+		
 
 
 	.globl	_apic_pin_trigger
diff --git a/sys/kern/kern_shutdown.c b/sys/kern/kern_shutdown.c
index 448bcc0..e48c1ad 100644
--- a/sys/kern/kern_shutdown.c
+++ b/sys/kern/kern_shutdown.c
@@ -36,7 +36,7 @@
  * SUCH DAMAGE.
  *
  *	@(#)kern_shutdown.c	8.3 (Berkeley) 1/21/94
- * $Id: kern_shutdown.c,v 1.30 1998/05/06 22:14:48 julian Exp $
+ * $Id: kern_shutdown.c,v 1.31 1998/05/12 17:34:02 bde Exp $
  */
 
 #include "opt_ddb.h"
@@ -176,23 +176,8 @@ boot(howto)
 	sle_p ep;
 
 #ifdef SMP
-	int c, spins;
-
-	/* The MPSPEC says that the BSP must do the shutdown */
 	if (smp_active) {
-		smp_active = 0;
-
-		spins = 100;
-
 		printf("boot() called on cpu#%d\n", cpuid);
-		while ((c = cpuid) != 0) {
-			if (spins-- < 1) {
-				printf("timeout waiting for cpu #0!\n");
-				break;
-			}
-			printf("I'm on cpu#%d, I need to be on cpu#0, sleeping..\n", c);
-			tsleep((caddr_t)&smp_active, PZERO, "cpu0wt", 10);
-		}
 	}
 #endif
 	/*
diff --git a/sys/kern/kern_synch.c b/sys/kern/kern_synch.c
index cd3ef44..342cb3a 100644
--- a/sys/kern/kern_synch.c
+++ b/sys/kern/kern_synch.c
@@ -36,7 +36,7 @@
  * SUCH DAMAGE.
  *
  *	@(#)kern_synch.c	8.9 (Berkeley) 5/19/95
- * $Id: kern_synch.c,v 1.54 1998/04/04 13:25:20 phk Exp $
+ * $Id: kern_synch.c,v 1.55 1998/05/17 11:52:45 phk Exp $
  */
 
 #include "opt_ktrace.h"
@@ -57,6 +57,10 @@
 #endif
 
 #include <machine/cpu.h>
+#ifdef SMP
+#include <machine/smp.h>
+#endif
+#include <machine/ipl.h>
 #include <machine/limits.h>	/* for UCHAR_MAX = typeof(p_priority)_MAX */
 
 static void rqinit __P((void *));
@@ -130,8 +134,13 @@ roundrobin(arg)
 {
  	struct proc *p = curproc; /* XXX */
  
+#ifdef SMP
+	need_resched();
+	forward_roundrobin();
+#else 
  	if (p == 0 || RTP_PRIO_NEED_RR(p->p_rtprio.type))
  		need_resched();
+#endif
 
  	timeout(roundrobin, NULL, ROUNDROBIN_INTERVAL);
 }
diff --git a/sys/kern/subr_smp.c b/sys/kern/subr_smp.c
index 7b58b87..c5e392a 100644
--- a/sys/kern/subr_smp.c
+++ b/sys/kern/subr_smp.c
@@ -22,7 +22,7 @@
  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  * SUCH DAMAGE.
  *
- *	$Id: mp_machdep.c,v 1.74 1998/05/11 01:06:06 dyson Exp $
+ *	$Id: mp_machdep.c,v 1.75 1998/05/17 18:53:17 tegge Exp $
  */
 
 #include "opt_smp.h"
@@ -312,6 +312,8 @@ extern pt_entry_t SMP_prvpt[];
 /* Private page pointer to curcpu's PTD, used during BSP init */
 extern pd_entry_t *my_idlePTD;
 
+struct pcb stoppcbs[NCPU];
+
 static int smp_started;		/* has the system started? */
 
 /*
@@ -2009,13 +2011,10 @@ stop_cpus(u_int map)
 	if (!smp_started)
 		return 0;
 
-	/* send IPI to all CPUs in map */
-	stopped_cpus = 0;
-
 	/* send the Xcpustop IPI to all CPUs in map */
 	selected_apic_ipi(map, XCPUSTOP_OFFSET, APIC_DELMODE_FIXED);
-
-	while (stopped_cpus != map)
+	
+	while ((stopped_cpus & map) != map)
 		/* spin */ ;
 
 	return 1;
@@ -2043,9 +2042,8 @@ restart_cpus(u_int map)
 
 	started_cpus = map;		/* signal other cpus to restart */
 
-	while (started_cpus)		/* wait for each to clear its bit */
+	while ((stopped_cpus & map) != 0) /* wait for each to clear its bit */
 		/* spin */ ;
-	stopped_cpus = 0;
 
 	return 1;
 }
@@ -2075,6 +2073,11 @@ int forward_signal_enabled = 1;
 SYSCTL_INT(_machdep, OID_AUTO, forward_signal_enabled, CTLFLAG_RW,
 	   &forward_signal_enabled, 0, "");
 
+/* Enable forwarding of roundrobin to all other cpus */
+int forward_roundrobin_enabled = 1;
+SYSCTL_INT(_machdep, OID_AUTO, forward_roundrobin_enabled, CTLFLAG_RW,
+	   &forward_roundrobin_enabled, 0, "");
+
 /*
  * This is called once the rest of the system is up and running and we're
  * ready to let the AP's out of the pen.
@@ -2296,9 +2299,11 @@ forward_statclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_statclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2369,9 +2374,11 @@ forward_hardclock(int pscnt)
 	while (checkstate_probed_cpus != map) {
 		/* spin */
 		i++;
-		if (i == 1000000) {
+		if (i == 100000) {
+#ifdef BETTER_CLOCK_DIAGNOSTIC
 			printf("forward_hardclock: checkstate %x\n",
 			       checkstate_probed_cpus);
+#endif
 			break;
 		}
 	}
@@ -2475,6 +2482,37 @@ forward_signal(struct proc *p)
 	}
 }
 
+void
+forward_roundrobin(void)
+{
+	u_int map;
+	int i;
+
+	if (!smp_started || !invltlb_ok || cold || panicstr)
+		return;
+	if (!forward_roundrobin_enabled)
+		return;
+	resched_cpus |= other_cpus;
+	map = other_cpus & ~stopped_cpus ;
+#if 1
+	selected_apic_ipi(map, XCPUAST_OFFSET, APIC_DELMODE_FIXED);
+#else
+	(void) all_but_self_ipi(XCPUAST_OFFSET);
+#endif
+	i = 0;
+	while ((checkstate_need_ast & map) != 0) {
+		/* spin */
+		i++;
+		if (i > 100000) {
+#if 0
+			printf("forward_roundrobin: dropped ast 0x%x\n",
+			       checkstate_need_ast & map);
+#endif
+			break;
+		}
+	}
+}
+
 
 #ifdef APIC_INTR_REORDER
 /*
diff --git a/sys/sys/smp.h b/sys/sys/smp.h
index 283c925..f258630 100644
--- a/sys/sys/smp.h
+++ b/sys/sys/smp.h
@@ -6,7 +6,7 @@
  * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
  * ----------------------------------------------------------------------------
  *
- * $Id: smp.h,v 1.41 1998/04/01 20:38:28 tegge Exp $
+ * $Id: smp.h,v 1.42 1998/04/01 21:07:36 tegge Exp $
  *
  */
 
@@ -80,6 +80,8 @@ extern volatile u_int		started_cpus;
 
 extern volatile u_int		checkstate_probed_cpus;
 extern volatile u_int		checkstate_need_ast;
+extern volatile u_int		resched_cpus;
+extern void (*cpustop_restartfunc) __P((void));
 
 /* functions in apic_ipl.s */
 void	apic_eoi		__P((void));
@@ -103,6 +105,7 @@ extern int			io_num_to_apic_id[];
 extern int			apic_id_to_logical[];
 extern u_int			all_cpus;
 extern u_char			SMP_ioapic[];
+extern struct pcb		stoppcbs[];
 
 /* functions in mp_machdep.c */
 u_int	mp_bootaddress		__P((u_int));
@@ -131,6 +134,7 @@ void	forward_statclock	__P((int pscnt));
 void	forward_hardclock	__P((int pscnt));
 #endif /* BETTER_CLOCK */
 void	forward_signal		__P((struct proc *));
+void	forward_roundrobin	__P((void));
 #ifdef	APIC_INTR_REORDER
 void	set_lapic_isrloc	__P((int, int));
 #endif /* APIC_INTR_REORDER */