Merge basic SMP support from HEAD

13 files changed, 421 insertions, 115 deletions

diff --git a/sys/conf/files.i386 b/sys/conf/files.i386
index 2d56480..1337c73 100644
--- a/sys/conf/files.i386
+++ b/sys/conf/files.i386
@@ -326,7 +326,8 @@ i386/i386/mp_machdep.c		optional native smp
 i386/xen/mp_machdep.c		optional xen smp
 i386/i386/mp_watchdog.c		optional mp_watchdog smp
 i386/i386/mpboot.s		optional native smp
-i386/i386/mptable.c		optional apic
+i386/xen/mptable.c		optional apic xen
+i386/i386/mptable.c		optional apic native
 i386/i386/mptable_pci.c		optional apic pci
 i386/i386/msi.c			optional apic pci
 i386/i386/nexus.c		standard
diff --git a/sys/i386/i386/apic_vector.s b/sys/i386/i386/apic_vector.s
index 294e2de..514ef75 100644
--- a/sys/i386/i386/apic_vector.s
+++ b/sys/i386/i386/apic_vector.s
@@ -299,6 +299,7 @@ IDTVEC(invlcache)
 /*
  * Handler for IPIs sent via the per-cpu IPI bitmap.
  */
+#ifndef XEN
 	.text
 	SUPERALIGN_TEXT
 IDTVEC(ipi_intr_bitmap_handler)	
@@ -320,7 +321,7 @@ IDTVEC(ipi_intr_bitmap_handler)
 	addl	$4, %esp	/* XXX convert clockframe to trapframe */
 	MEXITCOUNT
 	jmp	doreti
-
+#endif
 /*
  * Executed by a CPU when it receives an Xcpustop IPI from another CPU,
  *
diff --git a/sys/i386/include/apicvar.h b/sys/i386/include/apicvar.h
index fdb724f..ef3c01a 100644
--- a/sys/i386/include/apicvar.h
+++ b/sys/i386/include/apicvar.h
@@ -113,6 +113,17 @@
 #define	APIC_THERMAL_INT (APIC_LOCAL_INTS + 1)
 
 #define	APIC_IPI_INTS	(APIC_LOCAL_INTS + 2)
+#ifdef XEN
+#define	IPI_RENDEZVOUS		(2)	/* Inter-CPU rendezvous. */
+#define	IPI_INVLTLB		(3)	/* TLB Shootdown IPIs */
+#define	IPI_INVLPG		(4)
+#define	IPI_INVLRNG		(5)
+#define	IPI_INVLCACHE		(6)
+#define	IPI_LAZYPMAP		(7)	/* Lazy pmap release. */
+/* Vector to handle bitmap based IPIs */
+#define	IPI_BITMAP_VECTOR	(8)
+
+#else
 #define	IPI_RENDEZVOUS	(APIC_IPI_INTS)		/* Inter-CPU rendezvous. */
 #define	IPI_INVLTLB	(APIC_IPI_INTS + 1)	/* TLB Shootdown IPIs */
 #define	IPI_INVLPG	(APIC_IPI_INTS + 2)
@@ -121,6 +132,7 @@
 #define	IPI_LAZYPMAP	(APIC_IPI_INTS + 5)	/* Lazy pmap release. */
 /* Vector to handle bitmap based IPIs */
 #define	IPI_BITMAP_VECTOR	(APIC_IPI_INTS + 6) 
+#endif
 
 /* IPIs handled by IPI_BITMAPED_VECTOR  (XXX ups is there a better place?) */
 #define	IPI_AST		0 	/* Generate software trap. */
diff --git a/sys/i386/include/pcpu.h b/sys/i386/include/pcpu.h
index 26902ea..3e27a3f 100644
--- a/sys/i386/include/pcpu.h
+++ b/sys/i386/include/pcpu.h
@@ -47,6 +47,24 @@
  */
 
 #ifdef XEN
+#ifndef NR_VIRQS
+#define	NR_VIRQS	24
+#endif
+#ifndef NR_IPIS
+#define	NR_IPIS		2
+#endif
+
+/* These are peridically updated in shared_info, and then copied here. */
+struct shadow_time_info {
+	uint64_t tsc_timestamp;     /* TSC at last update of time vals.  */
+	uint64_t system_timestamp;  /* Time, in nanosecs, since boot.    */
+	uint32_t tsc_to_nsec_mul;
+	uint32_t tsc_to_usec_mul;
+	int tsc_shift;
+	uint32_t version;
+};
+
+
 #define	PCPU_MD_FIELDS							\
 	struct	pcpu *pc_prvspace;	/* Self-reference */		\
 	struct	pmap *pc_curpmap;					\
@@ -63,7 +81,14 @@
         u_int     pc_pdir;                                              \
         u_int     pc_lazypmap;                                          \
         u_int     pc_rendezvous;                                        \
-        u_int     pc_cpuast						
+        u_int     pc_cpuast;						\
+	uint64_t  pc_processed_system_time;				\
+	struct shadow_time_info pc_shadow_time;				\
+	int	pc_resched_irq;						\
+	int	pc_callfunc_irq;					\
+        int	pc_virq_to_irq[NR_VIRQS];				\
+	int	pc_ipi_to_irq[NR_IPIS]
+	
 
 	
 #else
