 @@ -1,237 +1,242 @@
-/*	$NetBSD: spe.c,v 1.4 2011/05/02 06:43:16 matt Exp $	*/
+/*	$NetBSD: spe.c,v 1.5 2011/06/07 01:01:42 matt Exp $	*/
 /*-
  * Copyright (c) 2011 The NetBSD Foundation, Inc.
  * All rights reserved.
+ *
  * This code is derived from software contributed to The NetBSD Foundation
  * by Matt Thomas of 3am Software Foundry.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
  *    notice, this list of conditions and the following disclaimer in the
  *    documentation and/or other materials provided with the distribution.
+ *
  * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
  * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
  * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: spe.c,v 1.4 2011/05/02 06:43:16 matt Exp $");
+__KERNEL_RCSID(0, "$NetBSD: spe.c,v 1.5 2011/06/07 01:01:42 matt Exp $");
 #include "opt_altivec.h"
 #ifdef PPC_HAVE_SPE
 #include <sys/param.h>
 #include <sys/proc.h>
 #include <sys/systm.h>
 #include <sys/atomic.h>
 #include <sys/siginfo.h>
 #include <sys/pcu.h>
 #include <powerpc/altivec.h>
 #include <powerpc/spr.h>
 #include <powerpc/booke/spr.h>
 #include <powerpc/psl.h>
 #include <powerpc/pcb.h>
 static void vec_state_load(lwp_t *, bool);
 static void vec_state_save(lwp_t *);
 static void vec_state_release(lwp_t *);
 const pcu_ops_t vec_ops = {
 	.pcu_id = PCU_VEC,
 	.pcu_state_load = vec_state_load,
 	.pcu_state_save = vec_state_save,
 	.pcu_state_release = vec_state_release,
 };
 bool
 vec_used_p(lwp_t *l)
+{
 	return (l->l_md.md_flags & MDLWP_USEDVEC) != 0;
+}
 void
 vec_mark_used(lwp_t *l)
+{
 	l->l_md.md_flags |= MDLWP_USEDVEC;
+}
 void
 vec_state_load(lwp_t *l, bool used)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	if (__predict_false(!vec_used_p(l))) {
 		memset(&pcb->pcb_vr, 0, sizeof(pcb->pcb_vr));
 		vec_mark_used(l);
+	}
 	/*
 	 * Enable SPE temporarily (and disable interrupts).
 	 */
 	const register_t msr = mfmsr();
 	mtmsr((msr & ~PSL_EE) | PSL_SPV);
 	__asm volatile ("isync");
 	/*
 	 * Call an assembly routine to do load everything.
 	 */
 	vec_load_from_vreg(&pcb->pcb_vr);
 	__asm volatile ("sync");
 	/*
 	 * Restore MSR (turn off SPE)
 	 */
 	mtmsr(msr);
 	__asm volatile ("isync");
 	/*
 	 * Note that vector has now been used.
 	 */
 	l->l_md.md_flags |= MDLWP_USEDVEC;
 	l->l_md.md_utf->tf_srr1 |= PSL_SPV;
+}
 void
 vec_state_save(lwp_t *l)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	/*
 	 * Turn on SPE, turn off interrupts.
 	 */
 	const register_t msr = mfmsr();
 	mtmsr((msr & ~PSL_EE) | PSL_SPV);
 	__asm volatile ("isync");
 	/*
 	 * Save the vector state which is best done in assembly.
 	 */
 	vec_unload_to_vreg(&pcb->pcb_vr);
 	__asm volatile ("sync");
 	/*
 	 * Restore MSR (turn off SPE)
 	 */
 	mtmsr(msr);
 	__asm volatile ("isync");
+}
 void
 vec_state_release(lwp_t *l)
