7 files changed, 459 insertions, 149 deletions

diff --git a/sys/powerpc/aim/copyinout.c b/sys/powerpc/aim/copyinout.c
index 5ef42caf4817..0f1dbc557f06 100644
--- a/sys/powerpc/aim/copyinout.c
+++ b/sys/powerpc/aim/copyinout.c
@@ -321,3 +321,30 @@ fuword32(const void *addr)
 {
 	return ((int32_t)fuword(addr));
 }
+
+intptr_t
+casuptr(intptr_t *addr, intptr_t old, intptr_t new)
+{
+	struct thread *td;
+	pmap_t pm;
+	faultbuf env;
+	intptr_t *p, val;
+
+	td = PCPU_GET(curthread);
+	pm = &td->td_proc->p_vmspace->vm_pmap;
+	p = (intptr_t *)((u_int)USER_ADDR + ((u_int)addr & ~SEGMENT_MASK));
+
+	set_user_sr(pm->pm_sr[(u_int)addr >> ADDR_SR_SHFT]);
+
+	if (setfault(env)) {
+		td->td_pcb->pcb_onfault = NULL;
+		return (-1);
+	}
+
+	val = *p;
+	(void) atomic_cmpset_32(p, old, new);
+
+	td->td_pcb->pcb_onfault = NULL;
+
+	return (val);
+}
diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index b19ce6e5faea..88ebf78a1a2f 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -150,6 +150,8 @@ void		install_extint(void (*)(void));
 
 int             setfault(faultbuf);             /* defined in locore.S */
 
+static int	grab_mcontext(struct thread *, mcontext_t *, int);
+
 void		asm_panic(char *);
 
 long		Maxmem = 0;
@@ -265,6 +267,33 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	}
 
 	/*
+	 * Init params/tunables that can be overridden by the loader
+	 */
+	init_param1();
+
+	/*
+	 * Start initializing proc0 and thread0.
+	 */
+	proc_linkup(&proc0, &ksegrp0, &kse0, &thread0);
+	proc0.p_uarea = (struct user *)uarea0;
+	proc0.p_stats = &proc0.p_uarea->u_stats;
+	thread0.td_frame = &frame0;
+
+	/*
+	 * Set up per-cpu data.
+	 */
+	pc = (struct pcpu *)(pcpu0 + PAGE_SIZE) - 1;
+	pcpu_init(pc, 0, sizeof(struct pcpu));
+	pc->pc_curthread = &thread0;
+	pc->pc_curpcb = thread0.td_pcb;
+	pc->pc_cpuid = 0;
+	/* pc->pc_mid = mid; */
+
+	__asm __volatile("mtsprg 0, %0" :: "r"(pc));
+
+	mutex_init();
+
+	/*
 	 * Initialize the console before printing anything.
 	 */
 	cninit();
@@ -304,28 +333,6 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	__syncicache(EXC_RSVD, EXC_LAST - EXC_RSVD);
 
 	/*
-	 * Start initializing proc0 and thread0.
-	 */
-	proc_linkup(&proc0, &ksegrp0, &kse0, &thread0);
-	proc0.p_uarea = (struct user *)uarea0;
-	proc0.p_stats = &proc0.p_uarea->u_stats;
-	thread0.td_frame = &frame0;
-
-	/*
-	 * Set up per-cpu data.
-	 */
-	pc = (struct pcpu *)(pcpu0 + PAGE_SIZE) - 1;
-	pcpu_init(pc, 0, sizeof(struct pcpu));
-	pc->pc_curthread = &thread0;
-	pc->pc_curpcb = thread0.td_pcb;
-	pc->pc_cpuid = 0;
-	/* pc->pc_mid = mid; */
-
-	__asm __volatile("mtsprg 0, %0" :: "r"(pc));
-
-	mutex_init();
-
-	/*
 	 * Make sure translation has been enabled
 	 */
 	mtmsr(mfmsr() | PSL_IR|PSL_DR|PSL_ME|PSL_RI);
@@ -336,9 +343,8 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	pmap_bootstrap(startkernel, endkernel);
 
 	/*
-	 * Initialize tunables.
+	 * Initialize params/tunables that are derived from memsize
 	 */
-	init_param1();
 	init_param2(physmem);
 
 	/*
@@ -421,16 +427,16 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 	 * Save user context
 	 */
 	memset(&sf, 0, sizeof(sf));