diff --git a/sys/i386/include/smp.h b/sys/i386/include/smp.h
index 095fa8a..5eb128b 100644
--- a/sys/i386/include/smp.h
+++ b/sys/i386/include/smp.h
@@ -68,7 +68,9 @@ void	ipi_selected(u_int cpus, u_int ipi);
 void	ipi_all(u_int ipi);
 void	ipi_all_but_self(u_int ipi);
 void	ipi_self(u_int ipi);
+#ifndef XEN
 void 	ipi_bitmap_handler(struct clockframe frame);
+#endif
 u_int	mp_bootaddress(u_int);
 int	mp_grab_cpu_hlt(void);
 void	mp_topology(void);
@@ -85,7 +87,14 @@ void	smp_masked_invltlb(u_int mask);
 int ipi_nmi_handler(void);
 void ipi_nmi_selected(u_int32_t cpus);
 #endif
+#ifdef XEN
+void ipi_to_irq_init(void);
+
+#define RESCHEDULE_VECTOR	0
+#define CALL_FUNCTION_VECTOR	1
+#define NR_IPIS			2
 
+#endif
 #endif /* !LOCORE */
 #endif /* SMP */
 
diff --git a/sys/i386/include/xen/evtchn.h b/sys/i386/include/xen/evtchn.h
index 3036f5b..f81a0bb 100644
--- a/sys/i386/include/xen/evtchn.h
+++ b/sys/i386/include/xen/evtchn.h
@@ -35,13 +35,24 @@ void mask_evtchn(int port);
 
 void unmask_evtchn(int port);
 
-
+#ifdef SMP
+void rebind_evtchn_to_cpu(int port, unsigned int cpu);
+#else
+#define rebind_evtchn_to_cpu(port, cpu)	((void)0)
+#endif
+
+static inline
+int test_and_set_evtchn_mask(int port)
+{
+	shared_info_t *s = HYPERVISOR_shared_info;
+	return synch_test_and_set_bit(port, s->evtchn_mask);
+}
 
 static inline void 
 clear_evtchn(int port)
 {
-    shared_info_t *s = HYPERVISOR_shared_info;
-    synch_clear_bit(port, &s->evtchn_pending[0]);
+	shared_info_t *s = HYPERVISOR_shared_info;
+	synch_clear_bit(port, &s->evtchn_pending[0]);
 }
 
 static inline void 
diff --git a/sys/i386/include/xen/xen-os.h b/sys/i386/include/xen/xen-os.h
index 47564db..481e374 100644
--- a/sys/i386/include/xen/xen-os.h
+++ b/sys/i386/include/xen/xen-os.h
@@ -73,10 +73,7 @@ static inline void rep_nop(void)
 #define __builtin_expect(x, expected_value) (x)
 #endif
 
-#define DEFINE_PER_CPU(type, name) \
-    __typeof__(type) per_cpu__##name
-
-#define per_cpu(var, cpu)           (*((void)cpu, &per_cpu__##var))
+#define per_cpu(var, cpu)           (pcpu_find((cpu))->pc_ ## var)
 
 /* crude memory allocator for memory allocation early in 
  *  boot
@@ -94,12 +91,6 @@ void printk(const char *fmt, ...);
 /* some function prototypes */
 void trap_init(void);
 
-extern int preemptable;
-#define preempt_disable() (preemptable = 0)
-#define preempt_enable() (preemptable = 1)
-#define preempt_enable_no_resched() (preemptable = 1)
-
-
 /*
  * STI/CLI equivalents. These basically set and clear the virtual
  * event_enable flag in teh shared_info structure. Note that when
@@ -114,10 +105,8 @@ extern int preemptable;
 #define __cli()                                                         \
 do {                                                                    \
         vcpu_info_t *_vcpu;                                             \
-        preempt_disable();                                              \
         _vcpu = &HYPERVISOR_shared_info->vcpu_info[smp_processor_id()]; \
         _vcpu->evtchn_upcall_mask = 1;                                  \
-        preempt_enable_no_resched();                                    \
         barrier();                                                      \
 } while (0)
 
@@ -125,36 +114,23 @@ do {                                                                    \
 do {                                                                    \
         vcpu_info_t *_vcpu;                                             \
         barrier();                                                      \
-        preempt_disable();                                              \
         _vcpu = &HYPERVISOR_shared_info->vcpu_info[smp_processor_id()]; \
         _vcpu->evtchn_upcall_mask = 0;                                  \
         barrier(); /* unmask then check (avoid races) */                \
         if ( unlikely(_vcpu->evtchn_upcall_pending) )                   \
                 force_evtchn_callback();                                \