+{
 	/*
 	 * Turn off SPV so the next SPE instruction will cause a
 	 * SPE unavailable exception
 	 */
 	l->l_md.md_utf->tf_srr1 &= ~PSL_SPV;
+}
 void
 vec_restore_from_mcontext(lwp_t *l, const mcontext_t *mcp)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	const union __vr *vr = mcp->__vrf.__vrs;
 	KASSERT(l == curlwp);
 	vec_save();
 	/* grab the accumulator */
 	pcb->pcb_vr.vreg[8][0] = vr->__vr32[2];
 	pcb->pcb_vr.vreg[8][1] = vr->__vr32[3];
 	/*
 	 * We store the high parts of each register in the first 8 vectors.
 	 */
 	for (u_int i = 0; i < 8; i++, vr += 4) {
 		pcb->pcb_vr.vreg[i][0] = vr[0].__vr32[0];
 		pcb->pcb_vr.vreg[i][1] = vr[1].__vr32[0];
 		pcb->pcb_vr.vreg[i][2] = vr[2].__vr32[0];
 		pcb->pcb_vr.vreg[i][3] = vr[3].__vr32[0];
+	}
 	l->l_md.md_utf->tf_spefscr = pcb->pcb_vr.vscr = mcp->__vrf.__vscr;
 	pcb->pcb_vr.vrsave = mcp->__vrf.__vrsave;
+}
 bool
 vec_save_to_mcontext(lwp_t *l, mcontext_t *mcp, unsigned int *flagp)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	KASSERT(l == curlwp);
 	if (!vec_used_p(l))
 		return false;
 	vec_save();
 	mcp->__gregs[_REG_MSR] |= PSL_SPV;
 	union __vr *vr = mcp->__vrf.__vrs;
 	const register_t *fixreg = l->l_md.md_utf->tf_fixreg;
 	for (u_int i = 0; i < 32; i++, vr += 4, fixreg += 4) {
 		vr[0].__vr32[0] = pcb->pcb_vr.vreg[i][0];
 		vr[0].__vr32[1] = fixreg[0];
 		vr[0].__vr32[2] = 0;
 		vr[0].__vr32[3] = 0;
 		vr[1].__vr32[0] = pcb->pcb_vr.vreg[i][1];
 		vr[1].__vr32[1] = fixreg[1];
 		vr[1].__vr32[2] = 0;
 		vr[1].__vr32[3] = 0;
 		vr[2].__vr32[0] = pcb->pcb_vr.vreg[i][2];
 		vr[2].__vr32[1] = fixreg[2];
 		vr[2].__vr32[2] = 0;
 		vr[2].__vr32[3] = 0;
 		vr[3].__vr32[0] = pcb->pcb_vr.vreg[i][3];
 		vr[3].__vr32[1] = fixreg[3];
 		vr[3].__vr32[2] = 0;
 		vr[3].__vr32[3] = 0;
+	}
 	mcp->__vrf.__vrs[0].__vr32[2] = pcb->pcb_vr.vreg[8][0];
 	mcp->__vrf.__vrs[0].__vr32[3] = pcb->pcb_vr.vreg[8][1];
 	mcp->__vrf.__vrsave = pcb->pcb_vr.vrsave;
 	mcp->__vrf.__vscr = l->l_md.md_utf->tf_spefscr;
 	*flagp |= _UC_POWERPC_SPE;
 	return true;
+}
 static const struct {
 	uint32_t mask;
 	int code;
 } spefscr_siginfo_map[] = {
 	{ SPEFSCR_FINV|SPEFSCR_FINVH, FPE_FLTINV },
 	{ SPEFSCR_FOVF|SPEFSCR_FOVFH, FPE_FLTOVF },
 	{ SPEFSCR_FUNF|SPEFSCR_FUNFH, FPE_FLTUND },
 	{ SPEFSCR_FX  |SPEFSCR_FXH,   FPE_FLTRES },
 	{ SPEFSCR_FDBZ|SPEFSCR_FDBZH, FPE_FLTDIV },
 	{ SPEFSCR_OV  |SPEFSCR_OVH,   FPE_INTOVF },
 };
 int
 vec_siginfo_code(const struct trapframe *tf)
+{
 	for (u_int i = 0; i < __arraycount(spefscr_siginfo_map); i++) {
 		if (tf->tf_spefscr & spefscr_siginfo_map[i].mask)
 			return spefscr_siginfo_map[i].code;
+	}
 	return 0;
+}
 #endif /* PPC_HAVE_SPE */

 @@ -1,314 +1,319 @@
