Improve SMP support:

o  Allocate a VHPT per CPU. The VHPT is a hash table that the CPU
   uses to look up translations it can't find in the TLB. As such,
   the VHPT serves as a level 1 cache (the TLB being a level 0 cache)
   and best results are obtained when it's not shared between CPUs.
   The collision chain (i.e. the hash bucket) is shared between CPUs,
   as all buckets together constitute our collection of PTEs. To
   achieve this, the collision chain does not point to the first PTE
   in the list anymore, but to a hash bucket head structure. The
   head structure contains the pointer to the first PTE in the list,
   as well as a mutex to lock the bucket. Thus, each bucket is locked
   independently of each other. With at least 1024 buckets in the VHPT,
   this provides for sufficiently finei-grained locking to make the
   ssolution scalable to large SMP machines.
o  Add synchronisation to the lazy FP context switching. We do this
   with a seperate per-thread lock. On SMP machines the lazy high FP
   context switching without synchronisation caused inconsistent
   state, which resulted in a panic. Since the use of the high FP
   registers is not common, it's possible that races exist. The ia64
   package build has proven to be a good stress test, so this will
   get plenty of exercise in the near future.
o  Don't use the local ID of the processor we want to send the IPI to
   as the argument to ipi_send(). use the struct pcpu pointer instead.
   The reason for this is that IPI delivery is unreliable. It has been
   observed that sending an IPI to a CPU causes it to receive a stray
   external interrupt. As such, we need a way to make the delivery
   reliable. The intended solution is to queue requests in the target
   CPU's per-CPU structure and use a single IPI to inform the CPU that
   there's a new entry in the queue. If that IPI gets lost, the CPU
   can check it's queue at any convenient time (such as for each
   clock interrupt). This also allows us to send requests to a CPU
   without interrupting it, if such would be beneficial.

With these changes SMP is almost working. There are still some random
process crashes and the machine can hang due to having the IPI lost
that deals with the high FP context switch.

The overhead of introducing the hash bucket head structure results
in a performance degradation of about 1% for UP (extra pointer
indirection). This is surprisingly small and is offset by gaining
reasonably/good scalable SMP support.

9 files changed, 318 insertions, 320 deletions

diff --git a/sys/ia64/ia64/exception.S b/sys/ia64/ia64/exception.S
index 1130461..96ed2de 100644
--- a/sys/ia64/ia64/exception.S
+++ b/sys/ia64/ia64/exception.S
@@ -644,6 +644,7 @@ IVT_ENTRY(Instruction_TLB, 0x0400)
 	add	r20=24,r18		// collision chain
 	;; 
 	ld8	r21=[r21]		// check VHPT tag
+	ld8	r20=[r20]		// bucket head
 	;;
 	cmp.ne	p15,p0=r21,r19
 (p15)	br.dpnt.few 1f
@@ -722,6 +723,7 @@ IVT_ENTRY(Data_TLB, 0x0800)
 	add	r20=24,r18		// collision chain
 	;; 
 	ld8	r21=[r21]		// check VHPT tag
+	ld8	r20=[r20]		// bucket head
 	;;
 	cmp.ne	p15,p0=r21,r19
 (p15)	br.dpnt.few 1f
@@ -937,6 +939,8 @@ IVT_ENTRY(Dirty_Bit, 0x2000)
 	ttag	r19=r16
 	add	r20=24,r18		// collision chain
 	;; 
+	ld8	r20=[r20]		// bucket head
+	;;
 	ld8	r20=[r20]		// first entry
 	;; 
 	rsm	psr.dt			// turn off data translations
@@ -1003,6 +1007,8 @@ IVT_ENTRY(Instruction_Access_Bit, 0x2400)
 	ttag	r19=r16
 	add	r20=24,r18		// collision chain
 	;; 
+	ld8	r20=[r20]		// bucket head
+	;;
 	ld8	r20=[r20]		// first entry
 	;;
 	rsm	psr.dt			// turn off data translations
@@ -1069,6 +1075,8 @@ IVT_ENTRY(Data_Access_Bit, 0x2800)
 	ttag	r19=r16
 	add	r20=24,r18		// collision chain
 	;;
+	ld8	r20=[r20]		// bucket head
+	;;
 	ld8	r20=[r20]		// first entry
 	;;
 	rsm	psr.dt			// turn off data translations
diff --git a/sys/ia64/ia64/interrupt.c b/sys/ia64/ia64/interrupt.c
index 7e28935..3e1a12a 100644
--- a/sys/ia64/ia64/interrupt.c
+++ b/sys/ia64/ia64/interrupt.c
@@ -147,6 +147,8 @@ interrupt(u_int64_t vector, struct trapframe *tf)
 	if (vector == 0) {
 		vector = ib->ib_inta;
 		printf("ExtINT interrupt: vector=%ld\n", vector);
+		if (vector == 15)
+			goto stray;
 	}
 
 	if (vector == CLOCK_VECTOR) {/* clock interrupt */
@@ -207,9 +209,11 @@ interrupt(u_int64_t vector, struct trapframe *tf)
 	} else if (vector == ipi_vector[IPI_HIGH_FP]) {
 		struct thread *thr = PCPU_GET(fpcurthread);
 		if (thr != NULL) {
+			mtx_lock(&thr->td_md.md_highfp_mtx);
 			save_high_fp(&thr->td_pcb->pcb_high_fp);
 			thr->td_pcb->pcb_fpcpu = NULL;
 			PCPU_SET(fpcurthread, NULL);
+			mtx_unlock(&thr->td_md.md_highfp_mtx);
 		}
 	} else if (vector == ipi_vector[IPI_RENDEZVOUS]) {
 		rdvs[PCPU_GET(cpuid)]++;
@@ -239,6 +243,7 @@ interrupt(u_int64_t vector, struct trapframe *tf)
 		ia64_dispatch_intr(tf, vector);
 	}
 
+stray:
 	atomic_subtract_int(&td->td_intr_nesting_level, 1);
 	return (TRAPF_USERMODE(tf));
 }
diff --git a/sys/ia64/ia64/machdep.c b/sys/ia64/ia64/machdep.c
index 4dd3a38..9aa6045 100644
--- a/sys/ia64/ia64/machdep.c
+++ b/sys/ia64/ia64/machdep.c
@@ -55,6 +55,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/ptrace.h>
 #include <sys/random.h>
 #include <sys/reboot.h>
+#include <sys/sched.h>
 #include <sys/signalvar.h>
 #include <sys/syscall.h>
 #include <sys/sysctl.h>
@@ -764,8 +765,8 @@ ia64_init(void)
 	 */
 	proc0kstack = (vm_offset_t)kstack;
 	thread0.td_kstack = proc0kstack;
-	thread0.td_pcb = (struct pcb *)
-	    (thread0.td_kstack + KSTACK_PAGES * PAGE_SIZE) - 1;
+	thread0.td_kstack_pages = KSTACK_PAGES;
+
 	/*
 	 * Setup the global data for the bootstrap cpu.
 	 */