+	grab_mcontext(td, &sf.sf_uc.uc_mcontext, 0);
 	sf.sf_uc.uc_sigmask = *mask;
 	sf.sf_uc.uc_stack = td->td_sigstk;
 	sf.sf_uc.uc_stack.ss_flags = (td->td_pflags & TDP_ALTSTACK)
 	    ? ((oonstack) ? SS_ONSTACK : 0) : SS_DISABLE;
 
 	sf.sf_uc.uc_mcontext.mc_onstack = (oonstack) ? 1 : 0;
-	memcpy(&sf.sf_uc.uc_mcontext.mc_frame, tf, sizeof(struct trapframe));
 
 	/*
-	 * Allocate and validate space for the signal handler context. 
+	 * Allocate and validate space for the signal handler context.
 	 */
 	if ((td->td_pflags & TDP_ALTSTACK) != 0 && !oonstack &&
 	    SIGISMEMBER(psp->ps_sigonstack, sig)) {
@@ -440,14 +446,14 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 		sfp = (struct sigframe *)(tf->fixreg[1] - rndfsize);
 	}
 
-	/* 
+	/*
 	 * Translate the signal if appropriate (Linux emu ?)
 	 */
 	if (p->p_sysent->sv_sigtbl && sig <= p->p_sysent->sv_sigsize)
-		sig = p->p_sysent->sv_sigtbl[_SIG_IDX(sig)];       
+		sig = p->p_sysent->sv_sigtbl[_SIG_IDX(sig)];
 
 	/*
-	 * Save the floating-point state, if necessary, then copy it. 
+	 * Save the floating-point state, if necessary, then copy it.
 	 */
 	/* XXX */
 
@@ -466,7 +472,7 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 	tf->fixreg[FIRSTARG] = sig;
 	tf->fixreg[FIRSTARG+2] = (register_t)&sfp->sf_uc;
 	if (SIGISMEMBER(psp->ps_siginfo, sig)) {
-		/* 
+		/*
 		 * Signal handler installed with SA_SIGINFO.
 		 */
 		tf->fixreg[FIRSTARG+1] = (register_t)&sfp->sf_si;
@@ -498,7 +504,7 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 		sigexit(td, SIGILL);
 	}
 
-	CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td, 
+	CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td,
 	     tf->srr0, tf->fixreg[1]);
 
 	PROC_LOCK(p);
@@ -527,9 +533,9 @@ cpu_thread_siginfo(int sig, u_long code, siginfo_t *si)
 int
 sigreturn(struct thread *td, struct sigreturn_args *uap)
 {
-	struct trapframe *tf;
 	struct proc *p;
 	ucontext_t uc;
+	int error;
 
 	CTR2(KTR_SIG, "sigreturn: td=%p ucp=%p", td, uap->sigcntxp);
 
@@ -538,19 +544,9 @@ sigreturn(struct thread *td, struct sigreturn_args *uap)
 		return (EFAULT);
 	}
 
-	/*
-	 * Don't let the user set privileged MSR bits
-	 */
-	tf = td->td_frame;
-	if ((uc.uc_mcontext.mc_frame.srr1 & PSL_USERSTATIC) != 
-	    (tf->srr1 & PSL_USERSTATIC)) {
-		return (EINVAL);
-	}
-
-	/*
-	 * Restore the user-supplied context
-	 */
-	memcpy(tf, &uc.uc_mcontext.mc_frame, sizeof(struct trapframe));
+	error = set_mcontext(td, &uc.uc_mcontext);
+	if (error != 0)
+		return (error);
 
 	p = td->td_proc;
 	PROC_LOCK(p);
@@ -559,13 +555,8 @@ sigreturn(struct thread *td, struct sigreturn_args *uap)
 	signotify(td);
 	PROC_UNLOCK(p);
 
-	/*
-	 * Restore FP state
-	 */
-	/* XXX */
-
 	CTR3(KTR_SIG, "sigreturn: return td=%p pc=%#x sp=%#x",
-	     td, tf->srr0, tf->fixreg[1]);
+	     td, uc.uc_mcontext.mc_srr0, uc.uc_mcontext.mc_gpr[1]);
 
 	return (EJUSTRETURN);
 }
@@ -579,18 +570,101 @@ freebsd4_sigreturn(struct thread *td, struct freebsd4_sigreturn_args *uap)
 }
 #endif
 