-/*	$NetBSD: altivec.c,v 1.24 2011/05/25 05:42:37 matt Exp $	*/
+/*	$NetBSD: altivec.c,v 1.25 2011/06/07 01:01:43 matt Exp $	*/
 /*
  * Copyright (C) 1996 Wolfgang Solfrank.
  * Copyright (C) 1996 TooLs GmbH.
  * All rights reserved.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
  *    notice, this list of conditions and the following disclaimer in the
  *    documentation and/or other materials provided with the distribution.
  * 3. All advertising materials mentioning features or use of this software
  *    must display the following acknowledgement:
  *	This product includes software developed by TooLs GmbH.
  * 4. The name of TooLs GmbH may not be used to endorse or promote products
  *    derived from this software without specific prior written permission.
+ *
  * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``AS IS'' AND ANY EXPRESS OR
  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  * IN NO EVENT SHALL TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: altivec.c,v 1.24 2011/05/25 05:42:37 matt Exp $");
+__KERNEL_RCSID(0, "$NetBSD: altivec.c,v 1.25 2011/06/07 01:01:43 matt Exp $");
 #include "opt_multiprocessor.h"
 #include <sys/param.h>
 #include <sys/proc.h>
 #include <sys/systm.h>
 #include <sys/atomic.h>
 #include <uvm/uvm_extern.h>		/*  for vcopypage/vzeropage */
 #include <powerpc/pcb.h>
 #include <powerpc/altivec.h>
 #include <powerpc/spr.h>
 #include <powerpc/oea/spr.h>
 #include <powerpc/psl.h>
 static void vec_state_load(lwp_t *, bool);
 static void vec_state_save(lwp_t *);
 static void vec_state_release(lwp_t *);
 const pcu_ops_t vec_ops = {
 	.pcu_id = PCU_VEC,
 	.pcu_state_load = vec_state_load,
 	.pcu_state_save = vec_state_save,
 	.pcu_state_release = vec_state_release,
 };
 bool
 vec_used_p(lwp_t *l)
+{
 	return (l->l_md.md_flags & MDLWP_USEDVEC) != 0;
+}
 void
 vec_mark_used(lwp_t *l)
+{
 	l->l_md.md_flags |= MDLWP_USEDVEC;
+}
 void
 vec_state_load(lwp_t *l, bool used)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	if (__predict_false(!vec_used_p(l))) {
 		memset(&pcb->pcb_vr, 0, sizeof(pcb->pcb_vr));
 		vec_mark_used(l);
+	}
 	/*
 	 * Enable AltiVec temporarily (and disable interrupts).
 	 */
 	const register_t msr = mfmsr();
 	mtmsr((msr & ~PSL_EE) | PSL_VEC);
 	__asm volatile ("isync");
 	/*
 	 * Load the vector unit from vreg which is best done in
 	 * assembly.
 	 */
 	vec_load_from_vreg(&pcb->pcb_vr);
 	/*
 	 * VRSAVE will be restored when trap frame returns
 	 */
 	l->l_md.md_utf->tf_vrsave = pcb->pcb_vr.vrsave;
 	/*
 	 * Restore MSR (turn off AltiVec)
 	 */
 	mtmsr(msr);
 	__asm volatile ("isync");
 	/*
 	 * Mark vector registers as modified.
 	 */
 	l->l_md.md_flags |= MDLWP_USEDVEC|PSL_VEC;
 	l->l_md.md_utf->tf_srr1 |= PSL_VEC;
+}
 void
 vec_state_save(lwp_t *l)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	/*
 	 * Turn on AltiVEC, turn off interrupts.
 	 */
 	const register_t msr = mfmsr();
 	mtmsr((msr & ~PSL_EE) | PSL_VEC);
 	__asm volatile ("isync");
 	/*
 	 * Grab contents of vector unit.
 	 */
 	vec_unload_to_vreg(&pcb->pcb_vr);
 	/*
 	 * Save VRSAVE
 	 */
 	pcb->pcb_vr.vrsave = l->l_md.md_utf->tf_vrsave;
 	/*
 	 * Note that we aren't using any CPU resources and stop any
 	 * data streams.
 	 */
 	__asm volatile ("dssall; sync");
 	/*
 	 * Restore MSR (turn off AltiVec)
 	 */
 	mtmsr(msr);
 	__asm volatile ("isync");
+}
 void
 vec_state_release(lwp_t *l)