@@ -774,6 +775,8 @@ ia64_init(void)
 	pcpu_init(pcpup, 0, PAGE_SIZE);
 	PCPU_SET(curthread, &thread0);
 
+	mutex_init();
+
 	/*
 	 * Initialize the rest of proc 0's PCB.
 	 *
@@ -781,14 +784,11 @@ ia64_init(void)
 	 * and make proc0's trapframe pointer point to it for sanity.
 	 * Initialise proc0's backing store to start after u area.
 	 */
-	thread0.td_frame = (struct trapframe *)thread0.td_pcb - 1;
-	thread0.td_frame->tf_length = sizeof(struct trapframe);
+	cpu_thread_setup(&thread0);
 	thread0.td_frame->tf_flags = FRAME_SYSCALL;
 	thread0.td_pcb->pcb_special.sp =
 	    (u_int64_t)thread0.td_frame - 16;
-	thread0.td_pcb->pcb_special.bspstore = (u_int64_t)proc0kstack;
-
-	mutex_init();
+	thread0.td_pcb->pcb_special.bspstore = thread0.td_kstack;
 
 	/*
 	 * Initialize the virtual memory system.
@@ -1428,7 +1428,6 @@ set_fpregs(struct thread *td, struct fpreg *fpregs)
 
 /*
  * High FP register functions.
- * XXX no synchronization yet.
  */
 
 int
@@ -1438,13 +1437,17 @@ ia64_highfp_drop(struct thread *td)
 	struct pcpu *cpu;
 	struct thread *thr;
 
+	mtx_lock(&td->td_md.md_highfp_mtx);
 	pcb = td->td_pcb;
 	cpu = pcb->pcb_fpcpu;
-	if (cpu == NULL)
+	if (cpu == NULL) {
+		mtx_unlock(&td->td_md.md_highfp_mtx);
 		return (0);
+	}
 	pcb->pcb_fpcpu = NULL;
 	thr = cpu->pc_fpcurthread;
 	cpu->pc_fpcurthread = NULL;
+	mtx_unlock(&td->td_md.md_highfp_mtx);
 
 	/* Post-mortem sanity checking. */
 	KASSERT(thr == td, ("Inconsistent high FP state"));
@@ -1462,22 +1465,36 @@ ia64_highfp_save(struct thread *td)
 	if ((td->td_frame->tf_special.psr & IA64_PSR_MFH) == 0)
 		return (ia64_highfp_drop(td));
 
+	mtx_lock(&td->td_md.md_highfp_mtx);
 	pcb = td->td_pcb;
 	cpu = pcb->pcb_fpcpu;
-	if (cpu == NULL)
+	if (cpu == NULL) {
+		mtx_unlock(&td->td_md.md_highfp_mtx);
 		return (0);
+	}
 #ifdef SMP
+	if (td == curthread)
+		sched_pin();
 	if (cpu != pcpup) {
-		ipi_send(cpu->pc_lid, IPI_HIGH_FP);
-		while (pcb->pcb_fpcpu != cpu)
+		mtx_unlock(&td->td_md.md_highfp_mtx);
+		ipi_send(cpu, IPI_HIGH_FP);
+		if (td == curthread)
+			sched_unpin();
+		while (pcb->pcb_fpcpu == cpu)
 			DELAY(100);
 		return (1);
+	} else {
+		save_high_fp(&pcb->pcb_high_fp);
+		if (td == curthread)
+			sched_unpin();
 	}
-#endif
+#else
 	save_high_fp(&pcb->pcb_high_fp);
+#endif
 	pcb->pcb_fpcpu = NULL;
 	thr = cpu->pc_fpcurthread;
 	cpu->pc_fpcurthread = NULL;
+	mtx_unlock(&td->td_md.md_highfp_mtx);
 
 	/* Post-mortem sanity cxhecking. */
 	KASSERT(thr == td, ("Inconsistent high FP state"));
diff --git a/sys/ia64/ia64/mp_machdep.c b/sys/ia64/ia64/mp_machdep.c
index 511a407..59a4b7d 100644
--- a/sys/ia64/ia64/mp_machdep.c
+++ b/sys/ia64/ia64/mp_machdep.c
@@ -64,8 +64,9 @@ MALLOC_DECLARE(M_PMAP);
 
 void ia64_ap_startup(void);
 
-extern vm_offset_t vhpt_base, vhpt_size;
-extern u_int64_t ia64_lapic_address;
+extern uint64_t vhpt_base[];
+extern size_t vhpt_size;
+extern uint64_t ia64_lapic_address;
 
 #define	LID_SAPIC_ID(x)		((int)((x) >> 24) & 0xff)
 #define	LID_SAPIC_EID(x)	((int)((x) >> 16) & 0xff)
@@ -74,9 +75,10 @@ extern u_int64_t ia64_lapic_address;
 
 int	mp_ipi_test = 0;
 
-/* Variables used by os_boot_rendez */
-void *ap_stack;
+/* Variables used by os_boot_rendez and ia64_ap_startup */
 struct pcpu *ap_pcpu;
+void *ap_stack;
+uint64_t ap_vhpt;
 volatile int ap_delay;
 volatile int ap_awake;
 volatile int ap_spin;