+/*
+ * get_mcontext/sendsig helper routine that doesn't touch the
+ * proc lock
+ */
+static int
+grab_mcontext(struct thread *td, mcontext_t *mcp, int flags)
+{
+	struct pcb *pcb;
+
+	pcb = td->td_pcb;
+
+	memset(mcp, 0, sizeof(mcontext_t));
+
+	mcp->mc_vers = _MC_VERSION;
+	mcp->mc_flags = 0;
+	memcpy(&mcp->mc_frame, td->td_frame, sizeof(struct trapframe));
+	if (flags & GET_MC_CLEAR_RET) {
+		mcp->mc_gpr[3] = 0;
+		mcp->mc_gpr[4] = 0;
+	}
+
+	/*
+	 * This assumes that floating-point context is *not* lazy,
+	 * so if the thread has used FP there would have been a
+	 * FP-unavailable exception that would have set things up
+	 * correctly.
+	 */
+	if (pcb->pcb_flags & PCB_FPU) {
+		KASSERT(td == curthread,
+			("get_mcontext: fp save not curthread"));
+		critical_enter();
+		save_fpu(td);
+		critical_exit();
+		mcp->mc_flags |= _MC_FP_VALID;
+		memcpy(&mcp->mc_fpscr, &pcb->pcb_fpu.fpscr, sizeof(double));
+		memcpy(mcp->mc_fpreg, pcb->pcb_fpu.fpr, 32*sizeof(double));
+	}
+
+	/* XXX Altivec context ? */
+
+	mcp->mc_len = sizeof(*mcp);
+
+	return (0);
+}
+
 int
 get_mcontext(struct thread *td, mcontext_t *mcp, int flags)
 {
+	int error;
 
-	return (ENOSYS);
+	error = grab_mcontext(td, mcp, flags);
+	if (error == 0) {
+		PROC_LOCK(curthread->td_proc);
+		mcp->mc_onstack = sigonstack(td->td_frame->fixreg[1]);
+		PROC_UNLOCK(curthread->td_proc);
+	}
+
+	return (error);
 }
 
 int
 set_mcontext(struct thread *td, const mcontext_t *mcp)
 {
+	struct pcb *pcb;
+	struct trapframe *tf;
 
-	return (ENOSYS);
+	pcb = td->td_pcb;
+	tf = td->td_frame;
+
+	if (mcp->mc_vers != _MC_VERSION ||
+	    mcp->mc_len != sizeof(*mcp))
+		return (EINVAL);
+
+	/*
+	 * Don't let the user set privileged MSR bits
+	 */
+	if ((mcp->mc_srr1 & PSL_USERSTATIC) != (tf->srr1 & PSL_USERSTATIC)) {
+		return (EINVAL);
+	}
+
+	memcpy(tf, mcp->mc_frame, sizeof(mcp->mc_frame));
+
+	if (mcp->mc_flags & _MC_FP_VALID) {
+		if ((pcb->pcb_flags & PCB_FPU) != PCB_FPU) {
+			critical_enter();
+			enable_fpu(td);
+			critical_exit();
+		}
+		memcpy(&pcb->pcb_fpu.fpscr, &mcp->mc_fpscr, sizeof(double));
+		memcpy(pcb->pcb_fpu.fpr, mcp->mc_fpreg, 32*sizeof(double));
+	}
+
+	/* XXX Altivec context? */
+
+	return (0);
 }
 
 void
@@ -774,13 +848,6 @@ kcopy(const void *src, void *dst, size_t len)
 	return (0);
 }
 