+{
 	__asm volatile("dssall;sync");
 	l->l_md.md_utf->tf_srr1 &= ~PSL_VEC;
 	l->l_md.md_flags &= ~PSL_VEC;
+}
 void
 vec_restore_from_mcontext(struct lwp *l, const mcontext_t *mcp)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	KASSERT(l == curlwp);
 	/* we don't need to save the state, just drop it */
 	pcu_discard(&vec_ops);
 	memcpy(pcb->pcb_vr.vreg, &mcp->__vrf.__vrs, sizeof (pcb->pcb_vr.vreg));
 	pcb->pcb_vr.vscr = mcp->__vrf.__vscr;
 	pcb->pcb_vr.vrsave = mcp->__vrf.__vrsave;
 	l->l_md.md_utf->tf_vrsave = pcb->pcb_vr.vrsave;
+}
 bool
 vec_save_to_mcontext(struct lwp *l, mcontext_t *mcp, unsigned int *flagp)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	KASSERT(l == curlwp);
 	/* Save AltiVec context, if any. */
 	if (!vec_used_p(l))
 		return false;
 	/*
 	 * If we're the AltiVec owner, dump its context to the PCB first.
 	 */
 	pcu_save(&vec_ops);
 	mcp->__gregs[_REG_MSR] |= PSL_VEC;
 	mcp->__vrf.__vscr = pcb->pcb_vr.vscr;
 	mcp->__vrf.__vrsave = l->l_md.md_utf->tf_vrsave;
 	memcpy(mcp->__vrf.__vrs, pcb->pcb_vr.vreg, sizeof (mcp->__vrf.__vrs));
 	*flagp |= _UC_POWERPC_VEC;
 	return true;
+}
 #define ZERO_VEC	19
 void
 vzeropage(paddr_t pa)
+{
 	const paddr_t ea = pa + PAGE_SIZE;
 	uint32_t vec[7], *vp = (void *) roundup((uintptr_t) vec, 16);
 	register_t omsr, msr;
 	__asm volatile("mfmsr %0" : "=r"(omsr) :);
 	/*
 	 * Turn on AltiVec, turn off interrupts.
 	 */
 	msr = (omsr & ~PSL_EE) | PSL_VEC;
 	__asm volatile("sync; mtmsr %0; isync" :: "r"(msr));
 	/*
 	 * Save the VEC register we are going to use before we disable
 	 * relocation.
 	 */
 	__asm("stvx %1,0,%0" :: "r"(vp), "n"(ZERO_VEC));
 	__asm("vxor %0,%0,%0" :: "n"(ZERO_VEC));
 	/*
 	 * Zero the page using a single cache line.
 	 */
 	__asm volatile(
 	    "   sync ;"
 	    "   mfmsr  %[msr];"
 	    "   rlwinm %[msr],%[msr],0,28,26;"	/* Clear PSL_DR */
 	    "   mtmsr  %[msr];"			/* Turn off DMMU */
 	    "   isync;"
 	    "1: stvx   %[zv], %[pa], %[off0];"
 	    "   stvxl  %[zv], %[pa], %[off16];"
 	    "   stvx   %[zv], %[pa], %[off32];"
 	    "   stvxl  %[zv], %[pa], %[off48];"
 	    "   addi   %[pa], %[pa], 64;"
 	    "   cmplw  %[pa], %[ea];"
 	    "	blt+   1b;"
 	    "   ori    %[msr], %[msr], 0x10;"	/* Set PSL_DR */
 	    "   sync;"
 	    "	mtmsr  %[msr];"			/* Turn on DMMU */
 	    "   isync;"
 	    :: [msr] "r"(msr), [pa] "b"(pa), [ea] "b"(ea),
 	    [off0] "r"(0), [off16] "r"(16), [off32] "r"(32), [off48] "r"(48),
 	    [zv] "n"(ZERO_VEC));
 	/*
 	 * Restore VEC register (now that we can access the stack again).
 	 */
 	__asm("lvx %1,0,%0" :: "r"(vp), "n"(ZERO_VEC));
 	/*
 	 * Restore old MSR (AltiVec OFF).
 	 */
 	__asm volatile("sync; mtmsr %0; isync" :: "r"(omsr));
+}
 #define LO_VEC	16
 #define HI_VEC	17
 void
 vcopypage(paddr_t dst, paddr_t src)