@@ -86,15 +88,16 @@ static void cpu_mp_unleash(void *);
 void
 ia64_ap_startup(void)
 {
-	ap_awake = 1;
-	ap_delay = 0;
-
-	__asm __volatile("mov cr.pta=%0;; srlz.i;;" ::
-	    "r" (vhpt_base + (1<<8) + (vhpt_size<<2) + 1));
 
 	pcpup = ap_pcpu;
 	ia64_set_k4((intptr_t)pcpup);
 
+	__asm __volatile("mov cr.pta=%0;; srlz.i;;" ::
+	    "r" (ap_vhpt + (1<<8) + (vhpt_size<<2) + 1));
+
+	ap_awake = 1;
+	ap_delay = 0;
+
 	map_pal_code();
 	map_gateway_page();
 
@@ -102,7 +105,7 @@ ia64_ap_startup(void)
 
 	/* Wait until it's time for us to be unleashed */
 	while (ap_spin)
-		/* spin */;
+		DELAY(0);
 
 	__asm __volatile("ssm psr.i;; srlz.d;;");
 
@@ -119,7 +122,7 @@ ia64_ap_startup(void)
 
 	ap_awake++;
 	while (!smp_started)
-		/* spin */;
+		DELAY(0);
 
 	CTR1(KTR_SMP, "SMP: cpu%d launched", PCPU_GET(cpuid));
 
@@ -242,16 +245,17 @@ cpu_mp_start()
 		pc->pc_current_pmap = kernel_pmap;
 		pc->pc_other_cpus = all_cpus & ~pc->pc_cpumask;
 		if (pc->pc_cpuid > 0) {
+			ap_pcpu = pc;
 			ap_stack = malloc(KSTACK_PAGES * PAGE_SIZE, M_PMAP,
 			    M_WAITOK);
-			ap_pcpu = pc;
+			ap_vhpt = vhpt_base[pc->pc_cpuid];
 			ap_delay = 2000;
 			ap_awake = 0;
 
 			if (bootverbose)
 				printf("SMP: waking up cpu%d\n", pc->pc_cpuid);
 
-			ipi_send(pc->pc_lid, IPI_AP_WAKEUP);
+			ipi_send(pc, IPI_AP_WAKEUP);
 
 			do {
 				DELAY(1000);
@@ -292,7 +296,7 @@ cpu_mp_unleash(void *dummy)
 	ap_spin = 0;
 
 	while (ap_awake != smp_cpus)
-		/* spin */;
+		DELAY(0);
 
 	if (smp_cpus != cpus || cpus != mp_ncpus) {
 		printf("SMP: %d CPUs found; %d CPUs usable; %d CPUs woken\n",
@@ -307,13 +311,13 @@ cpu_mp_unleash(void *dummy)
  * send an IPI to a set of cpus.
  */
 void
-ipi_selected(u_int64_t cpus, int ipi)
+ipi_selected(cpumask_t cpus, int ipi)
 {
 	struct pcpu *pc;
 
 	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
 		if (cpus & pc->pc_cpumask)
-			ipi_send(pc->pc_lid, ipi);
+			ipi_send(pc, ipi);
 	}
 }
 
@@ -326,7 +330,7 @@ ipi_all(int ipi)
 	struct pcpu *pc;
 
 	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
-		ipi_send(pc->pc_lid, ipi);
+		ipi_send(pc, ipi);
 	}
 }
 
@@ -340,7 +344,7 @@ ipi_all_but_self(int ipi)
 
 	SLIST_FOREACH(pc, &cpuhead, pc_allcpu) {
 		if (pc != pcpup)
-			ipi_send(pc->pc_lid, ipi);
+			ipi_send(pc, ipi);
 	}
 }
 
@@ -351,7 +355,7 @@ void
 ipi_self(int ipi)
 {
 
-	ipi_send(ia64_get_lid(), ipi);
+	ipi_send(pcpup, ipi);
 }
 
 /*
@@ -360,17 +364,17 @@ ipi_self(int ipi)
  * fields are used here.
  */
 void
-ipi_send(u_int64_t lid, int ipi)
+ipi_send(struct pcpu *cpu, int ipi)
 {
-	volatile u_int64_t *pipi;
-	u_int64_t vector;
+	volatile uint64_t *pipi;
+	uint64_t vector;
 
 	pipi = __MEMIO_ADDR(ia64_lapic_address |
-	    ((lid & LID_SAPIC_MASK) >> 12));
-	vector = (u_int64_t)(ipi_vector[ipi] & 0xff);
+	    ((cpu->pc_lid & LID_SAPIC_MASK) >> 12));
+	vector = (uint64_t)(ipi_vector[ipi] & 0xff);
+	*pipi = vector;
 	CTR3(KTR_SMP, "ipi_send(%p, %ld), cpuid=%d", pipi, vector,
 	    PCPU_GET(cpuid));
-	*pipi = vector;
 }
 
 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, cpu_mp_unleash, NULL);
diff --git a/sys/ia64/ia64/pmap.c b/sys/ia64/ia64/pmap.c
index 647a7c6..93f3bef 100644
--- a/sys/ia64/ia64/pmap.c
+++ b/sys/ia64/ia64/pmap.c
@@ -155,8 +155,15 @@ struct pmap kernel_pmap_store;
 vm_offset_t virtual_avail;	/* VA of first avail page (after kernel bss) */
 vm_offset_t virtual_end;	/* VA of last avail page (end of kernel AS) */
 
-vm_offset_t vhpt_base, vhpt_size;
-struct mtx pmap_vhptmutex;
+struct ia64_bucket {
+	uint64_t	chain;
+	struct mtx	mutex;
+	u_int		length;
+};
+
+struct ia64_bucket *vhpt_bucket;
+uint64_t vhpt_base[MAXCPU];
+size_t vhpt_size;
 
 /*
  * Kernel virtual memory management.
@@ -177,6 +184,7 @@ static uint64_t pmap_ptc_e_count1 = 3;
 static uint64_t pmap_ptc_e_count2 = 2;
 static uint64_t pmap_ptc_e_stride1 = 0x2000;
 static uint64_t pmap_ptc_e_stride2 = 0x100000000;
+struct mtx pmap_ptcmutex;
 
 /*
  * Data for the RID allocator
@@ -204,14 +212,11 @@ static uma_zone_t ptezone;
  * VHPT instrumentation.
  */
 static int pmap_vhpt_inserts;
-static int pmap_vhpt_collisions;
 static int pmap_vhpt_resident;
 SYSCTL_DECL(_vm_stats);
 SYSCTL_NODE(_vm_stats, OID_AUTO, vhpt, CTLFLAG_RD, 0, "");
 SYSCTL_INT(_vm_stats_vhpt, OID_AUTO, inserts, CTLFLAG_RD,
 	   &pmap_vhpt_inserts, 0, "");
-SYSCTL_INT(_vm_stats_vhpt, OID_AUTO, collisions, CTLFLAG_RD,
-	   &pmap_vhpt_collisions, 0, "");
 SYSCTL_INT(_vm_stats_vhpt, OID_AUTO, resident, CTLFLAG_RD,
 	   &pmap_vhpt_resident, 0, "");
 