-        preempt_enable();                                               \
-} while (0)
-
-
-#define __save_flags(x)                                                       \
-do {                                                                          \
-    vcpu_info_t *vcpu;                                                        \
-    vcpu = HYPERVISOR_shared_info->vcpu_info[smp_processor_id()];             \
-    (x) = _vcpu->evtchn_upcall_mask;                                          \
 } while (0)
 
 #define __restore_flags(x)                                              \
 do {                                                                    \
         vcpu_info_t *_vcpu;                                             \
         barrier();                                                      \
-        preempt_disable();                                              \
         _vcpu = &HYPERVISOR_shared_info->vcpu_info[smp_processor_id()]; \
         if ((_vcpu->evtchn_upcall_mask = (x)) == 0) {                   \
                 barrier(); /* unmask then check (avoid races) */        \
                 if ( unlikely(_vcpu->evtchn_upcall_pending) )           \
                         force_evtchn_callback();                        \
-                preempt_enable();                                       \
-        } else                                                          \
-                preempt_enable_no_resched();                            \
+        } 								\
 } while (0)
 
 /*
diff --git a/sys/i386/include/xen/xen_intr.h b/sys/i386/include/xen/xen_intr.h
index 18fd8b8..5da39e4 100644
--- a/sys/i386/include/xen/xen_intr.h
+++ b/sys/i386/include/xen/xen_intr.h
@@ -29,7 +29,6 @@
 /* Dynamic binding of event channels and VIRQ sources to Linux IRQ space. */
 extern void unbind_from_irq(int irq);
 
-extern void bind_evtchn_to_cpu(unsigned int chn, unsigned int cpu);
 extern int bind_caller_port_to_irqhandler(unsigned int caller_port,
 	const char *devname, driver_intr_t handler, void *arg,
 	unsigned long irqflags, void **cookiep);
@@ -38,8 +37,12 @@ extern int bind_listening_port_to_irqhandler(unsigned int remote_domain,
 	void **cookiep);
 extern int bind_virq_to_irqhandler(unsigned int virq, unsigned int cpu, const char *devname,
 	driver_intr_t handler, unsigned long irqflags);
-extern int bind_ipi_to_irqhandler(unsigned int ipi, unsigned int cpu, const char *devname,
-									 driver_intr_t handler, unsigned long irqflags);
+extern int bind_ipi_to_irqhandler(unsigned int ipi,
+	unsigned int cpu,
+	const char *devname,
+	driver_intr_t handler,
+	unsigned long irqflags);
+
 extern int bind_interdomain_evtchn_to_irqhandler(unsigned int remote_domain,
 	                                             unsigned int remote_port,
 	                                             const char *devname,
@@ -61,7 +64,7 @@ extern void enable_irq(unsigned int);
 extern void irq_suspend(void);
 extern void irq_resume(void);
 
-extern void idle_block(void);
-
+extern void	idle_block(void);
+extern int	ap_cpu_initclocks(int cpu);
 
 #endif /* _XEN_INTR_H_ */
diff --git a/sys/i386/include/xen/xenfunc.h b/sys/i386/include/xen/xenfunc.h
index fe40792..f80204d 100644
--- a/sys/i386/include/xen/xenfunc.h
+++ b/sys/i386/include/xen/xenfunc.h
@@ -65,8 +65,6 @@ void _xen_machphys_update(vm_paddr_t, vm_paddr_t, char *file, int line);
 
 void xen_update_descriptor(union descriptor *, union descriptor *);
 
-void ap_cpu_initclocks(void);
-
 extern struct mtx balloon_lock;
 #if 0
 #define balloon_lock(__flags)   mtx_lock_irqsave(&balloon_lock, __flags)
diff --git a/sys/i386/xen/clock.c b/sys/i386/xen/clock.c
index 41b2f6a..55819b5 100644
--- a/sys/i386/xen/clock.c
+++ b/sys/i386/xen/clock.c
@@ -161,19 +161,6 @@ SYSCTL_INT(_machdep, OID_AUTO, xen_disable_rtc_set,
 })
 
 
-/* These are peridically updated in shared_info, and then copied here. */
-struct shadow_time_info {
-	uint64_t tsc_timestamp;     /* TSC at last update of time vals.  */
-	uint64_t system_timestamp;  /* Time, in nanosecs, since boot.    */
-	uint32_t tsc_to_nsec_mul;
-	uint32_t tsc_to_usec_mul;
-	int tsc_shift;
-	uint32_t version;
-};
-static DEFINE_PER_CPU(uint64_t, processed_system_time);
-static DEFINE_PER_CPU(struct shadow_time_info, shadow_time);
-
-
 #define NS_PER_TICK (1000000000ULL/hz)
 
 #define rdtscll(val) \
@@ -868,25 +855,26 @@ cpu_initclocks(void)
 	/* should fast clock be enabled ? */
 }
 
-/*
- *
- * XXX 
- */
-#if 0 && defined(SMP)
-void
-ap_cpu_initclocks(void)
+
+int
+ap_cpu_initclocks(int cpu)
 {
-	int irq;
-	int cpu = smp_processor_id();
-	
-	per_cpu(processed_system_time, cpu) = processed_system_time;
+	int time_irq;
+
+	xen_set_periodic_tick.period_ns = NS_PER_TICK;
 
-	irq = bind_virq_to_irq(VIRQ_TIMER);
-	PCPU_SET(time_irq, irq);
-	PANIC_IF(intr_add_handler("clk", irq, (driver_intr_t *)clkintr, NULL,
-				  NULL, INTR_TYPE_CLK | INTR_FAST, NULL));
+	HYPERVISOR_vcpu_op(VCPUOP_set_periodic_timer, cpu,
+			   &xen_set_periodic_tick);
+
+        if ((time_irq = bind_virq_to_irqhandler(VIRQ_TIMER, cpu, "clk", 
+		    (driver_intr_t *)clkintr, 
+						INTR_TYPE_CLK | INTR_FAST)) < 0) {
+		panic("failed to register clock interrupt\n");
+	}
+
+	return (0);
 }