-
-intptr_t
-casuptr(intptr_t *p, intptr_t old, intptr_t new)
-{
-	return (-1);
-}
-
 void
 asm_panic(char *pstr)
 {
diff --git a/sys/powerpc/aim/vm_machdep.c b/sys/powerpc/aim/vm_machdep.c
index 1d582f3bd3a9..857822ce0be5 100644
--- a/sys/powerpc/aim/vm_machdep.c
+++ b/sys/powerpc/aim/vm_machdep.c
@@ -45,17 +45,17 @@
  * All rights reserved.
  *
  * Author: Chris G. Demetriou
- * 
+ *
  * Permission to use, copy, modify and distribute this software and
  * its documentation is hereby granted, provided that both the copyright
  * notice and this permission notice appear in all copies of the
  * software, derivative works or modified versions, and any portions
  * thereof, and that both notices appear in supporting documentation.
- * 
- * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS" 
- * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND 
+ *
+ * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS"
+ * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND
  * FOR ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
- * 
+ *
  * Carnegie Mellon requests users of this software to return to
  *
  *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
@@ -275,7 +275,7 @@ sf_buf_alloc(struct vm_page *m)
 		sf_buf_alloc_want--;
 
 		/*
-		 * If we got a signal, don't risk going back to sleep. 
+		 * If we got a signal, don't risk going back to sleep.
 		 */
 		if (error)
 			break;
@@ -324,10 +324,10 @@ sf_buf_free(void *addr, void *args)
 
 /*
  * Software interrupt handler for queued VM system processing.
- */   
-void  
-swi_vm(void *dummy) 
-{     
+ */
+void
+swi_vm(void *dummy)
+{
 #if 0 /* XXX: Don't have busdma stuff yet */
 	if (busdma_swi_pending != 0)
 		busdma_swi();
@@ -358,18 +358,24 @@ is_physical_memory(addr)
  * KSE functions
  */
 void
-cpu_thread_exit(struct thread *td)     
+cpu_thread_exit(struct thread *td)
 {
 }
 
 void
-cpu_thread_clean(struct thread *td)     
+cpu_thread_clean(struct thread *td)
 {
 }
 
 void
 cpu_thread_setup(struct thread *td)
 {
+	struct pcb *pcb;
+
+	pcb = (struct pcb *)((td->td_kstack + KSTACK_PAGES * PAGE_SIZE -
+	    sizeof(struct pcb)) & ~0x2fU);
+	td->td_pcb = pcb;
+	td->td_frame = (struct trapframe *)pcb - 1;
 }
 
 void
@@ -385,9 +391,51 @@ cpu_thread_swapout(struct thread *td)
 void
 cpu_set_upcall(struct thread *td, struct thread *td0)
 {
+	struct pcb *pcb2;
+	struct trapframe *tf;
+	struct callframe *cf;
+
+	pcb2 = td->td_pcb;
+
+	/* Copy the upcall pcb */
+	bcopy(td0->td_pcb, pcb2, sizeof(*pcb2));
+
+	/* Create a stack for the new thread */
+	tf = td->td_frame;
+	bcopy(td0->td_frame, tf, sizeof(struct trapframe));
+	tf->fixreg[FIRSTARG] = 0;
+	tf->fixreg[FIRSTARG + 1] = 0;
+	tf->cr &= ~0x10000000;
+
+	/* Set registers for trampoline to user mode. */
+	cf = (struct callframe *)tf - 1;
+	cf->cf_func = (register_t)fork_return;
+	cf->cf_arg0 = (register_t)td;
+	cf->cf_arg1 = (register_t)tf;
+
+	pcb2->pcb_sp = (register_t)cf;
+	pcb2->pcb_lr = (register_t)fork_trampoline;
+	pcb2->pcb_usr = kernel_pmap->pm_sr[USER_SR];
 }
 
 void
 cpu_set_upcall_kse(struct thread *td, struct kse_upcall *ku)
 {
+        struct trapframe *tf;
+        uint32_t sp;
+
+	tf = td->td_frame;
+	/* align stack and alloc space for frame ptr and saved LR */
+        sp = ((uint32_t)ku->ku_stack.ss_sp + ku->ku_stack.ss_size
+		- 2*sizeof(u_int32_t)) & ~0x1f;
+	bzero(tf, sizeof(struct trapframe));
+
+	tf->fixreg[1] = (register_t)sp;
+        tf->fixreg[3] = (register_t)ku->ku_mailbox;
+        tf->srr0 = (register_t)ku->ku_func;
+        tf->srr1 = PSL_MBO | PSL_USERSET | PSL_FE_DFLT;
+        td->td_pcb->pcb_flags = 0;
+
+        td->td_retval[0] = (register_t)ku->ku_func;
+        td->td_retval[1] = 0;
 }
diff --git a/sys/powerpc/include/ucontext.h b/sys/powerpc/include/ucontext.h
index 28e50f345854..f053cd5df0f4 100644
--- a/sys/powerpc/include/ucontext.h
+++ b/sys/powerpc/include/ucontext.h
@@ -35,12 +35,38 @@
 #ifndef	_MACHINE_UCONTEXT_H_
 #define	_MACHINE_UCONTEXT_H_
 
-#include <machine/frame.h>
+typedef struct __mcontext {
+	int		mc_vers;
+	int		mc_flags;
+#define _MC_FP_VALID	0x01
+#define _MC_AV_VALID	0x02
+	int		mc_onstack;	  	/* saved onstack flag */
+	int		mc_len;			/* sizeof(__mcontext) */
+	uint64_t	mc_avec[32*2];		/* vector register file */
+	uint32_t	mc_av[2];
+	uint32_t	mc_frame[41];
+	uint64_t	mc_fpreg[33];
+} mcontext_t __aligned(16);
 
+/* GPRs and supervisor-level regs */
+#define mc_gpr		mc_frame
+#define mc_lr		mc_frame[32]
+#define mc_cr		mc_frame[33]
+#define mc_xer		mc_frame[34]
+#define	mc_ctr		mc_frame[35]
+#define mc_srr0		mc_frame[36]
+#define mc_srr1		mc_frame[37]
+#define mc_dar		mc_frame[38]
+#define mc_dsisr	mc_frame[39]
+#define mc_exc		mc_frame[40]
 
-typedef struct __mcontext {
-	int mc_onstack;			/* saved onstack flag */
-	struct trapframe mc_frame;	/* saved registers */
-} mcontext_t;
+/* floating-point state */
+#define mc_fpscr	mc_fpreg[32]
+
+/* altivec state */
+#define mc_vscr		mc_av[0]
+#define mc_vrsave	mc_av[1]
+
+#define _MC_VERSION	0x1
 
 #endif	/* !_MACHINE_UCONTEXT_H_ */
diff --git a/sys/powerpc/powerpc/copyinout.c b/sys/powerpc/powerpc/copyinout.c
index 5ef42caf4817..0f1dbc557f06 100644
--- a/sys/powerpc/powerpc/copyinout.c
+++ b/sys/powerpc/powerpc/copyinout.c
@@ -321,3 +321,30 @@ fuword32(const void *addr)
 {
 	return ((int32_t)fuword(addr));
 }
+
+intptr_t
+casuptr(intptr_t *addr, intptr_t old, intptr_t new)
+{
+	struct thread *td;
+	pmap_t pm;
+	faultbuf env;
+	intptr_t *p, val;
+
+	td = PCPU_GET(curthread);
+	pm = &td->td_proc->p_vmspace->vm_pmap;
+	p = (intptr_t *)((u_int)USER_ADDR + ((u_int)addr & ~SEGMENT_MASK));
+
+	set_user_sr(pm->pm_sr[(u_int)addr >> ADDR_SR_SHFT]);
+
+	if (setfault(env)) {
+		td->td_pcb->pcb_onfault = NULL;
+		return (-1);
+	}
+
+	val = *p;
+	(void) atomic_cmpset_32(p, old, new);
+
+	td->td_pcb->pcb_onfault = NULL;
+
+	return (val);
+}
diff --git a/sys/powerpc/powerpc/machdep.c b/sys/powerpc/powerpc/machdep.c
index b19ce6e5faea..88ebf78a1a2f 100644
--- a/sys/powerpc/powerpc/machdep.c
+++ b/sys/powerpc/powerpc/machdep.c
@@ -150,6 +150,8 @@ void		install_extint(void (*)(void));
 
 int             setfault(faultbuf);             /* defined in locore.S */
 
+static int	grab_mcontext(struct thread *, mcontext_t *, int);
+
 void		asm_panic(char *);
 
 long		Maxmem = 0;
@@ -265,6 +267,33 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	}
 
 	/*
+	 * Init params/tunables that can be overridden by the loader
+	 */
+	init_param1();
+
+	/*
+	 * Start initializing proc0 and thread0.
+	 */
+	proc_linkup(&proc0, &ksegrp0, &kse0, &thread0);
+	proc0.p_uarea = (struct user *)uarea0;
+	proc0.p_stats = &proc0.p_uarea->u_stats;
+	thread0.td_frame = &frame0;
+
+	/*
+	 * Set up per-cpu data.
+	 */
+	pc = (struct pcpu *)(pcpu0 + PAGE_SIZE) - 1;
+	pcpu_init(pc, 0, sizeof(struct pcpu));
+	pc->pc_curthread = &thread0;
+	pc->pc_curpcb = thread0.td_pcb;
+	pc->pc_cpuid = 0;
+	/* pc->pc_mid = mid; */
+
+	__asm __volatile("mtsprg 0, %0" :: "r"(pc));
+
+	mutex_init();
+
+	/*
 	 * Initialize the console before printing anything.
 	 */
 	cninit();
@@ -304,28 +333,6 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	__syncicache(EXC_RSVD, EXC_LAST - EXC_RSVD);
 
 	/*
-	 * Start initializing proc0 and thread0.
-	 */
-	proc_linkup(&proc0, &ksegrp0, &kse0, &thread0);
-	proc0.p_uarea = (struct user *)uarea0;
-	proc0.p_stats = &proc0.p_uarea->u_stats;
-	thread0.td_frame = &frame0;
-
-	/*
-	 * Set up per-cpu data.
-	 */
-	pc = (struct pcpu *)(pcpu0 + PAGE_SIZE) - 1;
-	pcpu_init(pc, 0, sizeof(struct pcpu));
-	pc->pc_curthread = &thread0;
-	pc->pc_curpcb = thread0.td_pcb;
-	pc->pc_cpuid = 0;
-	/* pc->pc_mid = mid; */
-
-	__asm __volatile("mtsprg 0, %0" :: "r"(pc));
-
-	mutex_init();
-
-	/*
 	 * Make sure translation has been enabled
 	 */
 	mtmsr(mfmsr() | PSL_IR|PSL_DR|PSL_ME|PSL_RI);
@@ -336,9 +343,8 @@ powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
 	pmap_bootstrap(startkernel, endkernel);
 
 	/*
-	 * Initialize tunables.
+	 * Initialize params/tunables that are derived from memsize
 	 */
-	init_param1();
 	init_param2(physmem);
 
 	/*
@@ -421,16 +427,16 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 	 * Save user context
 	 */
 	memset(&sf, 0, sizeof(sf));
+	grab_mcontext(td, &sf.sf_uc.uc_mcontext, 0);
 	sf.sf_uc.uc_sigmask = *mask;
 	sf.sf_uc.uc_stack = td->td_sigstk;
 	sf.sf_uc.uc_stack.ss_flags = (td->td_pflags & TDP_ALTSTACK)
 	    ? ((oonstack) ? SS_ONSTACK : 0) : SS_DISABLE;
 
 	sf.sf_uc.uc_mcontext.mc_onstack = (oonstack) ? 1 : 0;
-	memcpy(&sf.sf_uc.uc_mcontext.mc_frame, tf, sizeof(struct trapframe));
 
 	/*
-	 * Allocate and validate space for the signal handler context. 
+	 * Allocate and validate space for the signal handler context.
 	 */
 	if ((td->td_pflags & TDP_ALTSTACK) != 0 && !oonstack &&
 	    SIGISMEMBER(psp->ps_sigonstack, sig)) {
@@ -440,14 +446,14 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 		sfp = (struct sigframe *)(tf->fixreg[1] - rndfsize);
 	}
 
-	/* 
+	/*
 	 * Translate the signal if appropriate (Linux emu ?)
 	 */
 	if (p->p_sysent->sv_sigtbl && sig <= p->p_sysent->sv_sigsize)
-		sig = p->p_sysent->sv_sigtbl[_SIG_IDX(sig)];       
+		sig = p->p_sysent->sv_sigtbl[_SIG_IDX(sig)];
 
 	/*
-	 * Save the floating-point state, if necessary, then copy it. 
+	 * Save the floating-point state, if necessary, then copy it.
 	 */
 	/* XXX */
 
@@ -466,7 +472,7 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 	tf->fixreg[FIRSTARG] = sig;
 	tf->fixreg[FIRSTARG+2] = (register_t)&sfp->sf_uc;
 	if (SIGISMEMBER(psp->ps_siginfo, sig)) {
-		/* 
+		/*
 		 * Signal handler installed with SA_SIGINFO.
 		 */
 		tf->fixreg[FIRSTARG+1] = (register_t)&sfp->sf_si;
@@ -498,7 +504,7 @@ sendsig(sig_t catcher, int sig, sigset_t *mask, u_long code)
 		sigexit(td, SIGILL);
 	}
 
-	CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td, 
+	CTR3(KTR_SIG, "sendsig: return td=%p pc=%#x sp=%#x", td,
 	     tf->srr0, tf->fixreg[1]);
 
 	PROC_LOCK(p);
@@ -527,9 +533,9 @@ cpu_thread_siginfo(int sig, u_long code, siginfo_t *si)
 int
 sigreturn(struct thread *td, struct sigreturn_args *uap)
 {
-	struct trapframe *tf;
 	struct proc *p;
 	ucontext_t uc;
+	int error;
 
 	CTR2(KTR_SIG, "sigreturn: td=%p ucp=%p", td, uap->sigcntxp);
 
@@ -538,19 +544,9 @@ sigreturn(struct thread *td, struct sigreturn_args *uap)
 		return (EFAULT);
 	}
 
-	/*
-	 * Don't let the user set privileged MSR bits
-	 */
-	tf = td->td_frame;
-	if ((uc.uc_mcontext.mc_frame.srr1 & PSL_USERSTATIC) != 
-	    (tf->srr1 & PSL_USERSTATIC)) {
-		return (EINVAL);
-	}
-
-	/*
-	 * Restore the user-supplied context
-	 */
-	memcpy(tf, &uc.uc_mcontext.mc_frame, sizeof(struct trapframe));
+	error = set_mcontext(td, &uc.uc_mcontext);
+	if (error != 0)
+		return (error);
 
 	p = td->td_proc;
 	PROC_LOCK(p);
@@ -559,13 +555,8 @@ sigreturn(struct thread *td, struct sigreturn_args *uap)
 	signotify(td);
 	PROC_UNLOCK(p);
 
-	/*
-	 * Restore FP state
-	 */
-	/* XXX */
-
 	CTR3(KTR_SIG, "sigreturn: return td=%p pc=%#x sp=%#x",
-	     td, tf->srr0, tf->fixreg[1]);
+	     td, uc.uc_mcontext.mc_srr0, uc.uc_mcontext.mc_gpr[1]);
 
 	return (EJUSTRETURN);
 }