@@ -257,8 +262,11 @@ pmap_steal_memory(vm_size_t size)
 void
 pmap_bootstrap()
 {
-	int i, j, count, ridbits;
 	struct ia64_pal_result res;
+	struct ia64_lpte *pte;
+	vm_offset_t base, limit;
+	size_t size;
+	int i, j, count, ridbits;
 
 	/*
 	 * Query the PAL Code to find the loop parameters for the
@@ -280,6 +288,7 @@ pmap_bootstrap()
 		       pmap_ptc_e_count2,
 		       pmap_ptc_e_stride1,
 		       pmap_ptc_e_stride2);
+	mtx_init(&pmap_ptcmutex, "Global PTC lock", NULL, MTX_SPIN);
 
 	/*
 	 * Setup RIDs. RIDs 0..7 are reserved for the kernel.
@@ -335,7 +344,8 @@ pmap_bootstrap()
 	kernel_vm_end = NKPT * PAGE_SIZE * NKPTEPG + VM_MIN_KERNEL_ADDRESS -
 	    VM_GATEWAY_SIZE;
 
-	for (i = 0; phys_avail[i+2]; i+= 2) ;
+	for (i = 0; phys_avail[i+2]; i+= 2)
+		;
 	count = i+2;
 
 	/*
@@ -345,19 +355,20 @@ pmap_bootstrap()
 	 * size and aligned to a natural boundary).
 	 */
 	vhpt_size = 15;
-	while ((1<<vhpt_size) < Maxmem * 32)
+	size = 1UL << vhpt_size;
+	while (size < Maxmem * 32) {
 		vhpt_size++;
+		size <<= 1;
+	}
 
-	vhpt_base = 0;
-	while (!vhpt_base) {
-		vm_offset_t mask;
+	vhpt_base[0] = 0;
+	base = limit = 0;
+	while (vhpt_base[0] == 0) {
 		if (bootverbose)
-			printf("Trying VHPT size 0x%lx\n", (1L<<vhpt_size));
-		mask = (1L << vhpt_size) - 1;
+			printf("Trying VHPT size 0x%lx\n", size);
 		for (i = 0; i < count; i += 2) {
-			vm_offset_t base, limit;
-			base = (phys_avail[i] + mask) & ~mask;
-			limit = base + (1L << vhpt_size);
+			base = (phys_avail[i] + size - 1) & ~(size - 1);
+			limit = base + MAXCPU * size;
 			if (limit <= phys_avail[i+1])
 				/*
 				 * VHPT can fit in this region
@@ -365,46 +376,53 @@ pmap_bootstrap()
 				break;
 		}
 		if (!phys_avail[i]) {
-			/*
-			 * Can't fit, try next smaller size.
-			 */
+			/* Can't fit, try next smaller size. */
 			vhpt_size--;
-		} else {
-			vhpt_base = (phys_avail[i] + mask) & ~mask;
-		}
+			size >>= 1;
+		} else
+			vhpt_base[0] = IA64_PHYS_TO_RR7(base);
 	}
 	if (vhpt_size < 15)
 		panic("Can't find space for VHPT");
 
 	if (bootverbose)
-		printf("Putting VHPT at %p\n", (void *) vhpt_base);
-	if (vhpt_base != phys_avail[i]) {
-		/*
-		 * Split this region.
-		 */
+		printf("Putting VHPT at 0x%lx\n", base);
+
+	if (base != phys_avail[i]) {
+		/* Split this region. */
 		if (bootverbose)
-			printf("Splitting [%p-%p]\n",
-			       (void *) phys_avail[i],
-			       (void *) phys_avail[i+1]);
+			printf("Splitting [%p-%p]\n", (void *)phys_avail[i],
+			    (void *)phys_avail[i+1]);
 		for (j = count; j > i; j -= 2) {
 			phys_avail[j] = phys_avail[j-2];
 			phys_avail[j+1] = phys_avail[j-2+1];
 		}
-		phys_avail[count+2] = 0;
-		phys_avail[count+3] = 0;
-		phys_avail[i+1] = vhpt_base;
-		phys_avail[i+2] = vhpt_base + (1L << vhpt_size);
-	} else {
-		phys_avail[i] = vhpt_base + (1L << vhpt_size);
-	}
+		phys_avail[i+1] = base;
+		phys_avail[i+2] = limit;
+	} else
+		phys_avail[i] = limit;
 
-	vhpt_base = IA64_PHYS_TO_RR7(vhpt_base);
-	bzero((void *) vhpt_base, (1L << vhpt_size));
+	count = size / sizeof(struct ia64_lpte);
 
-	mtx_init(&pmap_vhptmutex, "VHPT collision chain lock", NULL, MTX_DEF);
+	vhpt_bucket = (void *)pmap_steal_memory(count * sizeof(struct ia64_bucket));
+	pte = (struct ia64_lpte *)vhpt_base[0];
+	for (i = 0; i < count; i++) {
+		pte[i].pte = 0;
+		pte[i].itir = 0;
+		pte[i].tag = 1UL << 63;	/* Invalid tag */
+		pte[i].chain = (uintptr_t)(vhpt_bucket + i);
+		/* Stolen memory is zeroed! */
+		mtx_init(&vhpt_bucket[i].mutex, "VHPT bucket lock", NULL,
+		    MTX_SPIN);
+	}
+
+	for (i = 1; i < MAXCPU; i++) {
+		vhpt_base[i] = vhpt_base[i - 1] + size;
+		bcopy((void *)vhpt_base[i - 1], (void *)vhpt_base[i], size);
+	}
 
-	__asm __volatile("mov cr.pta=%0;; srlz.i;;"
-			 :: "r" (vhpt_base + (1<<8) + (vhpt_size<<2) + 1));
+	__asm __volatile("mov cr.pta=%0;; srlz.i;;" ::
+	    "r" (vhpt_base[0] + (1<<8) + (vhpt_size<<2) + 1));
 
 	virtual_avail = VM_MIN_KERNEL_ADDRESS;
 	virtual_end = VM_MAX_KERNEL_ADDRESS;
@@ -494,12 +512,73 @@ pmap_init2()
  * Manipulate TLBs for a pmap
  ***************************************************/
 
+#if 0
+static __inline void
+pmap_invalidate_page_locally(void *arg)
+{
+	vm_offset_t va = (uintptr_t)arg;
+	struct ia64_lpte *pte;
+
+	pte = (struct ia64_lpte *)ia64_thash(va);
+	if (pte->tag == ia64_ttag(va))
+		pte->tag = 1UL << 63;
+	ia64_ptc_l(va, PAGE_SHIFT << 2);
+}
+
+#ifdef SMP
+static void
+pmap_invalidate_page_1(void *arg)
+{
+	void **args = arg;
+	pmap_t oldpmap;
+
+	critical_enter();
+	oldpmap = pmap_install(args[0]);
+	pmap_invalidate_page_locally(args[1]);
+	pmap_install(oldpmap);
+	critical_exit();
+}
+#endif
+
 static void
 pmap_invalidate_page(pmap_t pmap, vm_offset_t va)
 {
+
 	KASSERT((pmap == kernel_pmap || pmap == PCPU_GET(current_pmap)),
 		("invalidating TLB for non-current pmap"));
-	ia64_ptc_g(va, PAGE_SHIFT << 2);
+
+#ifdef SMP
+	if (mp_ncpus > 1) {
+		void *args[2];
+		args[0] = pmap;
+		args[1] = (void *)va;
+		smp_rendezvous(NULL, pmap_invalidate_page_1, NULL, args);
+	} else
+#endif
+	pmap_invalidate_page_locally((void *)va);
+}
+#endif /* 0 */
+
+static void
+pmap_invalidate_page(pmap_t pmap, vm_offset_t va)
+{
+	struct ia64_lpte *pte;
+	int i, vhpt_ofs;
+
+	KASSERT((pmap == kernel_pmap || pmap == PCPU_GET(current_pmap)),
+		("invalidating TLB for non-current pmap"));
+
+	vhpt_ofs = ia64_thash(va) - vhpt_base[PCPU_GET(cpuid)];
+	critical_enter();
+	for (i = 0; i < MAXCPU; i++) {
+		pte = (struct ia64_lpte *)(vhpt_base[i] + vhpt_ofs);
+		if (pte->tag == ia64_ttag(va))
+			pte->tag = 1UL << 63;
+	}
+	critical_exit();
+	mtx_lock_spin(&pmap_ptcmutex);
+	ia64_ptc_ga(va, PAGE_SHIFT << 2);
+	mtx_unlock_spin(&pmap_ptcmutex);
 }
 
 static void
@@ -507,9 +586,8 @@ pmap_invalidate_all_1(void *arg)
 {
 	uint64_t addr;
 	int i, j;
-	register_t psr;
 
-	psr = intr_disable();
+	critical_enter();
 	addr = pmap_ptc_e_base;
 	for (i = 0; i < pmap_ptc_e_count1; i++) {
 		for (j = 0; j < pmap_ptc_e_count2; j++) {
@@ -518,21 +596,22 @@ pmap_invalidate_all_1(void *arg)
 		}
 		addr += pmap_ptc_e_stride1;
 	}
-	intr_restore(psr);
+	critical_exit();
 }
 
 static void
 pmap_invalidate_all(pmap_t pmap)
 {
+
 	KASSERT((pmap == kernel_pmap || pmap == PCPU_GET(current_pmap)),
 		("invalidating TLB for non-current pmap"));
 
-
 #ifdef SMP
-	smp_rendezvous(0, pmap_invalidate_all_1, 0, 0);
-#else
-	pmap_invalidate_all_1(0);
+	if (mp_ncpus > 1)
+		smp_rendezvous(NULL, pmap_invalidate_all_1, NULL, NULL);
+	else
 #endif
+	pmap_invalidate_all_1(NULL);
 }
 
 static uint32_t
@@ -582,47 +661,6 @@ pmap_free_rid(uint32_t rid)
 	mtx_unlock(&pmap_ridmutex);
 }
 