-#endif
+
 
 void
 cpu_startprofclock(void)
diff --git a/sys/i386/xen/exception.s b/sys/i386/xen/exception.s
index 6b28c5a..3a2bfdb 100644
--- a/sys/i386/xen/exception.s
+++ b/sys/i386/xen/exception.s
@@ -37,18 +37,34 @@
 #include <machine/psl.h>
 #include <machine/trap.h>
 
-
 #include "assym.s"
 
 #define	SEL_RPL_MASK	0x0002
 #define __HYPERVISOR_iret	23
 	
 /* Offsets into shared_info_t. */
+
 #define evtchn_upcall_pending /* 0 */
 #define evtchn_upcall_mask       1
-#define XEN_BLOCK_EVENTS(reg)     movb $1,evtchn_upcall_mask(reg)
-#define XEN_UNBLOCK_EVENTS(reg)   movb $0,evtchn_upcall_mask(reg)
-#define XEN_TEST_PENDING(reg)     testb $0x1,evtchn_upcall_pending(reg)
+
+#define	sizeof_vcpu_shift	6
+
+		
+#ifdef SMP
+#define GET_VCPU_INFO(reg)	movl PCPU(CPUID),reg			; \
+				shl  $sizeof_vcpu_shift,reg		; \
+				addl HYPERVISOR_shared_info,reg
+#else
+#define GET_VCPU_INFO(reg)	movl HYPERVISOR_shared_info,reg
+#endif
+
+#define __DISABLE_INTERRUPTS(reg)	movb $1,evtchn_upcall_mask(reg)
+#define __ENABLE_INTERRUPTS(reg)	movb $0,evtchn_upcall_mask(reg)
+#define DISABLE_INTERRUPTS(reg)	GET_VCPU_INFO(reg)			; \
+				__DISABLE_INTERRUPTS(reg)
+#define ENABLE_INTERRUPTS(reg)	GET_VCPU_INFO(reg)			; \
+				__ENABLE_INTERRUPTS(reg)
+#define __TEST_PENDING(reg)	testb $0xFF,evtchn_upcall_pending(reg)
 
 #define POPA \
         popl %edi; \
@@ -161,8 +177,7 @@ call_evtchn_upcall:
 	
 	
 hypervisor_callback_pending:
-	movl	HYPERVISOR_shared_info,%esi
-	XEN_BLOCK_EVENTS(%esi)				/*	cli */	
+	DISABLE_INTERRUPTS(%esi)				/*	cli */	
 	jmp	10b
 
 	/*
@@ -327,12 +342,11 @@ doreti_ast:
 	 * interrupts provides sufficient locking even in the SMP case,
 	 * since we will be informed of any new ASTs by an IPI.
 	 */
-	movl	HYPERVISOR_shared_info,%esi
-	XEN_BLOCK_EVENTS(%esi)				/*	cli */
+	DISABLE_INTERRUPTS(%esi)				/*	cli */
 	movl	PCPU(CURTHREAD),%eax
 	testl	$TDF_ASTPENDING | TDF_NEEDRESCHED,TD_FLAGS(%eax)
 	je	doreti_exit
-	XEN_UNBLOCK_EVENTS(%esi)	/* sti */
+	ENABLE_INTERRUPTS(%esi)	/* sti */
 	pushl	%esp			/* pass a pointer to the trapframe */
 	call	ast
 	add	$4,%esp
@@ -346,12 +360,11 @@ doreti_ast:
 	 *	registers.  The fault is handled in trap.c.
 	 */
 doreti_exit:
-	movl	HYPERVISOR_shared_info,%esi
-	XEN_UNBLOCK_EVENTS(%esi) # reenable event callbacks (sti)
+	ENABLE_INTERRUPTS(%esi) # reenable event callbacks (sti)
 
 	.globl	scrit
 scrit:
-	XEN_TEST_PENDING(%esi)
+	__TEST_PENDING(%esi)
         jnz	hypervisor_callback_pending	/* More to go  */
 
 	MEXITCOUNT
diff --git a/sys/i386/xen/mp_machdep.c b/sys/i386/xen/mp_machdep.c
index 631dafe..a0946e3 100644
--- a/sys/i386/xen/mp_machdep.c
+++ b/sys/i386/xen/mp_machdep.c
@@ -83,6 +83,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/specialreg.h>
 
 #include <machine/xen/hypervisor.h>
+#include <machine/xen/xen_intr.h>
 #include <machine/xen/evtchn.h>
 #include <xen/interface/vcpu.h>
 
@@ -126,6 +127,9 @@ char *bootSTK;
 static int bootAP;
 static union descriptor *bootAPgdt;
 
+static char resched_name[NR_CPUS][15];
+static char callfunc_name[NR_CPUS][15];
+
 /* Free these after use */
 void *bootstacks[MAXCPU];
 
@@ -139,6 +143,9 @@ vm_offset_t smp_tlb_addr1;
 vm_offset_t smp_tlb_addr2;
 volatile int smp_tlb_wait;
 
