Clean up the trap handling code and make it consistent with the other platforms.

Submitted by:	jhb

7 files changed, 895 insertions, 613 deletions

diff --git a/sys/powerpc/aim/locore.S b/sys/powerpc/aim/locore.S
index 9deb42f..24571b6 100644
--- a/sys/powerpc/aim/locore.S
+++ b/sys/powerpc/aim/locore.S
@@ -927,35 +927,16 @@ s_trap:
 	FRAME_SETUP(tempsave)
 /* Now we can recover interrupts again: */
 	mfmsr	7
-	ori	7,7,(PSL_EE|PSL_ME|PSL_RI)@l
+	ori	7,7,(PSL_EE|PSL_FP|PSL_ME|PSL_RI)@l
 	mtmsr	7
 	isync
 /* Call C trap code: */
-trapagain:
 	addi	3,1,8
+	mr	30,3
 	bl	trap
-trapexit:
-/* Disable interrupts: */
-	mfmsr	3
-	andi.	3,3,~PSL_EE@l
-	mtmsr	3
-/* Test AST pending: */
-	lwz	5,FRAME_SRR1+8(1)
-	mtcr	5
-	bc	4,17,1f			/* branch if PSL_PR is false */
-	lis	3,astpending@ha
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
-#if 0 /* XXX */
-	li	6,EXC_AST
-#endif
-	stw	6,FRAME_EXC+8(1)
-	b	trapagain
-1:
-#if 0
+	mr	3,30
+	bl	ast
 	FRAME_LEAVE(tempsave)
-#endif
 	rfi
 
 /*
@@ -971,7 +952,10 @@ fork_trampoline:
 	mtlr	31
 	mr	3,30
 	blrl				/* jump indirect to r31 */
-	b	trapexit
+	mr	3,30
+	bl	ast
+	FRAME_LEAVE(tempsave)
+	rfi
 
 /*
  * DSI second stage fault handler
@@ -1129,10 +1113,6 @@ intr_exit:
 	lwz	3,GD_CURPCB(3)		/* get curpcb from globaldata */
 	lwz	3,PCB_PMR(3)		/* get pmap real address from curpcb */
 	mtsr	KERNEL_SR,3
-	lis	3,astpending@ha		/* Test AST pending */
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
 /* Setup for entry to realtrap: */
 	lwz	3,0(1)			/* get saved SP */
 	mtsprg	1,3
@@ -1152,7 +1132,7 @@ intr_exit:
 	lwz	31,intr_depth@l(30)
 	addi	31,31,-1
 	stw	31,intr_depth@l(30)
-	b	realtrap
+	b	realtrap		/* XXX:	should call ast(frame ptr) */
 1:
 /* Here is the normal exit of extintr: */
 	lwz	5,36(1)
diff --git a/sys/powerpc/aim/trap.c b/sys/powerpc/aim/trap.c
index 0a48fe8..e76bc88 100644
--- a/sys/powerpc/aim/trap.c
+++ b/sys/powerpc/aim/trap.c
@@ -41,16 +41,27 @@ static const char rcsid[] =
 
 #include <sys/param.h>
 #include <sys/proc.h>
+#include <sys/ktr.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/pioctl.h>
 #include <sys/reboot.h>
 #include <sys/syscall.h>
+#include <sys/sysent.h>
 #include <sys/systm.h>
 #include <sys/uio.h>
 #include <sys/user.h>
+#ifdef KTRACE
 #include <sys/ktrace.h>
+#endif
+#include <sys/vmmeter.h>
 
 #include <vm/vm.h>
-#include <vm/vm_kern.h>
 #include <vm/pmap.h>
+#include <vm/vm_extern.h>
+#include <vm/vm_kern.h>
+#include <vm/vm_map.h>
+#include <vm/vm_param.h>
 
 #include <machine/cpu.h>
 #include <machine/frame.h>
@@ -63,306 +74,466 @@ static const char rcsid[] =
 #define	NARGREG		8		/* 8 args are in registers */
 #define	MOREARGS(sp)	((caddr_t)((int)(sp) + 8)) /* more args go here */
 
-volatile int	astpending;
-volatile int	want_resched;
+#ifdef WITNESS
+extern char *syscallnames[];
+#endif
 
 #if 0 /* XXX: not used yet */
 static int fix_unaligned __P((struct proc *p, struct trapframe *frame));
 #endif