+{
 	const paddr_t edst = dst + PAGE_SIZE;
 	uint32_t vec[11], *vp = (void *) roundup((uintptr_t) vec, 16);
 	register_t omsr, msr;
 	__asm volatile("mfmsr %0" : "=r"(omsr) :);
 	/*
 	 * Turn on AltiVec, turn off interrupts.
 	 */
 	msr = (omsr & ~PSL_EE) | PSL_VEC;
 	__asm volatile("sync; mtmsr %0; isync" :: "r"(msr));
 	/*
 	 * Save the VEC registers we will be using before we disable
 	 * relocation.
 	 */
 	__asm("stvx %2,%1,%0" :: "b"(vp), "r"( 0), "n"(LO_VEC));
 	__asm("stvx %2,%1,%0" :: "b"(vp), "r"(16), "n"(HI_VEC));
 	/*
 	 * Copy the page using a single cache line, with DMMU
 	 * disabled.  On most PPCs, two vector registers occupy one
 	 * cache line.
 	 */
 	__asm volatile(
 	    "   sync ;"
 	    "   mfmsr  %[msr];"
 	    "   rlwinm %[msr],%[msr],0,28,26;"	/* Clear PSL_DR */
 	    "   mtmsr  %[msr];"			/* Turn off DMMU */
 	    "   isync;"
 	    "1: lvx    %[lv], %[src], %[off0];"
 	    "   stvx   %[lv], %[dst], %[off0];"
 	    "   lvxl   %[hv], %[src], %[off16];"
 	    "   stvxl  %[hv], %[dst], %[off16];"
 	    "   addi   %[src], %[src], 32;"
 	    "   addi   %[dst], %[dst], 32;"
 	    "   cmplw  %[dst], %[edst];"
 	    "	blt+   1b;"
 	    "   ori    %[msr], %[msr], 0x10;"	/* Set PSL_DR */
 	    "   sync;"
 	    "	mtmsr  %[msr];"			/* Turn on DMMU */
 	    "   isync;"
 	    :: [msr] "r"(msr), [src] "b"(src), [dst] "b"(dst),
 	    [edst] "b"(edst), [off0] "r"(0), [off16] "r"(16),
 	    [lv] "n"(LO_VEC), [hv] "n"(HI_VEC));
 	/*
 	 * Restore VEC registers (now that we can access the stack again).
 	 */
 	__asm("lvx %2,%1,%0" :: "b"(vp), "r"( 0), "n"(LO_VEC));
 	__asm("lvx %2,%1,%0" :: "b"(vp), "r"(16), "n"(HI_VEC));
 	/*
 	 * Restore old MSR (AltiVec OFF).
 	 */
 	__asm volatile("sync; mtmsr %0; isync" :: "r"(omsr));
+}

 @@ -1,249 +1,250 @@