+typedef void call_data_func_t(uintptr_t , uintptr_t);
+
+
 #ifdef COUNT_IPIS
 /* Interrupt counts. */
 #ifdef IPI_PREEMPTION
@@ -191,6 +198,7 @@ static u_int	hyperthreading_cpus;
 static cpumask_t	hyperthreading_cpus_mask;
 extern void Xhypervisor_callback(void);
 extern void failsafe_callback(void);
+extern void pmap_lazyfix_action(void);
 
 void
 mp_topology(void)
@@ -376,6 +384,151 @@ cpu_mp_start(void)
 	set_interrupt_apic_ids();
 }
 
+
+static void
+iv_rendezvous(uintptr_t a, uintptr_t b)
+{
+	smp_rendezvous_action();
+}
+
+static void
+iv_invltlb(uintptr_t a, uintptr_t b)
+{
+	xen_tlb_flush();
+}
+
+static void
+iv_invlpg(uintptr_t a, uintptr_t b)
+{
+	xen_invlpg(a);
+}
+
+static void
+iv_invlrng(uintptr_t a, uintptr_t b)
+{
+	vm_offset_t start = (vm_offset_t)a;
+	vm_offset_t end = (vm_offset_t)b;
+
+	while (start < end) {
+		xen_invlpg(start);
+		start += PAGE_SIZE;
+	}
+}
+
+
+static void
+iv_invlcache(uintptr_t a, uintptr_t b)
+{
+
+	wbinvd();
+}
+
+static void
+iv_lazypmap(uintptr_t a, uintptr_t b)
+{
+	pmap_lazyfix_action();
+}
+
+
+static void
+iv_noop(uintptr_t a, uintptr_t b)
+{
+}
+
+static call_data_func_t *ipi_vectors[IPI_BITMAP_VECTOR] = 
+{
+  iv_noop,
+  iv_noop,
+  iv_rendezvous,
+  iv_invltlb,
+  iv_invlpg,
+  iv_invlrng,
+  iv_invlcache,
+  iv_lazypmap,
+};
+
+/*
+ * Reschedule call back. Nothing to do,
+ * all the work is done automatically when
+ * we return from the interrupt.
+ */
+static void
+smp_reschedule_interrupt(void *unused)
+{
+	int cpu = PCPU_GET(cpuid);
+	u_int ipi_bitmap;
+
+	ipi_bitmap = atomic_readandclear_int(&cpu_ipi_pending[cpu]);
+
+#ifdef IPI_PREEMPTION
+	if (ipi_bitmap & (1 << IPI_PREEMPT)) {
+#ifdef COUNT_IPIS
+		*ipi_preempt_counts[cpu]++;
+#endif
+		mtx_lock_spin(&sched_lock);
+		/* Don't preempt the idle thread */
+		if (curthread != PCPU_GET(idlethread)) {
+			struct thread *running_thread = curthread;
+			if (running_thread->td_critnest > 1) 
+				running_thread->td_owepreempt = 1;
+			else 		
+				mi_switch(SW_INVOL | SW_PREEMPT, NULL);
+		}
+		mtx_unlock_spin(&sched_lock);
+	}
+#endif
+
+	if (ipi_bitmap & (1 << IPI_AST)) {
+#ifdef COUNT_IPIS
+		*ipi_ast_counts[cpu]++;
+#endif
+		/* Nothing to do for AST */
+	}
+}
+
+struct _call_data {
+	uint16_t func_id;
+	uint16_t wait;
+	uintptr_t arg1;
+	uintptr_t arg2;
+	atomic_t started;
+	atomic_t finished;
+};
+
+static struct _call_data *call_data;
+
+static void
+smp_call_function_interrupt(void *arg)
+{	
+	call_data_func_t *func;
+	uintptr_t arg1 = call_data->arg1;
+	uintptr_t arg2 = call_data->arg2;
+	int wait = call_data->wait;
+	atomic_t *started = &call_data->started;
+	atomic_t *finished = &call_data->finished;
+
+	if (call_data->func_id > IPI_BITMAP_VECTOR)
+		panic("invalid function id %u", call_data->func_id);
+	
+	func = ipi_vectors[call_data->func_id];
+	/*
+	 * Notify initiating CPU that I've grabbed the data and am
+	 * about to execute the function
+	 */
+	mb();
+	atomic_inc(started);
+	/*
+	 * At this point the info structure may be out of scope unless wait==1
+	 */
+	(*func)(arg1, arg2);
+
+	if (wait) {
+		mb();
+		atomic_inc(finished);
+	}
+	atomic_add_int(&smp_tlb_wait, 1);
+}
+
 /*
  * Print various information about the SMP system hardware and setup.
  */
@@ -399,6 +552,61 @@ cpu_mp_announce(void)
 	}
 }
 