+static void trap_fatal __P((struct trapframe *frame));
+static void printtrap __P((int vector, struct trapframe *frame, int isfatal,
+			      int user));
+static int trap_pfault __P((struct trapframe *frame, int user));
+static int handle_onfault (struct trapframe *frame);
+
+static const char *ppc_exception_names[] = {
+	"Reserved 0",				/* 0 */
+	"Reset",				/* 1 */
+	"Machine Check",			/* 2 */
+	"Data Storage Interrupt",		/* 3 */
+	"Instruction Storage Interrupt",	/* 4 */
+	"External Interrupt",			/* 5 */
+	"Alignment Interrupt",			/* 6 */
+	"Program Interrupt",			/* 7 */
+	"Floating Point Unavailable",		/* 8 */
+	"Decrementer Interrupt",		/* 9 */
+	"Reserved",				/* 10 */
+	"Reserved",				/* 11 */
+	"System Call",				/* 12 */
+	"Trace",				/* 13 */
+	"Floating Point Assist",		/* 14 */
+	"Performance Monitoring",		/* 15 */
+	"Instruction TLB Miss",			/* 16 */
+	"Data Load TLB Miss",			/* 17 */
+	"Data Store TLB Miss",			/* 18 */
+	"Instruction Breakpoint",		/* 19 */
+	"System Management Interrupt",		/* 20 */
+	"Reserved 21",				/* 21 */
+	"Reserved 22",				/* 22 */
+	"Reserved 23",				/* 23 */
+	"Reserved 24",				/* 24 */
+	"Reserved 25",				/* 25 */
+	"Reserved 26",				/* 26 */
+	"Reserved 27",				/* 27 */
+	"Reserved 28",				/* 28 */
+	"Reserved 29",				/* 29 */
+	"Reserved 30",				/* 30 */
+	"Reserved 31",				/* 31 */
+	"Reserved 32",				/* 32 */
+	"Reserved 33",				/* 33 */
+	"Reserved 34",				/* 34 */
+	"Reserved 35",				/* 35 */
+	"Reserved 36",				/* 36 */
+	"Reserved 37",				/* 37 */
+	"Reserved 38",				/* 38 */
+	"Reserved 39",				/* 39 */
+	"Reserved 40",				/* 40 */
+	"Reserved 41",				/* 41 */
+	"Reserved 42",				/* 42 */
+	"Reserved 43",				/* 43 */
+	"Reserved 44",				/* 44 */
+	"Reserved 45",				/* 45 */
+	"Reserved 46",				/* 46 */
+	"Reserved 47",				/* 47 */
+};
+
+static void
+printtrap __P((int vector, struct trapframe *frame, int isfatal, int user))
+{
+
+	printf("\n");
+	printf("%s %s trap:\n", isfatal ? "fatal" : "handled",
+	    user ? "user" : "kernel");
+	printf("\n");
+	printf("   exception       = 0x%x (%s)\n", vector >> 8,
+	    ppc_exception_names[vector >> 8]);
+	switch (vector) {
+	case EXC_DSI:
+		printf("   virtual address = 0x%x\n", frame->dar);
+		break;
+	case EXC_ISI:
+		printf("   virtual address = 0x%x\n", frame->srr0);
+		break;
+	}
+	printf("   srr0            = 0x%x", frame->srr0);
+	printf("   curthread       = %p\n", curthread);
+	if (curthread != NULL)
+		printf("          pid = %d, comm = %s\n",
+		    curthread->td_proc->p_pid, curthread->td_proc->p_comm);
+	printf("\n");
+}
+
+static void
+trap_fatal(struct trapframe *frame)
+{
+
+	printtrap(frame->exc, frame, 1, (frame->srr1 & PSL_PR));
+#ifdef DDB
+	if ((debugger_on_panic || db_active) && kdb_trap(frame->exc, 0, frame))
+		return;
+#endif
+	panic("%s Trap", ppc_exception_names[frame->exc >> 8]);
+}
+
+/*
+ * Handles a fatal fault when we have onfault state to recover.  Returns
+ * non-zero if there was onfault recovery state available.
+ */
+static int
+handle_onfault (struct trapframe *frame)
+{
+	struct thread *td;
+	faultbuf *fb;
+
+	td = curthread;
+	fb = td->td_pcb->pcb_onfault;
+	if (fb != NULL) {
+		frame->srr0 = (*fb)[0];
+		frame->fixreg[1] = (*fb)[1];
+		frame->fixreg[2] = (*fb)[2];
+		frame->cr = (*fb)[3];
+		bcopy(&(*fb)[4], &frame->fixreg[13],
+		    19 * sizeof(register_t));
+		return (1);
+	}
+	return (0);
+}
 
 void
 trap(struct trapframe *frame)
 {
-#if 0 /* XXX: This code hasn't been reworked yet. */
+	struct thread *td;
 	struct proc *p;
-	int type;
-	u_int sticks;
+	int sig, type, user;
+	u_int sticks, ucode;
 
-	p = curproc;
-	type = frame->exc;
+	atomic_add_int(&cnt.v_trap, 1);
 
-	if (frame->srr1 & PSL_PR) {
-		type |= EXC_USER;
-		sticks = p->p_sticks;
-	}
+	td = curthread;
+	p = td->td_proc;
 
-	switch (type) {
-	case EXC_TRC|EXC_USER:
-		frame->srr1 &= ~PSL_SE;
-		trapsignal(p, SIGTRAP, EXC_TRC);
+	type = frame->exc;
+	ucode = type;
+	sig = 0;
+	user = (frame->srr1 & PSL_PR);
+	sticks = 0;
+
+	CTR3(KTR_TRAP, "trap: %s type=%s (%s)", p->p_comm,
+	    ppc_exception_names[type >> 8],
+	    user ? "user" : "kernel");
+
+	if (user) {
+		sticks = td->td_kse->ke_sticks;
+		td->td_frame = frame;
+		KASSERT(td->td_ucred == NULL, ("already have a ucred"));
+		PROC_LOCK(p);
+		td->td_ucred = crhold(p->p_ucred);
+		PROC_UNLOCK(p);
+
+		/* User Mode Traps */
+		switch (type) {
+		case EXC_TRC:
+			frame->srr1 &= ~PSL_SE;
+			sig = SIGTRAP;
+			break;
+		case EXC_DSI:
+		case EXC_ISI:
+			sig = trap_pfault(frame, 1);
+			break;
+		case EXC_SC:
+			syscall(frame);
+			break;
+		case EXC_FPU:
+			enable_fpu(PCPU_GET(curpcb));
+			frame->srr1 |= PSL_FP;
+			break;
+
+		case EXC_ALI:
+#if 0			
+		if (fix_unaligned(p, frame) != 0)
+#endif	
+			sig = SIGBUS;
+#if 0		
+		else
+			frame->srr0 += 4;
+#endif		
 		break;
-	case EXC_DSI:
-		{
-			vm_map_t map;
-			vaddr_t va;
-			int ftype;
-			faultbuf *fb;
-			
-			map = kernel_map;
-			va = frame->dar;
-			if ((va >> ADDR_SR_SHFT) == USER_SR) {
-				sr_t user_sr;
-				
-				__asm ("mfsr %0, %1"
-				     : "=r"(user_sr) : "K"(USER_SR));
-				va &= ADDR_PIDX | ADDR_POFF;
-				va |= user_sr << ADDR_SR_SHFT;
-				map = &p->p_vmspace->vm_map;
-			}
-			if (frame->dsisr & DSISR_STORE)
-				ftype = VM_PROT_READ | VM_PROT_WRITE;
+
+		case EXC_PGM:
+			/* XXX temporarily */
+			/* XXX: Magic Number? */
+			if (frame->srr1 & 0x0002000)
+				sig = SIGTRAP;
 			else
-				ftype = VM_PROT_READ;
-			if (uvm_fault(map, trunc_page(va), 0, ftype)
-			    == KERN_SUCCESS)
+				sig = SIGILL;
+			break;
+
+		default:
+			trap_fatal(frame);
+		}
+	} else {
+		/* Kernel Mode Traps */
+
+		KASSERT(cold || td->td_ucred != NULL,
+		    ("kernel trap doesn't have ucred"));
+		switch (type) {
+		case EXC_DSI:
+			if (trap_pfault(frame, 0) == 0)
 				return;
-			if (fb = p->p_addr->u_pcb.pcb_onfault) {
-				frame->srr0 = (*fb)[0];
-				frame->fixreg[1] = (*fb)[1];
-				frame->fixreg[2] = (*fb)[2];
-				frame->cr = (*fb)[3];
-				bcopy(&(*fb)[4], &frame->fixreg[13],
-				      19 * sizeof(register_t));
+			break;
+		case EXC_MCHK:
+			if (handle_onfault(frame))
 				return;
-			}
-			map = kernel_map;
-		}
-		goto brain_damage;
-	case EXC_DSI|EXC_USER:
-		{
-			int ftype, rv;
-			
-			if (frame->dsisr & DSISR_STORE)
-				ftype = VM_PROT_READ | VM_PROT_WRITE;
-			else
-				ftype = VM_PROT_READ;
-			if ((rv = uvm_fault(&p->p_vmspace->vm_map,
-					    trunc_page(frame->dar), 0, ftype))
-			    == KERN_SUCCESS)
-				break;
-			if (rv == KERN_RESOURCE_SHORTAGE) {
-				printf("UVM: pid %d (%s), uid %d killed: "
-				       "out of swap\n",
-				       p->p_pid, p->p_comm,
-				       p->p_cred && p->p_ucred ?
-				       p->p_ucred->cr_uid : -1);
-				trapsignal(p, SIGKILL, EXC_DSI);
-			} else {
-				trapsignal(p, SIGSEGV, EXC_DSI);
-			}
+			break;
+		default:
+			trap_fatal(frame);
 		}
-		break;
-	case EXC_ISI|EXC_USER:
-		{
-			int ftype;
-			
-			ftype = VM_PROT_READ | VM_PROT_EXECUTE;
-			if (uvm_fault(&p->p_vmspace->vm_map,
-				     trunc_page(frame->srr0), 0, ftype)
-			    == KERN_SUCCESS)
-				break;
-		}
-		trapsignal(p, SIGSEGV, EXC_ISI);
-		break;
-	case EXC_SC|EXC_USER:
-		{
-			struct sysent *callp;
-			size_t argsize;
-			register_t code, error;
-			register_t *params, rval[2];
-			int nsys, n;
-			register_t args[10];
-			
-			uvmexp.syscalls++;
-			
-			nsys = p->p_emul->e_nsysent;
-			callp = p->p_emul->e_sysent;
+	}
+	if (sig != 0) {
+		if (p->p_sysent->sv_transtrap != NULL)
+			sig = (p->p_sysent->sv_transtrap)(sig, type);
+		trapsignal(p, sig, ucode);
+	}
+	userret(td, frame, sticks);
+	mtx_assert(&Giant, MA_NOTOWNED);
+	mtx_lock(&Giant);
+	crfree(td->td_ucred);
+	mtx_unlock(&Giant);
+	td->td_ucred = NULL;
+}
+
+void
+syscall(struct trapframe *frame)
+{
+	caddr_t params;
+	struct sysent *callp;
+	struct thread *td;
+	struct proc *p;
+	int error, n;
+	size_t narg;
+	register_t args[10];
+	u_int code;
+
+	td = curthread;
+	p = td->td_proc;
+
+	atomic_add_int(&cnt.v_syscall, 1);
 			
-			code = frame->fixreg[0];
-			params = frame->fixreg + FIRSTARG;
+	code = frame->fixreg[0];
+	params = (caddr_t) (frame->fixreg + FIRSTARG);
 			
-			switch (code) {
-			case SYS_syscall:
-				/*
-				 * code is first argument,
-				 * followed by actual args.
-				 */
-				code = *params++;
-				break;
-			case SYS___syscall:
-				/*
-				 * Like syscall, but code is a quad,
-				 * so as to maintain quad alignment
-				 * for the rest of the args.
-				 */
-				if (callp != sysent)
-					break;
-				params++;
-				code = *params++;
-				break;
-			default:
-				break;
-			}
-			if (code < 0 || code >= nsys)
-				callp += p->p_emul->e_nosys;
-			else
-				callp += code;
-			argsize = callp->sy_argsize;
-			n = NARGREG - (params - (frame->fixreg + FIRSTARG));
-			if (argsize > n * sizeof(register_t)) {
-				bcopy(params, args, n * sizeof(register_t));
-				if (error = copyin(MOREARGS(frame->fixreg[1]),
-						   args + n,
-						   argsize - n * sizeof(register_t))) {
-#ifdef	KTRACE
-					/* Can't get all the arguments! */
-					if (KTRPOINT(p, KTR_SYSCALL))
-						ktrsyscall(p, code, argsize,
-						    args);
-#endif
-					goto syscall_bad;
-				}
-				params = args;
-			}
+	if (p->p_sysent->sv_prepsyscall)
+		/*
+		 * The prep code is MP aware.
+		 */
+		(*p->p_sysent->sv_prepsyscall)(frame, args, &code, &params);
+	else if (code == SYS_syscall)
+		/*
+		 * code is first argument,
+		 * followed by actual args.
+		 */
+		code = *params++;
+	else if (code == SYS___syscall) {
+		/*
+		 * Like syscall, but code is a quad,
+		 * so as to maintain quad alignment
+		 * for the rest of the args.
+		 */
+		params++;
+		code = *params++;
+	}
+
+ 	if (p->p_sysent->sv_mask)
+ 		code &= p->p_sysent->sv_mask;
+
+ 	if (code >= p->p_sysent->sv_size)
+ 		callp = &p->p_sysent->sv_table[0];
+  	else
+ 		callp = &p->p_sysent->sv_table[code];
+
+	narg = callp->sy_narg & SYF_ARGMASK;
+
+	n = NARGREG - (params - (caddr_t)(frame->fixreg + FIRSTARG));
+	if (narg > n * sizeof(register_t)) {
+		bcopy(params, args, n * sizeof(register_t));
+		if (error = copyin(MOREARGS(frame->fixreg[1]), args + n,
+			narg - n * sizeof(register_t))) {
 #ifdef	KTRACE
+			/* Can't get all the arguments! */
 			if (KTRPOINT(p, KTR_SYSCALL))
-				ktrsyscall(p, code, argsize, params);
-#endif
-			rval[0] = 0;
-			rval[1] = frame->fixreg[FIRSTARG + 1];
-			
-			switch (error = (*callp->sy_call)(p, params, rval)) {
-			case 0:
-				frame->fixreg[FIRSTARG] = rval[0];
-				frame->fixreg[FIRSTARG + 1] = rval[1];
-				frame->cr &= ~0x10000000;
-				break;
-			case ERESTART:
-				/*
-				 * Set user's pc back to redo the system call.
-				 */
-				frame->srr0 -= 4;
-				break;
-			case EJUSTRETURN:
-				/* nothing to do */
-				break;
-			default:
-syscall_bad:
-				if (p->p_emul->e_errno)
-					error = p->p_emul->e_errno[error];
-				frame->fixreg[FIRSTARG] = error;
-				frame->cr |= 0x10000000;
-				break;
-			}
-#ifdef	KTRACE
-			if (KTRPOINT(p, KTR_SYSRET))
-				ktrsysret(p, code, error, rval[0]);
+				ktrsyscall(p->p_tracep, code, narg, args);
 #endif
+			goto bad;
 		}
-		break;
+		params = (caddr_t) args;
+	}
 
-	case EXC_FPU|EXC_USER:
-		if (fpuproc)
-			save_fpu(fpuproc);
-		fpuproc = p;
-		enable_fpu(p);
-		break;
+	/*
+	 * Try to run the syscall without Giant if the syscall is MP safe.
+	 */
+	if ((callp->sy_narg & SYF_MPSAFE) == 0)
+		mtx_lock(&Giant);
 
-	case EXC_AST|EXC_USER:
-		/* This is just here that we trap */
+#ifdef	KTRACE
+	if (KTRPOINT(p, KTR_SYSCALL))
+		ktrsyscall(p->p_tracep, code, narg, params);
+#endif
+	td->td_retval[0] = 0;
+	td->td_retval[1] = frame->fixreg[FIRSTARG + 1];
+
+	STOPEVENT(p, S_SCE, narg);
+
+	error = (*callp->sy_call)(td, args);
+	switch (error) {
+	case 0:
+		frame->fixreg[FIRSTARG] = td->td_retval[0];
+		frame->fixreg[FIRSTARG + 1] = td->td_retval[1];
+		/* XXX: Magic number */
+		frame->cr &= ~0x10000000;
 		break;
-
-	case EXC_ALI|EXC_USER:
-		if (fix_unaligned(p, frame) != 0)
-			trapsignal(p, SIGBUS, EXC_ALI);
-		else
-			frame->srr0 += 4;
+	case ERESTART:
+		/*
+		 * Set user's pc back to redo the system call.
+		 */
+		frame->srr0 -= 4;
 		break;
-
-	case EXC_PGM|EXC_USER:
-/* XXX temporarily */
-		if (frame->srr1 & 0x0002000)
-			trapsignal(p, SIGTRAP, EXC_PGM);
-		else
-			trapsignal(p, SIGILL, EXC_PGM);
+	case EJUSTRETURN:
+		/* nothing to do */
 		break;
-
-	case EXC_MCHK:
-		{
-			faultbuf *fb;
-
-			if (fb = p->p_addr->u_pcb.pcb_onfault) {
-				frame->srr0 = (*fb)[0];
-				frame->fixreg[1] = (*fb)[1];
-				frame->fixreg[2] = (*fb)[2];
-				frame->cr = (*fb)[3];
-				bcopy(&(*fb)[4], &frame->fixreg[13],
-				      19 * sizeof(register_t));
-				return;
-			}
+	default:
+bad:
+		if (p->p_sysent->sv_errsize) {
+			if (error >= p->p_sysent->sv_errsize)
+				error = -1;	/* XXX */
+			else
+				error = p->p_sysent->sv_errtbl[error];
 		}
-		goto brain_damage;
+		frame->fixreg[FIRSTARG] = error;
+		/* XXX: Magic number: Carry Flag Equivalent? */
+		frame->cr |= 0x10000000;
+		break;
+	}
 
-	default:
-brain_damage:
-		printf("trap type %x at %x\n", type, frame->srr0);
-#ifdef DDB
-		Debugger();			 /* XXX temporarily */
-#endif
-#ifdef TRAP_PANICWAIT
-		printf("Press a key to panic.\n");
-		cngetc();
+	
+#ifdef	KTRACE
+	if (KTRPOINT(p, KTR_SYSRET))
+		ktrsysret(p->p_tracep, code, error, td->td_retval[0]);
 #endif
-		panic("trap");
-	}
 
-	astpending = 0;		/* we are about to do it */
+	if ((callp->sy_narg & SYF_MPSAFE) == 0)
+		mtx_unlock(&Giant);
+
+	/*
+	 * Does the comment in the i386 code about errno apply here?
+	 */
+	STOPEVENT(p, S_SCX, code);
 
-	uvmexp.softs++;
+#ifdef WITNESS
+	if (witness_list(td)) {
+		panic("system call %s returning with mutex(s) held\n",
+		    syscallnames[code]);
+	}
+#endif
+	mtx_assert(&sched_lock, MA_NOTOWNED);
+	mtx_assert(&Giant, MA_NOTOWNED);	
+}
 
-	if (p->p_flag & P_OWEUPC) {
-		p->p_flag &= ~P_OWEUPC;
-		ADDUPROF(p);
+static int
+trap_pfault(struct trapframe *frame, int user)
+{
+	vm_offset_t eva, va;
+	struct thread *td;
+	struct proc *p;
+	vm_map_t map;
+	vm_prot_t ftype;
+	int rv;
+
+	td = curthread;
+	p = td->td_proc;
+	if (frame->exc == EXC_ISI) {
+		eva = frame->srr0;
+		ftype = VM_PROT_READ | VM_PROT_EXECUTE;
+	} else {
+		eva = frame->dar;
+		if (frame->dsisr & DSISR_STORE)
+			ftype = VM_PROT_READ | VM_PROT_WRITE;
+		else
+			ftype = VM_PROT_READ;
 	}
 
-	/* take pending signals */
-	{
-		int sig;
+	if ((eva >> ADDR_SR_SHFT) != USER_SR) {
+		if (user)
+			return (SIGSEGV);
+		map = kernel_map;
+	} else {
+		sr_t user_sr;
 
-		while (sig = CURSIG(p))
-			postsig(sig);
+		if (p->p_vmspace == NULL)
+			return (SIGSEGV);
+				
+		__asm ("mfsr %0, %1"
+		    : "=r"(user_sr)
+		    : "K"(USER_SR));
+		eva &= ADDR_PIDX | ADDR_POFF;
+		eva |= user_sr << ADDR_SR_SHFT;
+		map = &p->p_vmspace->vm_map;
 	}
+	va = trunc_page(eva);
 
-	p->p_priority = p->p_usrpri;
-	if (want_resched) {
-		int sig;
+	mtx_lock(&Giant);
+	if (map != kernel_map) {
 		/*
-		 * We are being preempted.
+		 * Keep swapout from messing with us during this
+		 *	critical time.
 		 */
-		preempt(NULL);
-		while (sig = CURSIG(p))
-			postsig(sig);
+		PROC_LOCK(p);
+		++p->p_lock;
+		PROC_UNLOCK(p);
+
+		/*
+		 * Grow the stack if necessary
+		 */
+		/* grow_stack returns false only if va falls into
+		 * a growable stack region and the stack growth
+		 * fails.  It returns true if va was not within
+		 * a growable stack region, or if the stack 
+		 * growth succeeded.
+		 */
+		if (!grow_stack (p, va))
+			rv = KERN_FAILURE;
+		else
+			/* Fault in the user page: */
+			rv = vm_fault(map, va, ftype,
+			      (ftype & VM_PROT_WRITE) ? VM_FAULT_DIRTY
+						      : VM_FAULT_NORMAL);
+
+		PROC_LOCK(p);
+		--p->p_lock;
+		PROC_UNLOCK(p);
+	} else {
+		/*
+		 * Don't have to worry about process locking or stacks in the
+		 * kernel.
+		 */
+		rv = vm_fault(map, va, ftype, VM_FAULT_NORMAL);
 	}
+	mtx_unlock(&Giant);
 
-	/*
-	 * If profiling, charge recent system time to the trapped pc.
-	 */
-	if (p->p_flag & P_PROFIL) {
-		extern int psratio;
+	if (rv == KERN_SUCCESS)
+		return (0);
 
-		addupc_task(p, frame->srr0,
-			    (int)(p->p_sticks - sticks) * psratio);
-	}
-	/*
-	 * If someone stole the fpu while we were away, disable it
-	 */
-	if (p != fpuproc)
-		frame->srr1 &= ~PSL_FP;
-	curcpu()->ci_schedstate.spc_curpriority = p->p_priority;
-#endif
+	if (!user && handle_onfault(frame))
+		return (0);
+
+	return (SIGSEGV);
 }
 
 #if 0 /* XXX: child_return not used */
+/*
+ * XXX: the trapframe return values should be setup in vm_machdep.c in
+ * cpu_fork().
+ */
 void
 child_return(void *arg)
 {
@@ -576,18 +747,20 @@ fix_unaligned(p, frame)
 			 * the FPRs, and that their current state is in
 			 * the PCB.
 			 */
-			if (fpuproc != p) {
-				if (fpuproc)
-					save_fpu(fpuproc);
-				enable_fpu(p);
+			if (!(pcb->pcb_flags & PCB_FPU))
+				enable_fpu(PCPU_GET(curpcb));
+				frame->srr1 |= PSL_FP;
 			}
-			save_fpu(p);
+			save_fpu(PCPU_GET(curpcb));
 
 			if (indicator == EXC_ALI_LFD) {
 				if (copyin((void *)frame->dar, fpr,
 				    sizeof(double)) != 0)
 					return -1;
-				enable_fpu(p);
+				if (!(pcb->pcb_flags & PCB_FPU))
+					enable_fpu(PCPU_GET(curpcb));
+					frame->srr1 |= PSL_FP;
+				}
 			} else {
 				if (copyout(fpr, (void *)frame->dar,
 				    sizeof(double)) != 0)
diff --git a/sys/powerpc/include/cpu.h b/sys/powerpc/include/cpu.h
index e23732f..9221b34 100644
--- a/sys/powerpc/include/cpu.h
+++ b/sys/powerpc/include/cpu.h
@@ -53,9 +53,6 @@
 void	delay(int);
 #define	DELAY(n)		delay(n)
 
-extern int want_resched;
-extern int astpending;
-
 extern char bootpath[];
 
 #if defined(_KERNEL) || defined(_STANDALONE)
diff --git a/sys/powerpc/include/trap.h b/sys/powerpc/include/trap.h
index dd359d2..2438f08 100644
--- a/sys/powerpc/include/trap.h
+++ b/sys/powerpc/include/trap.h
@@ -52,7 +52,7 @@
 /* The following are only available on 604: */
 #define	EXC_PERF	0x0f00		/* Performance Monitoring */
 #define	EXC_BPT		0x1300		/* Instruction Breakpoint */
-#define	EXC_SMI		0x1400		/* System Managment Interrupt */
+#define	EXC_SMI		0x1400		/* System Management Interrupt */
 
 /* And these are only on the 603: */
 #define	EXC_IMISS	0x1000		/* Instruction translation miss */
@@ -61,8 +61,6 @@
 
 #define	EXC_LAST	0x2f00		/* Last possible exception vector */
 
-#define	EXC_AST		0x3000		/* Fake AST vector */
-
 /* Trap was in user mode */
 #define	EXC_USER	0x10000
 
@@ -87,6 +85,7 @@
 #ifndef	LOCORE
 
 void	trap(struct trapframe *);
+void	syscall(struct trapframe *);
 
 #endif /* !LOCORE */
 
diff --git a/sys/powerpc/powerpc/locore.S b/sys/powerpc/powerpc/locore.S
index 9deb42f..24571b6 100644
--- a/sys/powerpc/powerpc/locore.S
+++ b/sys/powerpc/powerpc/locore.S
@@ -927,35 +927,16 @@ s_trap:
 	FRAME_SETUP(tempsave)
 /* Now we can recover interrupts again: */
 	mfmsr	7
-	ori	7,7,(PSL_EE|PSL_ME|PSL_RI)@l
+	ori	7,7,(PSL_EE|PSL_FP|PSL_ME|PSL_RI)@l
 	mtmsr	7
 	isync
 /* Call C trap code: */
-trapagain:
 	addi	3,1,8
+	mr	30,3
 	bl	trap
-trapexit:
-/* Disable interrupts: */
-	mfmsr	3
-	andi.	3,3,~PSL_EE@l
-	mtmsr	3
-/* Test AST pending: */
-	lwz	5,FRAME_SRR1+8(1)
-	mtcr	5
-	bc	4,17,1f			/* branch if PSL_PR is false */
-	lis	3,astpending@ha
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
-#if 0 /* XXX */
-	li	6,EXC_AST
-#endif
-	stw	6,FRAME_EXC+8(1)
-	b	trapagain
-1:
-#if 0
+	mr	3,30
+	bl	ast
 	FRAME_LEAVE(tempsave)
-#endif
 	rfi
 
 /*
@@ -971,7 +952,10 @@ fork_trampoline:
 	mtlr	31
 	mr	3,30
 	blrl				/* jump indirect to r31 */
-	b	trapexit
+	mr	3,30
+	bl	ast
+	FRAME_LEAVE(tempsave)
+	rfi
 
 /*
  * DSI second stage fault handler
@@ -1129,10 +1113,6 @@ intr_exit:
 	lwz	3,GD_CURPCB(3)		/* get curpcb from globaldata */
 	lwz	3,PCB_PMR(3)		/* get pmap real address from curpcb */
 	mtsr	KERNEL_SR,3
-	lis	3,astpending@ha		/* Test AST pending */
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
 /* Setup for entry to realtrap: */
 	lwz	3,0(1)			/* get saved SP */
 	mtsprg	1,3
@@ -1152,7 +1132,7 @@ intr_exit:
 	lwz	31,intr_depth@l(30)
 	addi	31,31,-1
 	stw	31,intr_depth@l(30)
-	b	realtrap
+	b	realtrap		/* XXX:	should call ast(frame ptr) */
 1:
 /* Here is the normal exit of extintr: */
 	lwz	5,36(1)
diff --git a/sys/powerpc/powerpc/locore.s b/sys/powerpc/powerpc/locore.s
index 9deb42f..24571b6 100644
--- a/sys/powerpc/powerpc/locore.s
+++ b/sys/powerpc/powerpc/locore.s
@@ -927,35 +927,16 @@ s_trap:
 	FRAME_SETUP(tempsave)
 /* Now we can recover interrupts again: */
 	mfmsr	7
-	ori	7,7,(PSL_EE|PSL_ME|PSL_RI)@l
+	ori	7,7,(PSL_EE|PSL_FP|PSL_ME|PSL_RI)@l
 	mtmsr	7
 	isync
 /* Call C trap code: */
-trapagain:
 	addi	3,1,8
+	mr	30,3
 	bl	trap
-trapexit:
-/* Disable interrupts: */
-	mfmsr	3
-	andi.	3,3,~PSL_EE@l
-	mtmsr	3
-/* Test AST pending: */
-	lwz	5,FRAME_SRR1+8(1)
-	mtcr	5
-	bc	4,17,1f			/* branch if PSL_PR is false */
-	lis	3,astpending@ha
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
-#if 0 /* XXX */
-	li	6,EXC_AST
-#endif
-	stw	6,FRAME_EXC+8(1)
-	b	trapagain
-1:
-#if 0
+	mr	3,30
+	bl	ast
 	FRAME_LEAVE(tempsave)
-#endif
 	rfi
 
 /*
@@ -971,7 +952,10 @@ fork_trampoline:
 	mtlr	31
 	mr	3,30
 	blrl				/* jump indirect to r31 */
-	b	trapexit
+	mr	3,30
+	bl	ast
+	FRAME_LEAVE(tempsave)
+	rfi
 
 /*
  * DSI second stage fault handler
@@ -1129,10 +1113,6 @@ intr_exit:
 	lwz	3,GD_CURPCB(3)		/* get curpcb from globaldata */
 	lwz	3,PCB_PMR(3)		/* get pmap real address from curpcb */
 	mtsr	KERNEL_SR,3
-	lis	3,astpending@ha		/* Test AST pending */
-	lwz	4,astpending@l(3)
-	andi.	4,4,1
-	beq	1f
 /* Setup for entry to realtrap: */
 	lwz	3,0(1)			/* get saved SP */
 	mtsprg	1,3
@@ -1152,7 +1132,7 @@ intr_exit:
 	lwz	31,intr_depth@l(30)
 	addi	31,31,-1
 	stw	31,intr_depth@l(30)
-	b	realtrap
+	b	realtrap		/* XXX:	should call ast(frame ptr) */
 1:
 /* Here is the normal exit of extintr: */
 	lwz	5,36(1)
diff --git a/sys/powerpc/powerpc/trap.c b/sys/powerpc/powerpc/trap.c
index 0a48fe8..e76bc88 100644
--- a/sys/powerpc/powerpc/trap.c
+++ b/sys/powerpc/powerpc/trap.c
@@ -41,16 +41,27 @@ static const char rcsid[] =
 
 #include <sys/param.h>
 #include <sys/proc.h>
+#include <sys/ktr.h>
+#include <sys/lock.h>
+#include <sys/mutex.h>
+#include <sys/pioctl.h>
 #include <sys/reboot.h>
 #include <sys/syscall.h>
+#include <sys/sysent.h>
 #include <sys/systm.h>
 #include <sys/uio.h>
 #include <sys/user.h>
+#ifdef KTRACE
 #include <sys/ktrace.h>
+#endif
+#include <sys/vmmeter.h>
 
 #include <vm/vm.h>
-#include <vm/vm_kern.h>
 #include <vm/pmap.h>
+#include <vm/vm_extern.h>
+#include <vm/vm_kern.h>
+#include <vm/vm_map.h>
+#include <vm/vm_param.h>
 
 #include <machine/cpu.h>
 #include <machine/frame.h>
@@ -63,306 +74,466 @@ static const char rcsid[] =
 #define	NARGREG		8		/* 8 args are in registers */
 #define	MOREARGS(sp)	((caddr_t)((int)(sp) + 8)) /* more args go here */
 
-volatile int	astpending;
-volatile int	want_resched;
+#ifdef WITNESS
+extern char *syscallnames[];
+#endif
 
 #if 0 /* XXX: not used yet */
 static int fix_unaligned __P((struct proc *p, struct trapframe *frame));
 #endif
+static void trap_fatal __P((struct trapframe *frame));
+static void printtrap __P((int vector, struct trapframe *frame, int isfatal,
+			      int user));
+static int trap_pfault __P((struct trapframe *frame, int user));
+static int handle_onfault (struct trapframe *frame);
+
+static const char *ppc_exception_names[] = {
+	"Reserved 0",				/* 0 */
+	"Reset",				/* 1 */
+	"Machine Check",			/* 2 */
+	"Data Storage Interrupt",		/* 3 */
+	"Instruction Storage Interrupt",	/* 4 */
+	"External Interrupt",			/* 5 */
+	"Alignment Interrupt",			/* 6 */
+	"Program Interrupt",			/* 7 */
+	"Floating Point Unavailable",		/* 8 */
+	"Decrementer Interrupt",		/* 9 */
+	"Reserved",				/* 10 */
+	"Reserved",				/* 11 */
+	"System Call",				/* 12 */
+	"Trace",				/* 13 */
+	"Floating Point Assist",		/* 14 */
+	"Performance Monitoring",		/* 15 */
+	"Instruction TLB Miss",			/* 16 */
+	"Data Load TLB Miss",			/* 17 */
+	"Data Store TLB Miss",			/* 18 */
+	"Instruction Breakpoint",		/* 19 */
+	"System Management Interrupt",		/* 20 */
+	"Reserved 21",				/* 21 */
+	"Reserved 22",				/* 22 */
+	"Reserved 23",				/* 23 */
+	"Reserved 24",				/* 24 */
+	"Reserved 25",				/* 25 */
+	"Reserved 26",				/* 26 */
+	"Reserved 27",				/* 27 */
+	"Reserved 28",				/* 28 */
+	"Reserved 29",				/* 29 */
+	"Reserved 30",				/* 30 */
+	"Reserved 31",				/* 31 */
+	"Reserved 32",				/* 32 */
+	"Reserved 33",				/* 33 */
+	"Reserved 34",				/* 34 */
+	"Reserved 35",				/* 35 */
+	"Reserved 36",				/* 36 */
+	"Reserved 37",				/* 37 */
+	"Reserved 38",				/* 38 */
+	"Reserved 39",				/* 39 */
+	"Reserved 40",				/* 40 */
+	"Reserved 41",				/* 41 */
+	"Reserved 42",				/* 42 */
+	"Reserved 43",				/* 43 */
+	"Reserved 44",				/* 44 */
+	"Reserved 45",				/* 45 */
+	"Reserved 46",				/* 46 */
+	"Reserved 47",				/* 47 */
+};
+
+static void
+printtrap __P((int vector, struct trapframe *frame, int isfatal, int user))
+{
+
+	printf("\n");
+	printf("%s %s trap:\n", isfatal ? "fatal" : "handled",
+	    user ? "user" : "kernel");
+	printf("\n");
+	printf("   exception       = 0x%x (%s)\n", vector >> 8,
+	    ppc_exception_names[vector >> 8]);
+	switch (vector) {
+	case EXC_DSI:
+		printf("   virtual address = 0x%x\n", frame->dar);
+		break;
+	case EXC_ISI:
+		printf("   virtual address = 0x%x\n", frame->srr0);
+		break;
+	}
+	printf("   srr0            = 0x%x", frame->srr0);
+	printf("   curthread       = %p\n", curthread);
+	if (curthread != NULL)
+		printf("          pid = %d, comm = %s\n",
+		    curthread->td_proc->p_pid, curthread->td_proc->p_comm);
+	printf("\n");
+}
+
+static void
+trap_fatal(struct trapframe *frame)
+{
+
+	printtrap(frame->exc, frame, 1, (frame->srr1 & PSL_PR));
+#ifdef DDB
+	if ((debugger_on_panic || db_active) && kdb_trap(frame->exc, 0, frame))
+		return;
+#endif
+	panic("%s Trap", ppc_exception_names[frame->exc >> 8]);
+}
+
+/*
+ * Handles a fatal fault when we have onfault state to recover.  Returns
+ * non-zero if there was onfault recovery state available.
+ */
+static int
+handle_onfault (struct trapframe *frame)
+{
+	struct thread *td;
+	faultbuf *fb;
+
+	td = curthread;
+	fb = td->td_pcb->pcb_onfault;
+	if (fb != NULL) {
+		frame->srr0 = (*fb)[0];
+		frame->fixreg[1] = (*fb)[1];
+		frame->fixreg[2] = (*fb)[2];
+		frame->cr = (*fb)[3];
+		bcopy(&(*fb)[4], &frame->fixreg[13],
+		    19 * sizeof(register_t));
+		return (1);
+	}
+	return (0);
+}
 
 void
 trap(struct trapframe *frame)
 {
-#if 0 /* XXX: This code hasn't been reworked yet. */
+	struct thread *td;
 	struct proc *p;
-	int type;
-	u_int sticks;
+	int sig, type, user;
+	u_int sticks, ucode;
 
-	p = curproc;
-	type = frame->exc;
+	atomic_add_int(&cnt.v_trap, 1);
 
-	if (frame->srr1 & PSL_PR) {
-		type |= EXC_USER;
-		sticks = p->p_sticks;
-	}
+	td = curthread;
+	p = td->td_proc;
 
-	switch (type) {
-	case EXC_TRC|EXC_USER:
-		frame->srr1 &= ~PSL_SE;
-		trapsignal(p, SIGTRAP, EXC_TRC);
+	type = frame->exc;
+	ucode = type;
+	sig = 0;
+	user = (frame->srr1 & PSL_PR);
+	sticks = 0;
+
+	CTR3(KTR_TRAP, "trap: %s type=%s (%s)", p->p_comm,
+	    ppc_exception_names[type >> 8],
+	    user ? "user" : "kernel");
+
+	if (user) {
+		sticks = td->td_kse->ke_sticks;
+		td->td_frame = frame;
+		KASSERT(td->td_ucred == NULL, ("already have a ucred"));
+		PROC_LOCK(p);
+		td->td_ucred = crhold(p->p_ucred);
+		PROC_UNLOCK(p);
+
+		/* User Mode Traps */
+		switch (type) {
+		case EXC_TRC:
+			frame->srr1 &= ~PSL_SE;
+			sig = SIGTRAP;
+			break;
+		case EXC_DSI:
+		case EXC_ISI:
+			sig = trap_pfault(frame, 1);
+			break;
+		case EXC_SC:
+			syscall(frame);
+			break;
+		case EXC_FPU:
+			enable_fpu(PCPU_GET(curpcb));
+			frame->srr1 |= PSL_FP;
+			break;
+
+		case EXC_ALI:
+#if 0			
+		if (fix_unaligned(p, frame) != 0)
+#endif	
+			sig = SIGBUS;
+#if 0		
+		else
+			frame->srr0 += 4;
+#endif		
 		break;
-	case EXC_DSI:
-		{
-			vm_map_t map;
-			vaddr_t va;
-			int ftype;
-			faultbuf *fb;
-			
-			map = kernel_map;
-			va = frame->dar;
-			if ((va >> ADDR_SR_SHFT) == USER_SR) {
-				sr_t user_sr;
-				
-				__asm ("mfsr %0, %1"
-				     : "=r"(user_sr) : "K"(USER_SR));
-				va &= ADDR_PIDX | ADDR_POFF;
-				va |= user_sr << ADDR_SR_SHFT;
-				map = &p->p_vmspace->vm_map;
-			}
-			if (frame->dsisr & DSISR_STORE)
-				ftype = VM_PROT_READ | VM_PROT_WRITE;
+
+		case EXC_PGM:
+			/* XXX temporarily */
+			/* XXX: Magic Number? */
+			if (frame->srr1 & 0x0002000)
+				sig = SIGTRAP;
 			else
-				ftype = VM_PROT_READ;
-			if (uvm_fault(map, trunc_page(va), 0, ftype)
-			    == KERN_SUCCESS)
+				sig = SIGILL;
+			break;
+
+		default:
+			trap_fatal(frame);
+		}
+	} else {
+		/* Kernel Mode Traps */
+
+		KASSERT(cold || td->td_ucred != NULL,
+		    ("kernel trap doesn't have ucred"));
+		switch (type) {
+		case EXC_DSI:
+			if (trap_pfault(frame, 0) == 0)
 				return;
-			if (fb = p->p_addr->u_pcb.pcb_onfault) {
-				frame->srr0 = (*fb)[0];
-				frame->fixreg[1] = (*fb)[1];
-				frame->fixreg[2] = (*fb)[2];
-				frame->cr = (*fb)[3];
-				bcopy(&(*fb)[4], &frame->fixreg[13],
-				      19 * sizeof(register_t));
+			break;
+		case EXC_MCHK:
+			if (handle_onfault(frame))
 				return;
-			}
-			map = kernel_map;
-		}
-		goto brain_damage;
-	case EXC_DSI|EXC_USER:
-		{
-			int ftype, rv;
-			
-			if (frame->dsisr & DSISR_STORE)
-				ftype = VM_PROT_READ | VM_PROT_WRITE;
-			else
-				ftype = VM_PROT_READ;
-			if ((rv = uvm_fault(&p->p_vmspace->vm_map,
-					    trunc_page(frame->dar), 0, ftype))
-			    == KERN_SUCCESS)
-				break;
-			if (rv == KERN_RESOURCE_SHORTAGE) {
-				printf("UVM: pid %d (%s), uid %d killed: "
-				       "out of swap\n",
-				       p->p_pid, p->p_comm,
-				       p->p_cred && p->p_ucred ?
-				       p->p_ucred->cr_uid : -1);
-				trapsignal(p, SIGKILL, EXC_DSI);
-			} else {
-				trapsignal(p, SIGSEGV, EXC_DSI);
-			}
+			break;
+		default:
+			trap_fatal(frame);
 		}
-		break;
-	case EXC_ISI|EXC_USER:
-		{
-			int ftype;
-			
-			ftype = VM_PROT_READ | VM_PROT_EXECUTE;
-			if (uvm_fault(&p->p_vmspace->vm_map,
-				     trunc_page(frame->srr0), 0, ftype)
-			    == KERN_SUCCESS)
-				break;
-		}
-		trapsignal(p, SIGSEGV, EXC_ISI);
-		break;
-	case EXC_SC|EXC_USER:
-		{
-			struct sysent *callp;
-			size_t argsize;
-			register_t code, error;
-			register_t *params, rval[2];
-			int nsys, n;
-			register_t args[10];
-			
-			uvmexp.syscalls++;
-			
-			nsys = p->p_emul->e_nsysent;
-			callp = p->p_emul->e_sysent;
+	}
+	if (sig != 0) {
+		if (p->p_sysent->sv_transtrap != NULL)
+			sig = (p->p_sysent->sv_transtrap)(sig, type);
+		trapsignal(p, sig, ucode);
+	}
+	userret(td, frame, sticks);
+	mtx_assert(&Giant, MA_NOTOWNED);
+	mtx_lock(&Giant);
+	crfree(td->td_ucred);
+	mtx_unlock(&Giant);
+	td->td_ucred = NULL;
+}
+
+void
+syscall(struct trapframe *frame)
+{
+	caddr_t params;
+	struct sysent *callp;
+	struct thread *td;
+	struct proc *p;
+	int error, n;
+	size_t narg;
+	register_t args[10];
+	u_int code;
+
+	td = curthread;
+	p = td->td_proc;
+
+	atomic_add_int(&cnt.v_syscall, 1);
 			
-			code = frame->fixreg[0];
-			params = frame->fixreg + FIRSTARG;
+	code = frame->fixreg[0];
+	params = (caddr_t) (frame->fixreg + FIRSTARG);
 			
-			switch (code) {
-			case SYS_syscall:
-				/*
-				 * code is first argument,
-				 * followed by actual args.
-				 */
-				code = *params++;
-				break;
-			case SYS___syscall:
-				/*
-				 * Like syscall, but code is a quad,
-				 * so as to maintain quad alignment
-				 * for the rest of the args.
-				 */
-				if (callp != sysent)
-					break;
-				params++;
-				code = *params++;
-				break;
-			default:
-				break;
-			}
-			if (code < 0 || code >= nsys)
-				callp += p->p_emul->e_nosys;
-			else
-				callp += code;
-			argsize = callp->sy_argsize;
-			n = NARGREG - (params - (frame->fixreg + FIRSTARG));
-			if (argsize > n * sizeof(register_t)) {
-				bcopy(params, args, n * sizeof(register_t));
-				if (error = copyin(MOREARGS(frame->fixreg[1]),
-						   args + n,
-						   argsize - n * sizeof(register_t))) {
-#ifdef	KTRACE
-					/* Can't get all the arguments! */
-					if (KTRPOINT(p, KTR_SYSCALL))
-						ktrsyscall(p, code, argsize,
-						    args);
-#endif
-					goto syscall_bad;
-				}
-				params = args;
-			}
+	if (p->p_sysent->sv_prepsyscall)
+		/*
+		 * The prep code is MP aware.
+		 */
+		(*p->p_sysent->sv_prepsyscall)(frame, args, &code, &params);
+	else if (code == SYS_syscall)
+		/*
+		 * code is first argument,
+		 * followed by actual args.
+		 */
+		code = *params++;
+	else if (code == SYS___syscall) {
+		/*
+		 * Like syscall, but code is a quad,
+		 * so as to maintain quad alignment
+		 * for the rest of the args.
+		 */
+		params++;
+		code = *params++;
+	}
+
+ 	if (p->p_sysent->sv_mask)
+ 		code &= p->p_sysent->sv_mask;
+
+ 	if (code >= p->p_sysent->sv_size)
+ 		callp = &p->p_sysent->sv_table[0];
+  	else
+ 		callp = &p->p_sysent->sv_table[code];
+
+	narg = callp->sy_narg & SYF_ARGMASK;
+
+	n = NARGREG - (params - (caddr_t)(frame->fixreg + FIRSTARG));
+	if (narg > n * sizeof(register_t)) {
+		bcopy(params, args, n * sizeof(register_t));
+		if (error = copyin(MOREARGS(frame->fixreg[1]), args + n,
+			narg - n * sizeof(register_t))) {
 #ifdef	KTRACE
+			/* Can't get all the arguments! */
 			if (KTRPOINT(p, KTR_SYSCALL))
-				ktrsyscall(p, code, argsize, params);
-#endif
-			rval[0] = 0;
-			rval[1] = frame->fixreg[FIRSTARG + 1];
-			
-			switch (error = (*callp->sy_call)(p, params, rval)) {
-			case 0:
-				frame->fixreg[FIRSTARG] = rval[0];
-				frame->fixreg[FIRSTARG + 1] = rval[1];
-				frame->cr &= ~0x10000000;
-				break;
-			case ERESTART:
-				/*
-				 * Set user's pc back to redo the system call.
-				 */
-				frame->srr0 -= 4;
-				break;
-			case EJUSTRETURN:
-				/* nothing to do */
-				break;
-			default:
-syscall_bad:
-				if (p->p_emul->e_errno)
-					error = p->p_emul->e_errno[error];
-				frame->fixreg[FIRSTARG] = error;
-				frame->cr |= 0x10000000;
-				break;
-			}
-#ifdef	KTRACE
-			if (KTRPOINT(p, KTR_SYSRET))
-				ktrsysret(p, code, error, rval[0]);
+				ktrsyscall(p->p_tracep, code, narg, args);
 #endif
+			goto bad;
 		}
-		break;
+		params = (caddr_t) args;
+	}
 
-	case EXC_FPU|EXC_USER:
-		if (fpuproc)
-			save_fpu(fpuproc);
-		fpuproc = p;
-		enable_fpu(p);
-		break;
+	/*
+	 * Try to run the syscall without Giant if the syscall is MP safe.
+	 */
+	if ((callp->sy_narg & SYF_MPSAFE) == 0)
+		mtx_lock(&Giant);
 
-	case EXC_AST|EXC_USER:
-		/* This is just here that we trap */
+#ifdef	KTRACE
+	if (KTRPOINT(p, KTR_SYSCALL))
+		ktrsyscall(p->p_tracep, code, narg, params);
+#endif
+	td->td_retval[0] = 0;
+	td->td_retval[1] = frame->fixreg[FIRSTARG + 1];
+
+	STOPEVENT(p, S_SCE, narg);
+
+	error = (*callp->sy_call)(td, args);
+	switch (error) {
+	case 0:
+		frame->fixreg[FIRSTARG] = td->td_retval[0];
+		frame->fixreg[FIRSTARG + 1] = td->td_retval[1];
+		/* XXX: Magic number */
+		frame->cr &= ~0x10000000;
 		break;
-
-	case EXC_ALI|EXC_USER:
-		if (fix_unaligned(p, frame) != 0)
-			trapsignal(p, SIGBUS, EXC_ALI);
-		else
-			frame->srr0 += 4;
+	case ERESTART:
+		/*
+		 * Set user's pc back to redo the system call.
+		 */
+		frame->srr0 -= 4;
 		break;
-
-	case EXC_PGM|EXC_USER:
-/* XXX temporarily */
-		if (frame->srr1 & 0x0002000)
-			trapsignal(p, SIGTRAP, EXC_PGM);
-		else
-			trapsignal(p, SIGILL, EXC_PGM);
+	case EJUSTRETURN:
+		/* nothing to do */
 		break;
-
-	case EXC_MCHK:
-		{
-			faultbuf *fb;
-
-			if (fb = p->p_addr->u_pcb.pcb_onfault) {
-				frame->srr0 = (*fb)[0];
-				frame->fixreg[1] = (*fb)[1];
-				frame->fixreg[2] = (*fb)[2];
-				frame->cr = (*fb)[3];
-				bcopy(&(*fb)[4], &frame->fixreg[13],
-				      19 * sizeof(register_t));
-				return;
-			}
+	default:
+bad:
+		if (p->p_sysent->sv_errsize) {
+			if (error >= p->p_sysent->sv_errsize)
+				error = -1;	/* XXX */
+			else
+				error = p->p_sysent->sv_errtbl[error];
 		}
-		goto brain_damage;
+		frame->fixreg[FIRSTARG] = error;
+		/* XXX: Magic number: Carry Flag Equivalent? */
+		frame->cr |= 0x10000000;
+		break;
+	}
 
-	default:
-brain_damage:
-		printf("trap type %x at %x\n", type, frame->srr0);
-#ifdef DDB
-		Debugger();			 /* XXX temporarily */
-#endif
-#ifdef TRAP_PANICWAIT
-		printf("Press a key to panic.\n");
-		cngetc();
+	
+#ifdef	KTRACE
+	if (KTRPOINT(p, KTR_SYSRET))
+		ktrsysret(p->p_tracep, code, error, td->td_retval[0]);
 #endif
-		panic("trap");
-	}
 
-	astpending = 0;		/* we are about to do it */
+	if ((callp->sy_narg & SYF_MPSAFE) == 0)
+		mtx_unlock(&Giant);
+
+	/*
+	 * Does the comment in the i386 code about errno apply here?
+	 */
+	STOPEVENT(p, S_SCX, code);
 
-	uvmexp.softs++;
+#ifdef WITNESS
+	if (witness_list(td)) {
+		panic("system call %s returning with mutex(s) held\n",
+		    syscallnames[code]);
+	}
+#endif
+	mtx_assert(&sched_lock, MA_NOTOWNED);
+	mtx_assert(&Giant, MA_NOTOWNED);	
+}
 
-	if (p->p_flag & P_OWEUPC) {
-		p->p_flag &= ~P_OWEUPC;
-		ADDUPROF(p);
+static int
+trap_pfault(struct trapframe *frame, int user)
+{
+	vm_offset_t eva, va;
+	struct thread *td;
+	struct proc *p;
+	vm_map_t map;
+	vm_prot_t ftype;
+	int rv;
+
+	td = curthread;
+	p = td->td_proc;
+	if (frame->exc == EXC_ISI) {
+		eva = frame->srr0;
+		ftype = VM_PROT_READ | VM_PROT_EXECUTE;
+	} else {
+		eva = frame->dar;
+		if (frame->dsisr & DSISR_STORE)
+			ftype = VM_PROT_READ | VM_PROT_WRITE;
+		else
+			ftype = VM_PROT_READ;
 	}
 
-	/* take pending signals */
-	{
-		int sig;
+	if ((eva >> ADDR_SR_SHFT) != USER_SR) {
+		if (user)
+			return (SIGSEGV);
+		map = kernel_map;
+	} else {
+		sr_t user_sr;
 
-		while (sig = CURSIG(p))
-			postsig(sig);
+		if (p->p_vmspace == NULL)
+			return (SIGSEGV);
+				
+		__asm ("mfsr %0, %1"
+		    : "=r"(user_sr)
+		    : "K"(USER_SR));
+		eva &= ADDR_PIDX | ADDR_POFF;
+		eva |= user_sr << ADDR_SR_SHFT;
+		map = &p->p_vmspace->vm_map;
 	}
+	va = trunc_page(eva);
 
-	p->p_priority = p->p_usrpri;
-	if (want_resched) {
-		int sig;
+	mtx_lock(&Giant);
+	if (map != kernel_map) {
 		/*
-		 * We are being preempted.
+		 * Keep swapout from messing with us during this
+		 *	critical time.
 		 */
-		preempt(NULL);
-		while (sig = CURSIG(p))
-			postsig(sig);
+		PROC_LOCK(p);
+		++p->p_lock;
+		PROC_UNLOCK(p);
+
+		/*
+		 * Grow the stack if necessary
+		 */
+		/* grow_stack returns false only if va falls into
+		 * a growable stack region and the stack growth
+		 * fails.  It returns true if va was not within
+		 * a growable stack region, or if the stack 
+		 * growth succeeded.
+		 */
+		if (!grow_stack (p, va))
+			rv = KERN_FAILURE;
+		else
+			/* Fault in the user page: */
+			rv = vm_fault(map, va, ftype,
+			      (ftype & VM_PROT_WRITE) ? VM_FAULT_DIRTY
+						      : VM_FAULT_NORMAL);
+
+		PROC_LOCK(p);
+		--p->p_lock;
+		PROC_UNLOCK(p);
+	} else {
+		/*
+		 * Don't have to worry about process locking or stacks in the
+		 * kernel.
+		 */
+		rv = vm_fault(map, va, ftype, VM_FAULT_NORMAL);
 	}
+	mtx_unlock(&Giant);
 
-	/*
-	 * If profiling, charge recent system time to the trapped pc.
-	 */
-	if (p->p_flag & P_PROFIL) {
-		extern int psratio;
+	if (rv == KERN_SUCCESS)
+		return (0);
 
-		addupc_task(p, frame->srr0,
-			    (int)(p->p_sticks - sticks) * psratio);
-	}
-	/*
-	 * If someone stole the fpu while we were away, disable it
-	 */
-	if (p != fpuproc)
-		frame->srr1 &= ~PSL_FP;
-	curcpu()->ci_schedstate.spc_curpriority = p->p_priority;
-#endif
+	if (!user && handle_onfault(frame))
+		return (0);
+
+	return (SIGSEGV);
 }
 
 #if 0 /* XXX: child_return not used */
+/*
+ * XXX: the trapframe return values should be setup in vm_machdep.c in
+ * cpu_fork().
+ */
 void
 child_return(void *arg)
 {
@@ -576,18 +747,20 @@ fix_unaligned(p, frame)
 			 * the FPRs, and that their current state is in
 			 * the PCB.
 			 */
-			if (fpuproc != p) {
-				if (fpuproc)
-					save_fpu(fpuproc);
-				enable_fpu(p);
+			if (!(pcb->pcb_flags & PCB_FPU))
+				enable_fpu(PCPU_GET(curpcb));
+				frame->srr1 |= PSL_FP;
 			}
-			save_fpu(p);
+			save_fpu(PCPU_GET(curpcb));
 
 			if (indicator == EXC_ALI_LFD) {
 				if (copyin((void *)frame->dar, fpr,
 				    sizeof(double)) != 0)
 					return -1;
-				enable_fpu(p);
+				if (!(pcb->pcb_flags & PCB_FPU))
+					enable_fpu(PCPU_GET(curpcb));
+					frame->srr1 |= PSL_FP;
+				}
 			} else {
 				if (copyout(fpr, (void *)frame->dar,
 				    sizeof(double)) != 0)