-/*	$NetBSD: fpu.c,v 1.30 2011/06/07 00:48:32 matt Exp $	*/
+/*	$NetBSD: fpu.c,v 1.31 2011/06/07 01:01:43 matt Exp $	*/
 /*
  * Copyright (C) 1996 Wolfgang Solfrank.
  * Copyright (C) 1996 TooLs GmbH.
  * All rights reserved.
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  * 1. Redistributions of source code must retain the above copyright
  *    notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
  *    notice, this list of conditions and the following disclaimer in the
  *    documentation and/or other materials provided with the distribution.
  * 3. All advertising materials mentioning features or use of this software
  *    must display the following acknowledgement:
  *	This product includes software developed by TooLs GmbH.
  * 4. The name of TooLs GmbH may not be used to endorse or promote products
  *    derived from this software without specific prior written permission.
+ *
  * THIS SOFTWARE IS PROVIDED BY TOOLS GMBH ``AS IS'' AND ANY EXPRESS OR
  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  * IN NO EVENT SHALL TOOLS GMBH BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
  * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 #include <sys/cdefs.h>
-__KERNEL_RCSID(0, "$NetBSD: fpu.c,v 1.30 2011/06/07 00:48:32 matt Exp $");
+__KERNEL_RCSID(0, "$NetBSD: fpu.c,v 1.31 2011/06/07 01:01:43 matt Exp $");
 #include "opt_multiprocessor.h"
 #include <sys/param.h>
 #include <sys/proc.h>
 #include <sys/systm.h>
 #include <sys/atomic.h>
 #include <sys/siginfo.h>
 #include <sys/pcu.h>
 #include <machine/pcb.h>
 #include <machine/fpu.h>
 #include <machine/psl.h>
 #ifdef PPC_HAVE_FPU
 static void fpu_state_load(lwp_t *, bool);
 static void fpu_state_save(lwp_t *);
 static void fpu_state_release(lwp_t *);
 #endif
 const pcu_ops_t fpu_ops = {
 	.pcu_id = PCU_FPU,
 #ifdef PPC_HAVE_FPU
 	.pcu_state_load = fpu_state_load,
 	.pcu_state_save = fpu_state_save,
 	.pcu_state_release = fpu_state_release,
 #endif
 };
 bool
 fpu_used_p(lwp_t *l)
+{
 	return (l->l_md.md_flags & MDLWP_USEDFPU) != 0;
+}
 void
 fpu_mark_used(lwp_t *l)
+{
 	l->l_md.md_flags |= MDLWP_USEDFPU;
+}
 #ifdef PPC_HAVE_FPU
 void
 fpu_state_load(lwp_t *l, bool used)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
-	if (__predict_false(!used)) {
+	if (__predict_false(!fpu_used_p(l))) {
 		memset(&pcb->pcb_fpu, 0, sizeof(pcb->pcb_fpu));
 		fpu_mark_used(l);
+	}
 	const register_t msr = mfmsr();
         mtmsr((msr & ~PSL_EE) | PSL_FP);
 	__asm volatile ("isync");
 	fpu_load_from_fpreg(&pcb->pcb_fpu);
 	__asm volatile ("sync");
 	mtmsr(msr);
 	__asm volatile ("isync");
 	curcpu()->ci_ev_fpusw.ev_count++;
 	l->l_md.md_utf->tf_srr1 |= PSL_FP|(pcb->pcb_flags & (PCB_FE0|PCB_FE1));
 	l->l_md.md_flags |= MDLWP_USEDFPU;
+}
 /*
  * Save the contents of the current CPU's FPU to its PCB.
  */
 void
 fpu_state_save(lwp_t *l)
+{
 	struct pcb * const pcb = lwp_getpcb(l);
 	const register_t msr = mfmsr();
         mtmsr((msr & ~PSL_EE) | PSL_FP);
 	__asm volatile ("isync");
 	fpu_unload_to_fpreg(&pcb->pcb_fpu);
 	__asm volatile ("sync");
 	mtmsr(msr);
 	__asm volatile ("isync");
+}
 void
 fpu_state_release(lwp_t *l)
+{
 	l->l_md.md_utf->tf_srr1 &= ~PSL_FP;
+}
 #define	STICKYBITS	(FPSCR_VX|FPSCR_OX|FPSCR_UX|FPSCR_ZX|FPSCR_XX)
 #define	STICKYSHIFT	25
 #define	MASKBITS	(FPSCR_VE|FPSCR_OE|FPSCR_UE|FPSCR_ZE|FPSCR_XE)
 #define	MASKSHIFT	3
 int
 fpu_get_fault_code(void)