+static int
+xen_smp_intr_init(unsigned int cpu)
+{
+	int rc;
+
+	per_cpu(resched_irq, cpu) = per_cpu(callfunc_irq, cpu) = -1;
+
+	sprintf(resched_name[cpu], "resched%u", cpu);
+	rc = bind_ipi_to_irqhandler(RESCHEDULE_VECTOR,
+				    cpu,
+				    resched_name[cpu],
+				    smp_reschedule_interrupt,
+				    INTR_FAST|INTR_TYPE_TTY|INTR_MPSAFE);
+
+	printf("cpu=%d irq=%d vector=%d\n",
+	    cpu, rc, RESCHEDULE_VECTOR);
+	
+	per_cpu(resched_irq, cpu) = rc;
+
+	sprintf(callfunc_name[cpu], "callfunc%u", cpu);
+	rc = bind_ipi_to_irqhandler(CALL_FUNCTION_VECTOR,
+				    cpu,
+				    callfunc_name[cpu],
+				    smp_call_function_interrupt,
+				    INTR_FAST|INTR_TYPE_TTY|INTR_MPSAFE);
+	if (rc < 0)
+		goto fail;
+	per_cpu(callfunc_irq, cpu) = rc;
+
+	printf("cpu=%d irq=%d vector=%d\n",
+	    cpu, rc, CALL_FUNCTION_VECTOR);
+
+	
+	if ((cpu != 0) && ((rc = ap_cpu_initclocks(cpu)) != 0))
+		goto fail;
+
+	return 0;
+
+ fail:
+	if (per_cpu(resched_irq, cpu) >= 0)
+		unbind_from_irqhandler(per_cpu(resched_irq, cpu), NULL);
+	if (per_cpu(callfunc_irq, cpu) >= 0)
+		unbind_from_irqhandler(per_cpu(callfunc_irq, cpu), NULL);
+	return rc;
+}
+
+static void
+xen_smp_intr_init_cpus(void *unused)
+{
+	int i;
+	    
+	for (i = 0; i < mp_ncpus; i++)
+		xen_smp_intr_init(i);
+}
+
 #define MTOPSIZE (1<<(14 + PAGE_SHIFT))
 /*
  * AP CPU's call this to initialize themselves.
@@ -881,19 +1089,24 @@ static void
 smp_tlb_shootdown(u_int vector, vm_offset_t addr1, vm_offset_t addr2)
 {
 	u_int ncpu;
+	struct _call_data data;
 
+	call_data = &data;
+	
 	ncpu = mp_ncpus - 1;	/* does not shootdown self */
 	if (ncpu < 1)
 		return;		/* no other cpus */
 	if (!(read_eflags() & PSL_I))
 		panic("%s: interrupts disabled", __func__);
 	mtx_lock_spin(&smp_ipi_mtx);
-	smp_tlb_addr1 = addr1;
-	smp_tlb_addr2 = addr2;
+	call_data->func_id = vector;
+	call_data->arg1 = addr1;
+	call_data->arg2 = addr2;
 	atomic_store_rel_int(&smp_tlb_wait, 0);
 	ipi_all_but_self(vector);
 	while (smp_tlb_wait < ncpu)
 		ia32_pause();
+	call_data = NULL;
 	mtx_unlock_spin(&smp_ipi_mtx);
 }
 
@@ -901,6 +1114,7 @@ static void
 smp_targeted_tlb_shootdown(u_int mask, u_int vector, vm_offset_t addr1, vm_offset_t addr2)
 {
 	int ncpu, othercpus;
+	struct _call_data data;
 
 	othercpus = mp_ncpus - 1;
 	if (mask == (u_int)-1) {
@@ -925,8 +1139,10 @@ smp_targeted_tlb_shootdown(u_int mask, u_int vector, vm_offset_t addr1, vm_offse
 	if (!(read_eflags() & PSL_I))
 		panic("%s: interrupts disabled", __func__);
 	mtx_lock_spin(&smp_ipi_mtx);
-	smp_tlb_addr1 = addr1;
-	smp_tlb_addr2 = addr2;
+	call_data = &data;		
+	call_data->func_id = vector;
+	call_data->arg1 = addr1;
+	call_data->arg2 = addr2;
 	atomic_store_rel_int(&smp_tlb_wait, 0);
 	if (mask == (u_int)-1)
 		ipi_all_but_self(vector);
@@ -934,6 +1150,7 @@ smp_targeted_tlb_shootdown(u_int mask, u_int vector, vm_offset_t addr1, vm_offse
 		ipi_selected(mask, vector);
 	while (smp_tlb_wait < ncpu)
 		ia32_pause();
+	call_data = NULL;
 	mtx_unlock_spin(&smp_ipi_mtx);
 }
 
@@ -1019,6 +1236,8 @@ smp_masked_invlpg_range(u_int mask, vm_offset_t addr1, vm_offset_t addr2)
 	}
 }
 
+void
+ipi_bitmap_handler(struct clockframe frame);
 
 void
 ipi_bitmap_handler(struct clockframe frame)
@@ -1058,17 +1277,17 @@ ipi_bitmap_handler(struct clockframe frame)
  * send an IPI to a set of cpus.
  */
 void
-ipi_selected(u_int32_t cpus, u_int ipi)
+ipi_selected(uint32_t cpus, u_int ipi)
 {
 	int cpu;
 	u_int bitmap = 0;
 	u_int old_pending;
 	u_int new_pending;
-
+	
 	if (IPI_IS_BITMAPED(ipi)) { 
 		bitmap = 1 << ipi;
 		ipi = IPI_BITMAP_VECTOR;
-	}
+	} 
 
 	CTR3(KTR_SMP, "%s: cpus: %x ipi: %x", __func__, cpus, ipi);
 	while ((cpu = ffs(cpus)) != 0) {
@@ -1084,11 +1303,15 @@ ipi_selected(u_int32_t cpus, u_int ipi)
 				new_pending = old_pending | bitmap;
 			} while  (!atomic_cmpset_int(&cpu_ipi_pending[cpu],old_pending, new_pending));	
 
-			if (old_pending)
-				continue;
+			if (!old_pending)
+				ipi_pcpu(cpu, RESCHEDULE_VECTOR);
+			continue;
+			
 		}
 		