@@ -579,18 +570,101 @@ freebsd4_sigreturn(struct thread *td, struct freebsd4_sigreturn_args *uap)
 }
 #endif
 
+/*
+ * get_mcontext/sendsig helper routine that doesn't touch the
+ * proc lock
+ */
+static int
+grab_mcontext(struct thread *td, mcontext_t *mcp, int flags)
+{
+	struct pcb *pcb;
+
+	pcb = td->td_pcb;
+
+	memset(mcp, 0, sizeof(mcontext_t));
+
+	mcp->mc_vers = _MC_VERSION;
+	mcp->mc_flags = 0;
+	memcpy(&mcp->mc_frame, td->td_frame, sizeof(struct trapframe));
+	if (flags & GET_MC_CLEAR_RET) {
+		mcp->mc_gpr[3] = 0;
+		mcp->mc_gpr[4] = 0;
+	}
+
+	/*
+	 * This assumes that floating-point context is *not* lazy,
+	 * so if the thread has used FP there would have been a
+	 * FP-unavailable exception that would have set things up
+	 * correctly.
+	 */
+	if (pcb->pcb_flags & PCB_FPU) {
+		KASSERT(td == curthread,
+			("get_mcontext: fp save not curthread"));
+		critical_enter();
+		save_fpu(td);
+		critical_exit();
+		mcp->mc_flags |= _MC_FP_VALID;
+		memcpy(&mcp->mc_fpscr, &pcb->pcb_fpu.fpscr, sizeof(double));
+		memcpy(mcp->mc_fpreg, pcb->pcb_fpu.fpr, 32*sizeof(double));
+	}
+
+	/* XXX Altivec context ? */
+
+	mcp->mc_len = sizeof(*mcp);
+
+	return (0);
+}
+
 int
 get_mcontext(struct thread *td, mcontext_t *mcp, int flags)
 {
+	int error;
 
-	return (ENOSYS);
+	error = grab_mcontext(td, mcp, flags);
+	if (error == 0) {
+		PROC_LOCK(curthread->td_proc);
+		mcp->mc_onstack = sigonstack(td->td_frame->fixreg[1]);
+		PROC_UNLOCK(curthread->td_proc);
+	}
+
+	return (error);
 }
 
 int
 set_mcontext(struct thread *td, const mcontext_t *mcp)
 {
+	struct pcb *pcb;
+	struct trapframe *tf;
 
-	return (ENOSYS);
+	pcb = td->td_pcb;
+	tf = td->td_frame;
+
+	if (mcp->mc_vers != _MC_VERSION ||
+	    mcp->mc_len != sizeof(*mcp))
+		return (EINVAL);
+
+	/*
+	 * Don't let the user set privileged MSR bits
+	 */
+	if ((mcp->mc_srr1 & PSL_USERSTATIC) != (tf->srr1 & PSL_USERSTATIC)) {
+		return (EINVAL);
+	}
+
+	memcpy(tf, mcp->mc_frame, sizeof(mcp->mc_frame));
+
+	if (mcp->mc_flags & _MC_FP_VALID) {
+		if ((pcb->pcb_flags & PCB_FPU) != PCB_FPU) {
+			critical_enter();
+			enable_fpu(td);
+			critical_exit();
+		}
+		memcpy(&pcb->pcb_fpu.fpscr, &mcp->mc_fpscr, sizeof(double));
+		memcpy(pcb->pcb_fpu.fpr, mcp->mc_fpreg, 32*sizeof(double));
+	}
+
+	/* XXX Altivec context? */
+
+	return (0);
 }
 
 void