-/***************************************************
- * Low level helper routines.....
- ***************************************************/
-
-/*
- * Install a pte into the VHPT
- */
-static PMAP_INLINE void
-pmap_install_pte(struct ia64_lpte *vhpte, struct ia64_lpte *pte)
-{
-	uint64_t *vhp, *p;
-
-	vhp = (uint64_t *)vhpte;
-	p = (uint64_t *)pte;
-
-	critical_enter();
-
-	/* Invalidate the tag so the VHPT walker will not match this entry. */
-	vhp[2] = 1UL << 63;
-	ia64_mf();
-
-	vhp[0] = p[0];
-	vhp[1] = p[1];
-	ia64_mf();
-
-	/* Install a proper tag now that we're done. */
-	vhp[2] = p[2];
-	ia64_mf();
-
-	critical_exit();
-}
-
-/*
- * Compare essential parts of pte.
- */
-static PMAP_INLINE int
-pmap_equal_pte(struct ia64_lpte *pte1, struct ia64_lpte *pte2)
-{
-	return *(uint64_t *) pte1 == *(uint64_t *) pte2;
-}
-
 /*
  * this routine defines the region(s) of memory that should
  * not be tested for the modified bit.
@@ -759,50 +797,23 @@ get_pv_entry(void)
 static void
 pmap_enter_vhpt(struct ia64_lpte *pte, vm_offset_t va)
 {
+	struct ia64_bucket *bckt;
 	struct ia64_lpte *vhpte;
 
 	pmap_vhpt_inserts++;
 	pmap_vhpt_resident++;
 
-	vhpte = (struct ia64_lpte *) ia64_thash(va);
-
-	if (vhpte->chain)
-		pmap_vhpt_collisions++;
-
-	mtx_lock(&pmap_vhptmutex);
+	vhpte = (struct ia64_lpte *)ia64_thash(va);
+	bckt = (struct ia64_bucket *)vhpte->chain;
 
-	pte->chain = vhpte->chain;
+	mtx_lock_spin(&bckt->mutex);
+	pte->chain = bckt->chain;
 	ia64_mf();
-	vhpte->chain = ia64_tpa((vm_offset_t)pte);
+	bckt->chain = ia64_tpa((vm_offset_t)pte);
 	ia64_mf();
 
-	if (!pmap_lpte_present(vhpte) && pmap_lpte_present(pte)) {
-		ia64_ptc_g(va, PAGE_SHIFT << 2);
-		pmap_install_pte(vhpte, pte);
-	}
-
-	mtx_unlock(&pmap_vhptmutex);
-}
-
-/*
- * Update VHPT after a pte has changed.
- */
-static void
-pmap_update_vhpt(struct ia64_lpte *pte, vm_offset_t va)
-{
-	struct ia64_lpte *vhpte;
-
-	vhpte = (struct ia64_lpte *)ia64_thash(va);
-
-	mtx_lock(&pmap_vhptmutex);
-
-	if ((!pmap_lpte_present(vhpte) || vhpte->tag == pte->tag) &&
-	    pmap_lpte_present(pte)) {
-		ia64_ptc_g(va, PAGE_SHIFT << 2);
-		pmap_install_pte(vhpte, pte);
-	}
-
-	mtx_unlock(&pmap_vhptmutex);
+	bckt->length++;
+	mtx_unlock_spin(&bckt->mutex);
 }
 
 /*
@@ -812,55 +823,37 @@ pmap_update_vhpt(struct ia64_lpte *pte, vm_offset_t va)
 static int
 pmap_remove_vhpt(vm_offset_t va)
 {
+	struct ia64_bucket *bckt;
 	struct ia64_lpte *pte;
 	struct ia64_lpte *lpte;
 	struct ia64_lpte *vhpte;
 	uint64_t tag;
 
-	vhpte = (struct ia64_lpte *)ia64_thash(va);
-
-	/*
-	 * If the VHPTE is invalid, there can't be a collision chain.
-	 */
-	if (!pmap_lpte_present(vhpte)) {
-		KASSERT(!vhpte->chain, ("bad vhpte"));
-		return (ENOENT);
-	}
-
-	lpte = vhpte;
 	tag = ia64_ttag(va);
+	vhpte = (struct ia64_lpte *)ia64_thash(va);
+	bckt = (struct ia64_bucket *)vhpte->chain;
 
-	mtx_lock(&pmap_vhptmutex);
-
-	pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(vhpte->chain);
-	KASSERT(pte != NULL, ("foo"));
-
-	while (pte->tag != tag) {
+	mtx_lock_spin(&bckt->mutex);
+	lpte = NULL;
+	pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(bckt->chain);
+	while (pte != NULL && pte->tag != tag) {
 		lpte = pte;
-		if (pte->chain == 0) {
-			mtx_unlock(&pmap_vhptmutex);
-			return (ENOENT);
-		}
 		pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(pte->chain);
 	}
+	if (pte == NULL) {
+		mtx_unlock_spin(&bckt->mutex);
+		return (ENOENT);
+	}
 
 	/* Snip this pv_entry out of the collision chain. */
-	lpte->chain = pte->chain;
+	if (lpte == NULL)
+		bckt->chain = pte->chain;
+	else
+		lpte->chain = pte->chain;
 	ia64_mf();
 
-	/*
-	 * If the VHPTE matches as well, change it to map the first
-	 * element from the chain if there is one.
-	 */
-	if (vhpte->tag == tag) {
-		if (vhpte->chain) {
-			pte = (void*)IA64_PHYS_TO_RR7(vhpte->chain);
-			pmap_install_pte(vhpte, pte);
-		} else
-			pmap_clear_present(vhpte);
-	}
-
-	mtx_unlock(&pmap_vhptmutex);
+	bckt->length--;
+	mtx_unlock_spin(&bckt->mutex);
 	pmap_vhpt_resident--;
 	return (0);
 }