-		ipi_pcpu(cpu, ipi);
+		KASSERT(call_data != NULL, ("call_data not set"));
+
+		ipi_pcpu(cpu, CALL_FUNCTION_VECTOR);
 	}
 
 }
@@ -1101,7 +1324,7 @@ ipi_all(u_int ipi)
 {
 
 	CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
-	ipi_selected(all_cpus, ipi);
+	ipi_selected(PCPU_GET(other_cpus), ipi);
 }
 
 /*
@@ -1143,6 +1366,7 @@ release_aps(void *dummy __unused)
 	mtx_unlock_spin(&sched_lock);
 }
 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL);
+SYSINIT(start_ipis, SI_SUB_INTR, SI_ORDER_ANY, xen_smp_intr_init_cpus, NULL);
 
 #ifdef COUNT_IPIS
 /*
diff --git a/sys/xen/evtchn/evtchn.c b/sys/xen/evtchn/evtchn.c
index c296664..a782f24 100644
--- a/sys/xen/evtchn/evtchn.c
+++ b/sys/xen/evtchn/evtchn.c
@@ -18,6 +18,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/lock.h>
 #include <sys/mutex.h>
 #include <sys/interrupt.h>
+#include <sys/pcpu.h>
 
 #include <machine/cpufunc.h>
 #include <machine/intr_machdep.h>
@@ -28,6 +29,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/xen/synch_bitops.h>
 #include <machine/xen/evtchn.h>
 #include <machine/xen/hypervisor.h>
+#include <sys/smp.h>
 
 
 
@@ -101,27 +103,58 @@ enum {
 	IRQT_VIRQ,
 	IRQT_IPI,
 	IRQT_LOCAL_PORT,
-	IRQT_CALLER_PORT
+	IRQT_CALLER_PORT,
+	_IRQT_COUNT
+	
 };
 
+
+#define _IRQT_BITS 4
+#define _EVTCHN_BITS 12
+#define _INDEX_BITS (32 - _IRQT_BITS - _EVTCHN_BITS)
+
 /* Constructor for packed IRQ information. */
-#define mk_irq_info(type, index, evtchn)				\
-	(((uint32_t)(type) << 24) | ((uint32_t)(index) << 16) | (uint32_t)(evtchn))
+static inline uint32_t
+mk_irq_info(uint32_t type, uint32_t index, uint32_t evtchn)
+{
+
+	return ((type << (32 - _IRQT_BITS)) | (index << _EVTCHN_BITS) | evtchn);
+}
+
+/* Constructor for packed IRQ information. */
+
 /* Convenient shorthand for packed representation of an unbound IRQ. */
 #define IRQ_UNBOUND	mk_irq_info(IRQT_UNBOUND, 0, 0)
-/* Accessor macros for packed IRQ information. */
-#define evtchn_from_irq(irq) ((uint16_t)(irq_info[irq]))
-#define index_from_irq(irq)  ((uint8_t)(irq_info[irq] >> 16))
-#define type_from_irq(irq)   ((uint8_t)(irq_info[irq] >> 24))
+
+/*
+ * Accessors for packed IRQ information.
+ */
+
+static inline unsigned int evtchn_from_irq(int irq)
+{
+	return irq_info[irq] & ((1U << _EVTCHN_BITS) - 1);
+}
+
+static inline unsigned int index_from_irq(int irq)
+{
+	return (irq_info[irq] >> _EVTCHN_BITS) & ((1U << _INDEX_BITS) - 1);
+}
+
+static inline unsigned int type_from_irq(int irq)
+{
+	return irq_info[irq] >> (32 - _IRQT_BITS);
+}
+
 
 /* IRQ <-> VIRQ mapping. */ 
-DEFINE_PER_CPU(int, virq_to_irq[NR_VIRQS]) = {[0 ... NR_VIRQS-1] = -1}; 
  
 /* IRQ <-> IPI mapping. */ 
-#ifndef NR_IPIS 
+#ifndef NR_IPIS
+#ifdef SMP
+#error "NR_IPIS not defined"
+#endif
 #define NR_IPIS 1 
 #endif 
-DEFINE_PER_CPU(int, ipi_to_irq[NR_IPIS]) = {[0 ... NR_IPIS-1] = -1}; 
 
 /* Bitmap indicating which PIRQs require Xen to be notified on unmask. */
 static unsigned long pirq_needs_unmask_notify[NR_PIRQS/sizeof(unsigned long)];
@@ -131,9 +164,9 @@ static int irq_bindcount[NR_IRQS];
 
 #define VALID_EVTCHN(_chn) ((_chn) != 0)
 
-#ifdef CONFIG_SMP
+#ifdef SMP
 