@@ -774,13 +848,6 @@ kcopy(const void *src, void *dst, size_t len)
 	return (0);
 }
 
-
-intptr_t
-casuptr(intptr_t *p, intptr_t old, intptr_t new)
-{
-	return (-1);
-}
-
 void
 asm_panic(char *pstr)
 {
diff --git a/sys/powerpc/powerpc/vm_machdep.c b/sys/powerpc/powerpc/vm_machdep.c
index 1d582f3bd3a9..857822ce0be5 100644
--- a/sys/powerpc/powerpc/vm_machdep.c
+++ b/sys/powerpc/powerpc/vm_machdep.c
@@ -45,17 +45,17 @@
  * All rights reserved.
  *
  * Author: Chris G. Demetriou
- * 
+ *
  * Permission to use, copy, modify and distribute this software and
  * its documentation is hereby granted, provided that both the copyright
  * notice and this permission notice appear in all copies of the
  * software, derivative works or modified versions, and any portions
  * thereof, and that both notices appear in supporting documentation.
- * 
- * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS" 
- * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND 
+ *
+ * CARNEGIE MELLON ALLOWS FREE USE OF THIS SOFTWARE IN ITS "AS IS"
+ * CONDITION.  CARNEGIE MELLON DISCLAIMS ANY LIABILITY OF ANY KIND
  * FOR ANY DAMAGES WHATSOEVER RESULTING FROM THE USE OF THIS SOFTWARE.
- * 
+ *
  * Carnegie Mellon requests users of this software to return to
  *
  *  Software Distribution Coordinator  or  Software.Distribution@CS.CMU.EDU
@@ -275,7 +275,7 @@ sf_buf_alloc(struct vm_page *m)
 		sf_buf_alloc_want--;
 
 		/*
-		 * If we got a signal, don't risk going back to sleep. 
+		 * If we got a signal, don't risk going back to sleep.
 		 */
 		if (error)
 			break;
@@ -324,10 +324,10 @@ sf_buf_free(void *addr, void *args)
 
 /*
  * Software interrupt handler for queued VM system processing.
- */   
-void  
-swi_vm(void *dummy) 
-{     
+ */
+void
+swi_vm(void *dummy)
+{
 #if 0 /* XXX: Don't have busdma stuff yet */
 	if (busdma_swi_pending != 0)
 		busdma_swi();
@@ -358,18 +358,24 @@ is_physical_memory(addr)
  * KSE functions
  */
 void
-cpu_thread_exit(struct thread *td)     
+cpu_thread_exit(struct thread *td)
 {
 }
 
 void
-cpu_thread_clean(struct thread *td)     
+cpu_thread_clean(struct thread *td)
 {
 }
 
 void
 cpu_thread_setup(struct thread *td)
 {
+	struct pcb *pcb;
+
+	pcb = (struct pcb *)((td->td_kstack + KSTACK_PAGES * PAGE_SIZE -
+	    sizeof(struct pcb)) & ~0x2fU);
+	td->td_pcb = pcb;
+	td->td_frame = (struct trapframe *)pcb - 1;
 }
 
 void
@@ -385,9 +391,51 @@ cpu_thread_swapout(struct thread *td)
 void
 cpu_set_upcall(struct thread *td, struct thread *td0)
 {
+	struct pcb *pcb2;
+	struct trapframe *tf;
+	struct callframe *cf;
+
+	pcb2 = td->td_pcb;
+
+	/* Copy the upcall pcb */
+	bcopy(td0->td_pcb, pcb2, sizeof(*pcb2));
+
+	/* Create a stack for the new thread */
+	tf = td->td_frame;
+	bcopy(td0->td_frame, tf, sizeof(struct trapframe));
+	tf->fixreg[FIRSTARG] = 0;
+	tf->fixreg[FIRSTARG + 1] = 0;
+	tf->cr &= ~0x10000000;
+
+	/* Set registers for trampoline to user mode. */
+	cf = (struct callframe *)tf - 1;
+	cf->cf_func = (register_t)fork_return;
+	cf->cf_arg0 = (register_t)td;
+	cf->cf_arg1 = (register_t)tf;
+
+	pcb2->pcb_sp = (register_t)cf;
+	pcb2->pcb_lr = (register_t)fork_trampoline;
+	pcb2->pcb_usr = kernel_pmap->pm_sr[USER_SR];
 }
 
 void
 cpu_set_upcall_kse(struct thread *td, struct kse_upcall *ku)
 {
+        struct trapframe *tf;
+        uint32_t sp;
+
+	tf = td->td_frame;
+	/* align stack and alloc space for frame ptr and saved LR */
+        sp = ((uint32_t)ku->ku_stack.ss_sp + ku->ku_stack.ss_size
+		- 2*sizeof(u_int32_t)) & ~0x1f;
+	bzero(tf, sizeof(struct trapframe));
+
+	tf->fixreg[1] = (register_t)sp;
+        tf->fixreg[3] = (register_t)ku->ku_mailbox;
+        tf->srr0 = (register_t)ku->ku_func;
+        tf->srr1 = PSL_MBO | PSL_USERSET | PSL_FE_DFLT;
+        td->td_pcb->pcb_flags = 0;
+
+        td->td_retval[0] = (register_t)ku->ku_func;
+        td->td_retval[1] = 0;
 }