@@ -871,18 +864,21 @@ pmap_remove_vhpt(vm_offset_t va)
 static struct ia64_lpte *
 pmap_find_vhpt(vm_offset_t va)
 {
+	struct ia64_bucket *bckt;
 	struct ia64_lpte *pte;
 	uint64_t tag;
 
 	tag = ia64_ttag(va);
 	pte = (struct ia64_lpte *)ia64_thash(va);
-	if (pte->chain == 0)
+	bckt = (struct ia64_bucket *)pte->chain;
+	if (bckt->chain == 0)
 		return (NULL);
-	pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(pte->chain);
+
+	pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(bckt->chain);
 	while (pte->tag != tag) {
 		if (pte->chain == 0)
 			return (NULL);
-		pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(pte->chain );
+		pte = (struct ia64_lpte *)IA64_PHYS_TO_RR7(pte->chain);
 	}
 	return (pte);
 }
@@ -1028,12 +1024,9 @@ pmap_find_pte(vm_offset_t va)
 		return pmap_find_kpte(va);
 
 	pte = pmap_find_vhpt(va);
-	if (!pte) {
-		pte = uma_zalloc(ptezone, M_NOWAIT);
-		if (pte != NULL)
-			pmap_clear_present(pte);
-	}
-	return pte;
+	if (!pte)
+		pte = uma_zalloc(ptezone, M_NOWAIT | M_ZERO);
+	return (pte);
 }
 
 /*
@@ -1078,7 +1071,6 @@ static void
 pmap_set_pte(struct ia64_lpte *pte, vm_offset_t va, vm_offset_t pa,
     boolean_t wired, boolean_t managed)
 {
-	int wasvalid = pmap_lpte_present(pte);
 
 	pte->pte &= PTE_PROT_MASK | PTE_PL_MASK | PTE_AR_MASK;
 	pte->pte |= PTE_PRESENT | PTE_MA_WB;
@@ -1089,25 +1081,6 @@ pmap_set_pte(struct ia64_lpte *pte, vm_offset_t va, vm_offset_t pa,
 	pte->itir = PAGE_SHIFT << 2;
 
 	pte->tag = ia64_ttag(va);
-
-	if (wasvalid) {
-		pmap_update_vhpt(pte, va);
-	} else {
-		pmap_enter_vhpt(pte, va);
-	}
-}
-
-/*
- * If a pte contains a valid mapping, clear it and update the VHPT.
- */
-static void
-pmap_clear_pte(struct ia64_lpte *pte, vm_offset_t va)
-{
-	if (pmap_lpte_present(pte)) {
-		pmap_remove_vhpt(va);
-		ia64_ptc_g(va, PAGE_SHIFT << 2);
-		pmap_clear_present(pte);
-	}
 }
 
 /*
@@ -1129,12 +1102,10 @@ pmap_remove_pte(pmap_t pmap, struct ia64_lpte *pte, vm_offset_t va,
 	 */
 	error = pmap_remove_vhpt(va);
 	if (error)
-		return error;
+		return (error);
 
-	/*
-	 * Make sure pmap_set_pte() knows it isn't in the VHPT.
-	 */
-	pmap_clear_present(pte);
+	if (freepte)
+		pmap_invalidate_page(pmap, va);
 
 	if (pmap_lpte_wired(pte))
 		pmap->pm_stats.wired_count -= 1;
@@ -1150,12 +1121,13 @@ pmap_remove_pte(pmap_t pmap, struct ia64_lpte *pte, vm_offset_t va,
 
 		if (freepte)
 			pmap_free_pte(pte, va);
-		return pmap_remove_entry(pmap, m, va, pv);
+		error = pmap_remove_entry(pmap, m, va, pv);
 	} else {
 		if (freepte)
 			pmap_free_pte(pte, va);
-		return 0;
 	}
+
+	return (error);
 }
 
 /*
@@ -1199,18 +1171,18 @@ pmap_kextract(vm_offset_t va)
 void
 pmap_qenter(vm_offset_t va, vm_page_t *m, int count)
 {
-	int i;
 	struct ia64_lpte *pte;
+	int i;
 
 	for (i = 0; i < count; i++) {
-		vm_offset_t tva = va + i * PAGE_SIZE;
-		int wasvalid;
-		pte = pmap_find_kpte(tva);
-		wasvalid = pmap_lpte_present(pte);
+		pte = pmap_find_kpte(va);
+		if (pmap_lpte_present(pte))
+			pmap_invalidate_page(kernel_pmap, va);
+		else
+			pmap_enter_vhpt(pte, va);
 		pmap_pte_prot(kernel_pmap, pte, VM_PROT_ALL);
-		pmap_set_pte(pte, tva, VM_PAGE_TO_PHYS(m[i]), FALSE, FALSE);
-		if (wasvalid)
-			ia64_ptc_g(tva, PAGE_SHIFT << 2);
+		pmap_set_pte(pte, va, VM_PAGE_TO_PHYS(m[i]), FALSE, FALSE);
+		va += PAGE_SIZE;
 	}
 }
 
@@ -1221,12 +1193,16 @@ pmap_qenter(vm_offset_t va, vm_page_t *m, int count)
 void
 pmap_qremove(vm_offset_t va, int count)
 {
-	int i;
 	struct ia64_lpte *pte;
+	int i;
 
 	for (i = 0; i < count; i++) {
 		pte = pmap_find_kpte(va);
-		pmap_clear_pte(pte, va);
+		if (pmap_lpte_present(pte)) {
+			pmap_remove_vhpt(va);
+			pmap_invalidate_page(kernel_pmap, va);
+			pmap_clear_present(pte);
+		}
 		va += PAGE_SIZE;
 	}
 }
@@ -1239,14 +1215,14 @@ void
 pmap_kenter(vm_offset_t va, vm_offset_t pa)
 {
 	struct ia64_lpte *pte;
-	int wasvalid;
 
 	pte = pmap_find_kpte(va);
-	wasvalid = pmap_lpte_present(pte);
+	if (pmap_lpte_present(pte))
+		pmap_invalidate_page(kernel_pmap, va);
+	else
+		pmap_enter_vhpt(pte, va);
 	pmap_pte_prot(kernel_pmap, pte, VM_PROT_ALL);
 	pmap_set_pte(pte, va, pa, FALSE, FALSE);
-	if (wasvalid)
-		ia64_ptc_g(va, PAGE_SHIFT << 2);
 }
 
 /*
@@ -1258,7 +1234,11 @@ pmap_kremove(vm_offset_t va)
 	struct ia64_lpte *pte;
 
 	pte = pmap_find_kpte(va);
-	pmap_clear_pte(pte, va);
+	if (pmap_lpte_present(pte)) {
+		pmap_remove_vhpt(va);
+		pmap_invalidate_page(kernel_pmap, va);
+		pmap_clear_present(pte);
+	}
 }
 
 /*
@@ -1291,10 +1271,8 @@ pmap_remove_page(pmap_t pmap, vm_offset_t va)
 		("removing page for non-current pmap"));
 
 	pte = pmap_find_vhpt(va);
-	if (pte) {
+	if (pte)
 		pmap_remove_pte(pmap, pte, va, 0, 1);
-		pmap_invalidate_page(pmap, va);
-	}
 	return;
 }
 
@@ -1336,17 +1314,14 @@ pmap_remove(pmap_t pmap, vm_offset_t sva, vm_offset_t eva)
 				pte = pmap_find_vhpt(va);
 				KASSERT(pte != NULL, ("pte"));
 				pmap_remove_pte(pmap, pte, va, pv, 1);
-				pmap_invalidate_page(pmap, va);
 			}
 		}
 		
 	} else {
 		for (va = sva; va < eva; va = va += PAGE_SIZE) {
 			pte = pmap_find_vhpt(va);
-			if (pte) {
+			if (pte)
 				pmap_remove_pte(pmap, pte, va, 0, 1);
-				pmap_invalidate_page(pmap, va);
-			}
 		}
 	}
 out:
@@ -1399,7 +1374,6 @@ pmap_remove_all(vm_page_t m)
 		if (pmap_lpte_ppn(pte) != VM_PAGE_TO_PHYS(m))
 			panic("pmap_remove_all: pv_table for %lx is inconsistent", VM_PAGE_TO_PHYS(m));
 		pmap_remove_pte(pmap, pte, va, pv, 1);
-		pmap_invalidate_page(pmap, va);
 		pmap_install(oldpmap);
 		PMAP_UNLOCK(pmap);
 	}
@@ -1459,7 +1433,6 @@ pmap_protect(pmap_t pmap, vm_offset_t sva, vm_offset_t eva, vm_prot_t prot)
 				}
 			}
 			pmap_pte_prot(pmap, pte, prot);
-			pmap_update_vhpt(pte, sva);
 			pmap_invalidate_page(pmap, sva);
 		}
 
@@ -1516,14 +1489,18 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 		oldpmap = pmap_install(pmap);
 	}
 	origpte = *pte;
-	opa = (pmap_lpte_present(&origpte)) ? pmap_lpte_ppn(&origpte) : 0UL;
+	if (!pmap_lpte_present(pte)) {
+		opa = ~0UL;
+		pmap_enter_vhpt(pte, va);
+	} else
+		opa = pmap_lpte_ppn(pte);
 	managed = FALSE;
 	pa = VM_PAGE_TO_PHYS(m);
 
 	/*
 	 * Mapping has not changed, must be protection or wiring change.
 	 */