-static u8 cpu_evtchn[NR_EVENT_CHANNELS];
+static uint8_t cpu_evtchn[NR_EVENT_CHANNELS];
 static unsigned long cpu_evtchn_mask[NR_CPUS][NR_EVENT_CHANNELS/BITS_PER_LONG];
 
 #define active_evtchns(cpu,sh,idx)		\
@@ -224,8 +257,10 @@ evtchn_do_upcall(struct intrframe *frame)
 void
 ipi_pcpu(unsigned int cpu, int vector) 
 { 
-        int irq = per_cpu(ipi_to_irq, cpu)[vector]; 
+        int irq;
 
+	irq = per_cpu(ipi_to_irq, cpu)[vector]; 
+	
         notify_remote_via_irq(irq); 
 } 
 
@@ -333,6 +368,9 @@ bind_virq_to_irq(unsigned int virq, unsigned int cpu)
 	mtx_lock_spin(&irq_mapping_update_lock);
 
 	if ((irq = per_cpu(virq_to_irq, cpu)[virq]) == -1) {
+		if ((irq = find_unbound_irq()) < 0)
+			goto out;
+
 		bind_virq.virq = virq;
 		bind_virq.vcpu = cpu;
 		PANIC_IF(HYPERVISOR_event_channel_op(EVTCHNOP_bind_virq,
@@ -340,7 +378,6 @@ bind_virq_to_irq(unsigned int virq, unsigned int cpu)
 
 		evtchn = bind_virq.port;
 
-		irq = find_unbound_irq();
 		evtchn_to_irq[evtchn] = irq;
 		irq_info[irq] = mk_irq_info(IRQT_VIRQ, virq, evtchn);
 
@@ -350,13 +387,16 @@ bind_virq_to_irq(unsigned int virq, unsigned int cpu)
 	}
 
 	irq_bindcount[irq]++;
-
+out:
 	mtx_unlock_spin(&irq_mapping_update_lock);
 
 	return irq;
 }
 
-static int 
+
+extern int bind_ipi_to_irq(unsigned int ipi, unsigned int cpu);
+
+int 
 bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 {
 	struct evtchn_bind_ipi bind_ipi;
@@ -372,7 +412,6 @@ bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 		PANIC_IF(HYPERVISOR_event_channel_op(EVTCHNOP_bind_ipi, &bind_ipi) != 0);
 		evtchn = bind_ipi.port;
 
-		irq = find_unbound_irq();
 		evtchn_to_irq[evtchn] = irq;
 		irq_info[irq] = mk_irq_info(IRQT_IPI, ipi, evtchn);
 
@@ -380,7 +419,6 @@ bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 
 		bind_evtchn_to_cpu(evtchn, cpu);
 	}
-
 	irq_bindcount[irq]++;
 out:
 	
@@ -518,9 +556,8 @@ bind_ipi_to_irqhandler(unsigned int ipi,
 		       driver_intr_t handler,
 		       unsigned long irqflags)
 {
-	unsigned int irq;
-	int retval;
-
+	int irq, retval;
+	
 	irq = bind_ipi_to_irq(ipi, cpu);
 	intr_register_source(&xp->xp_pins[irq].xp_intsrc);
 	retval = intr_add_handler(devname, irq, handler, NULL, irqflags, NULL);
@@ -742,6 +779,8 @@ notify_remote_via_irq(int irq)
 
 	if (VALID_EVTCHN(evtchn))
 		notify_remote_via_evtchn(evtchn);
+	else
+		panic("invalid evtchn");
 }
 
 /* required for support of physical devices */
@@ -792,6 +831,9 @@ xenpic_pirq_enable_intr(struct intsrc *isrc)
 	bind_pirq.flags = probing_irq(irq) ? 0 : BIND_PIRQ__WILL_SHARE;
 	
 	if (HYPERVISOR_event_channel_op(EVTCHNOP_bind_pirq, &bind_pirq) != 0) {
+#ifndef XEN_PRIVILEGED_GUEST
+		panic("unexpected pirq call");
+#endif
 		if (!probing_irq(irq)) /* Some failures are expected when probing. */
 			printf("Failed to obtain physical IRQ %d\n", irq);
 		mtx_unlock_spin(&irq_mapping_update_lock);
@@ -992,8 +1034,11 @@ evtchn_init(void *dummy __unused)
 	int i, cpu;
 	struct xenpic_intsrc *pin, *tpin;
 
-	/* No VIRQ or IPI bindings. */
-	for (cpu = 0; cpu < NR_CPUS; cpu++) {
+
+	init_evtchn_cpu_bindings();
+	
+         /* No VIRQ or IPI bindings. */
+	for (cpu = 0; cpu < mp_ncpus; cpu++) {
 		for (i = 0; i < NR_VIRQS; i++)
 			per_cpu(virq_to_irq, cpu)[i] = -1;
 		for (i = 0; i < NR_IPIS; i++)
@@ -1060,7 +1105,7 @@ evtchn_init(void *dummy __unused)
 	}
 }
 
-SYSINIT(evtchn_init, SI_SUB_INTR, SI_ORDER_ANY, evtchn_init, NULL);
+SYSINIT(evtchn_init, SI_SUB_INTR, SI_ORDER_MIDDLE, evtchn_init, NULL);
     /*
      * irq_mapping_update_lock: in order to allow an interrupt to occur in a critical
      * 	        section, to set pcpu->ipending (etc...) properly, we