+{
 	lwp_t * const l = curlwp;
 	struct pcb * const pcb = lwp_getpcb(l);
 	uint64_t fpscr64;
 	uint32_t fpscr, ofpscr;
 	int code;
 	int s = splsoftclock();	/* disable preemption */
 	struct cpu_info * const ci = curcpu();
 	/*
 	 * If we got preempted, we may be running on a different CPU.  So we
 	 * need to check for that.
 	 */
 	KASSERT(fpu_used_p(l));
 	if (__predict_true(l->l_pcu_cpu[PCU_FPU] == ci)) {
 		uint64_t tmp;
 		const register_t msr = mfmsr();
 		mtmsr((msr & ~PSL_EE) | PSL_FP);
 		__asm volatile ("isync");
 		__asm volatile (
 			"stfd	0,0(%[tmp])\n"		/* save f0 */
 			"mffs	0\n"			/* get FPSCR */
 			"stfd	0,0(%[fpscr64])\n"	/* store a temp copy */
 			"mtfsb0	0\n"			/* clear FPSCR_FX */
 			"mtfsb0	24\n"			/* clear FPSCR_VE */
 			"mtfsb0	25\n"			/* clear FPSCR_OE */
 			"mtfsb0	26\n"			/* clear FPSCR_UE */
 			"mtfsb0	27\n"			/* clear FPSCR_ZE */
 			"mtfsb0	28\n"			/* clear FPSCR_XE */
 			"mffs	0\n"			/* get FPSCR */
 			"stfd	0,0(%[fpscr])\n"	/* store it */
 			"lfd	0,0(%[tmp])\n"		/* restore f0 */
 		    ::	[tmp] "b"(&tmp),
 			[fpscr] "b"(&pcb->pcb_fpu.fpscr),
 			[fpscr64] "b"(&fpscr64));
 		mtmsr(msr);
 		__asm volatile ("isync");
 	} else {
 		/*
 		 * We got preempted to a different CPU so we need to save
 		 * our FPU state.
 		 */
 		fpu_save();
 		fpscr64 = *(uint64_t *)&pcb->pcb_fpu.fpscr;
 		((uint32_t *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD] &= ~MASKBITS;
+	}
 	splx(s);	/* allow preemption */
 	/*
 	 * Now determine the fault type.  First we test to see if any of sticky
 	 * bits correspond to the enabled exceptions.  If so, we only test
 	 * those bits.  If not, we look at all the bits.  (In reality, we only
 	 * could get an exception if FPSCR_FEX changed state.  So we should
 	 * have at least one bit that corresponds).
 	 */
 	ofpscr = (uint32_t)fpscr64;
 	ofpscr &= ofpscr << (STICKYSHIFT - MASKSHIFT);
 	fpscr = ((uint32_t *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD];
 	if (fpscr & ofpscr & STICKYBITS)
 		fpscr &= ofpscr;
 	/*
 	 * Let's determine what the appropriate code is.
 	 */
 	if (fpscr & FPSCR_VX)		code = FPE_FLTINV;
 	else if (fpscr & FPSCR_OX)	code = FPE_FLTOVF;
 	else if (fpscr & FPSCR_UX)	code = FPE_FLTUND;
 	else if (fpscr & FPSCR_ZX)	code = FPE_FLTDIV;
 	else if (fpscr & FPSCR_XX)	code = FPE_FLTRES;
 	else				code = 0;
 	return code;
+}
 #endif /* PPC_HAVE_FPU */
 bool
 fpu_save_to_mcontext(lwp_t *l, mcontext_t *mcp, unsigned int *flagp)
+{
 	KASSERT(l == curlwp);
 	if (!pcu_used_p(&fpu_ops))
 		return false;
 	struct pcb * const pcb = lwp_getpcb(l);
 #ifdef PPC_HAVE_FPU
 	/* If we're the FPU owner, dump its context to the PCB first. */
 	pcu_save(&fpu_ops);
 #endif
 	(void)memcpy(mcp->__fpregs.__fpu_regs, pcb->pcb_fpu.fpreg,
 	    sizeof (mcp->__fpregs.__fpu_regs));
 	mcp->__fpregs.__fpu_fpscr =
 	    ((int *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD];
 	mcp->__fpregs.__fpu_valid = 1;
 	*flagp |= _UC_FPU;
 	return true;
+}
 void
 fpu_restore_from_mcontext(lwp_t *l, const mcontext_t *mcp)
+{
 	if (!mcp->__fpregs.__fpu_valid)
 		return;
 	struct pcb * const pcb = lwp_getpcb(l);
 #ifdef PPC_HAVE_FPU
 	/* we don't need to save the state, just drop it */
 	if (l == curlwp)
 		pcu_discard(&fpu_ops);
 #endif
 	(void)memcpy(&pcb->pcb_fpu.fpreg, &mcp->__fpregs.__fpu_regs,
 	    sizeof (pcb->pcb_fpu.fpreg));
 	((int *)&pcb->pcb_fpu.fpscr)[_QUAD_LOWWORD] = mcp->__fpregs.__fpu_fpscr;
+}