-	if (pmap_lpte_present(&origpte) && (opa == pa)) {
+	if (opa == pa) {
 		/*
 		 * Wiring change, just update stats. We don't worry about
 		 * wiring PT pages as they remain resident as long as there
@@ -1545,17 +1522,17 @@ pmap_enter(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_prot_t prot,
 		    pmap_track_modified(va))
 			vm_page_dirty(m);
 
+		pmap_invalidate_page(pmap, va);
 		goto validate;
 	}
+
 	/*
 	 * Mapping has changed, invalidate old range and fall
 	 * through to handle validating new mapping.
 	 */
-	if (opa) {
-		int error;
-		error = pmap_remove_pte(pmap, pte, va, 0, 0);
-		if (error)
-			panic("pmap_enter: pte vanished, va: 0x%lx", va);
+	if (opa != ~0UL) {
+		pmap_remove_pte(pmap, pte, va, 0, 0);
+		pmap_enter_vhpt(pte, va);
 	}
 
 	/*
@@ -1582,13 +1559,6 @@ validate:
 	pmap_pte_prot(pmap, pte, prot);
 	pmap_set_pte(pte, va, pa, wired, managed);
 
-	/*
-	 * if the mapping or permission bits are different, we need
-	 * to invalidate the page.
-	 */
-	if (!pmap_equal_pte(&origpte, pte))
-		pmap_invalidate_page(pmap, va);
-
 	vm_page_unlock_queues();
 	pmap_install(oldpmap);
 	PMAP_UNLOCK(pmap);
@@ -1629,30 +1599,24 @@ pmap_enter_quick(pmap_t pmap, vm_offset_t va, vm_page_t m, vm_page_t mpte)
 		PMAP_LOCK(pmap);
 		oldpmap = pmap_install(pmap);
 	}
-	if (pmap_lpte_present(pte))
-		goto reinstall;
-	managed = FALSE;
 
-	/*
-	 * Enter on the PV list since its part of our managed memory.
-	 */
-	if ((m->flags & (PG_FICTITIOUS | PG_UNMANAGED)) == 0) {
-		pmap_insert_entry(pmap, va, m);
-		managed = TRUE;
-	}
+	if (!pmap_lpte_present(pte)) {
+		/* Enter on the PV list if its managed. */
+		if ((m->flags & (PG_FICTITIOUS | PG_UNMANAGED)) == 0) {
+			pmap_insert_entry(pmap, va, m);
+			managed = TRUE;
+		} else
+			managed = FALSE;
 
-	/*
-	 * Increment counters
-	 */
-	pmap->pm_stats.resident_count++;
+		/* Increment counters. */
+		pmap->pm_stats.resident_count++;
 
-	/*
-	 * Initialise PTE with read-only protection and enter into VHPT.
-	 */
-	pmap_pte_prot(pmap, pte, VM_PROT_READ);
-	pmap_set_pte(pte, va, VM_PAGE_TO_PHYS(m), FALSE, managed);
+		/* Initialise with R/O protection and enter into VHPT. */
+		pmap_enter_vhpt(pte, va);
+		pmap_pte_prot(pmap, pte, VM_PROT_READ);
+		pmap_set_pte(pte, va, VM_PAGE_TO_PHYS(m), FALSE, managed);
+	}
 
-reinstall:
 	pmap_install(oldpmap);
 	PMAP_UNLOCK(pmap);
 	return (NULL);
@@ -1817,7 +1781,6 @@ pmap_page_exists_quick(pmap_t pmap, vm_page_t m)
 	return (FALSE);
 }
 
-#define PMAP_REMOVE_PAGES_CURPROC_ONLY
 /*
  * Remove all pages from specified address space
  * this aids process exit speeds.  Also, this code
@@ -1829,37 +1792,33 @@ pmap_page_exists_quick(pmap_t pmap, vm_page_t m)
 void
 pmap_remove_pages(pmap_t pmap, vm_offset_t sva, vm_offset_t eva)
 {
+	pmap_t oldpmap;
 	pv_entry_t pv, npv;
 
-#ifdef PMAP_REMOVE_PAGES_CURPROC_ONLY
 	if (pmap != vmspace_pmap(curthread->td_proc->p_vmspace)) {
 		printf("warning: pmap_remove_pages called with non-current pmap\n");
 		return;
 	}
-#endif
 
 	vm_page_lock_queues();
 	PMAP_LOCK(pmap);
-	for (pv = TAILQ_FIRST(&pmap->pm_pvlist);
-		pv;
-		pv = npv) {
+	oldpmap = pmap_install(pmap);
+
+	for (pv = TAILQ_FIRST(&pmap->pm_pvlist); pv; pv = npv) {
 		struct ia64_lpte *pte;
 
 		npv = TAILQ_NEXT(pv, pv_plist);
 
-		if (pv->pv_va >= eva || pv->pv_va < sva) {
+		if (pv->pv_va >= eva || pv->pv_va < sva)
 			continue;
-		}
 
 		pte = pmap_find_vhpt(pv->pv_va);
 		KASSERT(pte != NULL, ("pte"));
-		if (pmap_lpte_wired(pte))
-			continue;
-
-		pmap_remove_pte(pmap, pte, pv->pv_va, pv, 1);
+		if (!pmap_lpte_wired(pte))
+			pmap_remove_pte(pmap, pte, pv->pv_va, pv, 1);
 	}
 
-	pmap_invalidate_all(pmap);
+	pmap_install(oldpmap);
 	PMAP_UNLOCK(pmap);
 	vm_page_unlock_queues();
 }
@@ -1888,7 +1847,6 @@ pmap_page_protect(vm_page_t m, vm_prot_t prot)
 			pte = pmap_find_vhpt(pv->pv_va);
 			KASSERT(pte != NULL, ("pte"));
 			pmap_pte_prot(pmap, pte, prot);
-			pmap_update_vhpt(pte, pv->pv_va);
 			pmap_invalidate_page(pmap, pv->pv_va);
 			pmap_install(oldpmap);
 			PMAP_UNLOCK(pmap);
@@ -1930,7 +1888,6 @@ pmap_ts_referenced(vm_page_t m)
 		if (pmap_lpte_accessed(pte)) {
 			count++;
 			pmap_clear_accessed(pte);
-			pmap_update_vhpt(pte, pv->pv_va);
 			pmap_invalidate_page(pv->pv_pmap, pv->pv_va);
 		}
 		pmap_install(oldpmap);
@@ -2038,7 +1995,6 @@ pmap_clear_modify(vm_page_t m)
 		KASSERT(pte != NULL, ("pte"));
 		if (pmap_lpte_dirty(pte)) {
 			pmap_clear_dirty(pte);
-			pmap_update_vhpt(pte, pv->pv_va);
 			pmap_invalidate_page(pv->pv_pmap, pv->pv_va);
 		}
 		pmap_install(oldpmap);
@@ -2068,7 +2024,6 @@ pmap_clear_reference(vm_page_t m)
 		KASSERT(pte != NULL, ("pte"));
 		if (pmap_lpte_accessed(pte)) {
 			pmap_clear_accessed(pte);
-			pmap_update_vhpt(pte, pv->pv_va);
 			pmap_invalidate_page(pv->pv_pmap, pv->pv_va);
 		}
 		pmap_install(oldpmap);
diff --git a/sys/ia64/ia64/trap.c b/sys/ia64/ia64/trap.c
index 86dc147..fbb8e13 100644
--- a/sys/ia64/ia64/trap.c
+++ b/sys/ia64/ia64/trap.c
@@ -40,6 +40,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/exec.h>
 #include <sys/lock.h>
 #include <sys/mutex.h>
+#include <sys/sched.h>
 #include <sys/smp.h>
 #include <sys/vmmeter.h>
 #include <sys/sysent.h>
@@ -629,7 +630,7 @@ trap(int vector, struct trapframe *tf)
 		if (!user)
 			trap_panic(vector, tf);
 
-		critical_enter();
+		sched_pin();
 		thr = PCPU_GET(fpcurthread);
 		if (thr == td) {
 			/*
@@ -643,26 +644,29 @@ trap(int vector, struct trapframe *tf)
 			 */
 			printf("XXX: bogusly disabled high FP regs\n");
 			tf->tf_special.psr &= ~IA64_PSR_DFH;
-			critical_exit();
+			sched_unpin();
 			goto out;
 		} else if (thr != NULL) {
+			mtx_lock(&thr->td_md.md_highfp_mtx);
 			pcb = thr->td_pcb;
 			save_high_fp(&pcb->pcb_high_fp);
 			pcb->pcb_fpcpu = NULL;
 			PCPU_SET(fpcurthread, NULL);
+			mtx_unlock(&thr->td_md.md_highfp_mtx);
 			thr = NULL;
 		}
 
+		mtx_lock(&td->td_md.md_highfp_mtx);
 		pcb = td->td_pcb;
 		pcpu = pcb->pcb_fpcpu;
 
 #ifdef SMP
 		if (pcpu != NULL) {
-			ipi_send(pcpu->pc_lid, IPI_HIGH_FP);
-			critical_exit();
-			while (pcb->pcb_fpcpu != pcpu)
+			mtx_unlock(&td->td_md.md_highfp_mtx);
+			ipi_send(pcpu, IPI_HIGH_FP);
+			while (pcb->pcb_fpcpu == pcpu)
 				DELAY(100);
-			critical_enter();
+			mtx_lock(&td->td_md.md_highfp_mtx);
 			pcpu = pcb->pcb_fpcpu;
 			thr = PCPU_GET(fpcurthread);
 		}
@@ -676,7 +680,8 @@ trap(int vector, struct trapframe *tf)
 			tf->tf_special.psr &= ~IA64_PSR_DFH;
 		}
 
-		critical_exit();
+		mtx_unlock(&td->td_md.md_highfp_mtx);
+		sched_unpin();
 		goto out;
 	}
 
diff --git a/sys/ia64/ia64/vm_machdep.c b/sys/ia64/ia64/vm_machdep.c
index 685aece..8ce6641 100644
--- a/sys/ia64/ia64/vm_machdep.c
+++ b/sys/ia64/ia64/vm_machdep.c
@@ -118,6 +118,7 @@ cpu_thread_setup(struct thread *td)
 	sp -= sizeof(struct trapframe);
 	td->td_frame = (struct trapframe *)sp;
 	td->td_frame->tf_length = sizeof(struct trapframe);
+	mtx_init(&td->td_md.md_highfp_mtx, "High FP lock", NULL, MTX_DEF);
 }
 
 void
diff --git a/sys/ia64/include/proc.h b/sys/ia64/include/proc.h
index 6c8a1e1..5cbc0bf 100644
--- a/sys/ia64/include/proc.h
+++ b/sys/ia64/include/proc.h
@@ -30,8 +30,9 @@
 #define	_MACHINE_PROC_H_
 
 struct mdthread {
+	struct mtx md_highfp_mtx;
 	int	md_spinlock_count;	/* (k) */
-	register_t md_saved_intr;	/* (k) */
+	int	md_saved_intr;		/* (k) */
 };
 
 struct mdproc {
diff --git a/sys/ia64/include/smp.h b/sys/ia64/include/smp.h
index d2f8e5f..cf1fab3 100644
--- a/sys/ia64/include/smp.h
+++ b/sys/ia64/include/smp.h
@@ -28,13 +28,15 @@
 
 #ifndef LOCORE
 
+struct pcpu;
+
 extern int ipi_vector[];
 
 void	ipi_all(int ipi);
 void	ipi_all_but_self(int ipi);
-void	ipi_selected(u_int64_t cpus, int ipi);
+void	ipi_selected(cpumask_t cpus, int ipi);
 void	ipi_self(int ipi);
-void	ipi_send(u_int64_t lid, int ipi);
+void	ipi_send(struct pcpu *, int ipi);
 
 #endif /* !LOCORE */
 #endif /* _KERNEL */