Skip site navigation (1)Skip section navigation (2)
Date:      Sat, 7 Mar 2009 20:39:42 +0000 (UTC)
From:      Nathan Whitehorn <nwhitehorn@FreeBSD.org>
To:        src-committers@freebsd.org, svn-src-all@freebsd.org, svn-src-stable@freebsd.org, svn-src-stable-7@freebsd.org
Subject:   svn commit: r189498 - in stable/7: share/mk sys sys/conf sys/contrib/pf sys/dev/ath/ath_hal sys/dev/cxgb sys/powerpc/include sys/powerpc/powerpc
Message-ID:  <200903072039.n27Kdg2q062234@svn.freebsd.org>

next in thread | raw e-mail | index | archive | help
Author: nwhitehorn
Date: Sat Mar  7 20:39:42 2009
New Revision: 189498
URL: http://svn.freebsd.org/changeset/base/189498

Log:
  MFC r188860,188879,188923,188951:
  
  Altivec support for PowerPC.

Added:
  stable/7/sys/powerpc/include/altivec.h
     - copied unchanged from r188860, head/sys/powerpc/include/altivec.h
  stable/7/sys/powerpc/powerpc/altivec.c
     - copied unchanged from r188860, head/sys/powerpc/powerpc/altivec.c
Modified:
  stable/7/share/mk/   (props changed)
  stable/7/share/mk/bsd.cpu.mk
  stable/7/sys/   (props changed)
  stable/7/sys/conf/files.powerpc
  stable/7/sys/conf/kern.mk
  stable/7/sys/contrib/pf/   (props changed)
  stable/7/sys/dev/ath/ath_hal/   (props changed)
  stable/7/sys/dev/cxgb/   (props changed)
  stable/7/sys/powerpc/include/pcb.h
  stable/7/sys/powerpc/include/pcpu.h
  stable/7/sys/powerpc/powerpc/genassym.c
  stable/7/sys/powerpc/powerpc/machdep.c
  stable/7/sys/powerpc/powerpc/swtch.S
  stable/7/sys/powerpc/powerpc/trap.c
  stable/7/sys/powerpc/powerpc/trap_subr.S

Modified: stable/7/share/mk/bsd.cpu.mk
==============================================================================
--- stable/7/share/mk/bsd.cpu.mk	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/share/mk/bsd.cpu.mk	Sat Mar  7 20:39:42 2009	(r189498)
@@ -109,6 +109,8 @@ _CPUCFLAGS = -march=armv5te -D__XSCALE__
 .  else
 _CPUCFLAGS = -mcpu=${CPUTYPE}
 .  endif
+. elif ${MACHINE_ARCH} == "powerpc"
+_CPUCFLAGS = -mcpu=${CPUTYPE}
 . endif
 
 # Set up the list of CPU features based on the CPU type.  This is an

Modified: stable/7/sys/conf/files.powerpc
==============================================================================
--- stable/7/sys/conf/files.powerpc	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/conf/files.powerpc	Sat Mar  7 20:39:42 2009	(r189498)
@@ -34,6 +34,7 @@ dev/syscons/scterm-sc.c		optional	sc
 dev/syscons/scvtb.c		optional	sc
 dev/uart/uart_cpu_powerpc.c	optional	uart
 kern/syscalls.c			optional	ktr
+powerpc/powerpc/altivec.c	standard
 powerpc/powerpc/atomic.S	standard
 powerpc/powerpc/autoconf.c	standard
 powerpc/powerpc/bcopy.c		standard

Modified: stable/7/sys/conf/kern.mk
==============================================================================
--- stable/7/sys/conf/kern.mk	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/conf/kern.mk	Sat Mar  7 20:39:42 2009	(r189498)
@@ -78,9 +78,10 @@ INLINE_LIMIT?=	8000
 #
 # For PowerPC we tell gcc to use floating point emulation.  This avoids using
 # floating point registers for integer operations which it has a tendency to do.
+# Also explicitly disable Altivec instructions inside the kernel.
 #
 .if ${MACHINE_ARCH} == "powerpc"
-CFLAGS+=	-msoft-float
+CFLAGS+=	-msoft-float -mno-altivec
 INLINE_LIMIT?=	15000
 .endif
 

Copied: stable/7/sys/powerpc/include/altivec.h (from r188860, head/sys/powerpc/include/altivec.h)
==============================================================================
--- /dev/null	00:00:00 1970	(empty, because file is newly added)
+++ stable/7/sys/powerpc/include/altivec.h	Sat Mar  7 20:39:42 2009	(r189498, copy of r188860, head/sys/powerpc/include/altivec.h)
@@ -0,0 +1,36 @@
+/*-
+ * Copyright (c) 2008 Nathan Whitehorn
+ * 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.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.
+ *
+ * $FreeBSD$
+ */
+
+#ifndef	_MACHINE_ALTIVEC_H_
+#define	_MACHINE_ALTIVEC_H_
+
+void    enable_vec(struct thread *);
+void    save_vec(struct thread *);
+
+#endif	/* _MACHINE_ALTIVEC_H_ */
+

Modified: stable/7/sys/powerpc/include/pcb.h
==============================================================================
--- stable/7/sys/powerpc/include/pcb.h	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/include/pcb.h	Sat Mar  7 20:39:42 2009	(r189498)
@@ -49,12 +49,21 @@ struct pcb {
 						    copyin/copyout */
 	int		pcb_flags;
 #define	PCB_FPU		1	/* Process had FPU initialized */
+#define	PCB_VEC		2	/* Process had Altivec initialized */
 	struct fpu {
 		double	fpr[32];
 		double	fpscr;	/* FPSCR stored as double for easier access */
 	} pcb_fpu;		/* Floating point processor */
 	unsigned int	pcb_fpcpu;		/* which CPU had our FPU
 							stuff. */
+	struct vec {
+		uint32_t vr[32][4];
+		register_t vrsave;
+		register_t spare[2];
+		register_t vscr;
+	} pcb_vec __attribute__((aligned(16)));	/* Vector processor */
+	unsigned int	pcb_veccpu;		/* which CPU had our vector
+							stuff. */
 };
 
 #ifdef	_KERNEL

Modified: stable/7/sys/powerpc/include/pcpu.h
==============================================================================
--- stable/7/sys/powerpc/include/pcpu.h	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/include/pcpu.h	Sat Mar  7 20:39:42 2009	(r189498)
@@ -39,6 +39,7 @@ struct pmap;
 	int		pc_inside_intr;					\
 	struct pmap	*pc_curpmap;		/* current pmap */	\
         struct thread   *pc_fputhread;          /* current fpu user */  \
+	struct thread	*pc_vecthread;		/* current vec user */  \
 	register_t	pc_tempsave[CPUSAVE_LEN];			\
 	register_t	pc_disisave[CPUSAVE_LEN];			\
 	register_t	pc_dbsave[CPUSAVE_LEN];

Copied: stable/7/sys/powerpc/powerpc/altivec.c (from r188860, head/sys/powerpc/powerpc/altivec.c)
==============================================================================
--- /dev/null	00:00:00 1970	(empty, because file is newly added)
+++ stable/7/sys/powerpc/powerpc/altivec.c	Sat Mar  7 20:39:42 2009	(r189498, copy of r188860, head/sys/powerpc/powerpc/altivec.c)
@@ -0,0 +1,152 @@
+/*-
+ * 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.
+ *
+ *	$NetBSD: fpu.c,v 1.5 2001/07/22 11:29:46 wiz Exp $
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <sys/param.h>
+#include <sys/proc.h>
+#include <sys/systm.h>
+#include <sys/limits.h>
+
+#include <machine/altivec.h>
+#include <machine/pcb.h>
+#include <machine/psl.h>
+
+void
+enable_vec(struct thread *td)
+{
+	int	msr;
+	struct	pcb *pcb;
+	struct	trapframe *tf;
+
+	pcb = td->td_pcb;
+	tf = trapframe(td);
+
+	/*
+	 * Save the thread's Altivec CPU number, and set the CPU's current
+	 * vector thread
+	 */
+	td->td_pcb->pcb_veccpu = PCPU_GET(cpuid);
+	PCPU_SET(vecthread, td);
+
+	/*
+	 * Enable the vector unit for when the thread returns from the
+	 * exception. If this is the first time the unit has been used by
+	 * the thread, initialise the vector registers and VSCR to 0, and
+	 * set the flag to indicate that the vector unit is in use.
+	 */
+	tf->srr1 |= PSL_VEC;
+	if (!(pcb->pcb_flags & PCB_VEC)) {
+		memset(&pcb->pcb_vec, 0, sizeof pcb->pcb_vec);
+		pcb->pcb_flags |= PCB_VEC;
+	}
+
+	/*
+	 * Temporarily enable the vector unit so the registers
+	 * can be restored.
+	 */
+	msr = mfmsr();
+	mtmsr(msr | PSL_VEC);
+	isync();
+
+	/*
+	 * Restore VSCR by first loading it into a vector and then into VSCR.
+	 * (this needs to done before loading the user's vector registers
+	 * since we need to use a scratch vector register)
+	 */
+	__asm __volatile("vxor 0,0,0; lvewx 0,0,%0; mtvscr 0" \
+			  :: "b"(&pcb->pcb_vec.vscr));
+
+#define LVX(n)   __asm ("lvx " #n ",0,%0" \
+		:: "b"(&pcb->pcb_vec.vr[n]));
+	LVX(0);		LVX(1);		LVX(2);		LVX(3);
+	LVX(4);		LVX(5);		LVX(6);		LVX(7);
+	LVX(8);		LVX(9);		LVX(10);	LVX(11);
+	LVX(12);	LVX(13);	LVX(14);	LVX(15);
+	LVX(16);	LVX(17);	LVX(18);	LVX(19);
+	LVX(20);	LVX(21);	LVX(22);	LVX(23);
+	LVX(24);	LVX(25);	LVX(26);	LVX(27);
+	LVX(28);	LVX(29);	LVX(30);	LVX(31);
+#undef LVX
+
+	isync();
+	mtmsr(msr);
+}
+
+void
+save_vec(struct thread *td)
+{
+	int	msr;
+	struct	pcb *pcb;
+
+	pcb = td->td_pcb;
+
+	/*
+	 * Temporarily re-enable the vector unit during the save
+	 */
+	msr = mfmsr();
+	mtmsr(msr | PSL_VEC);
+	isync();
+
+	/*
+	 * Save the vector registers and VSCR to the PCB
+	 */
+#define STVX(n)   __asm ("stvx %1,0,%0" \
+		:: "b"(pcb->pcb_vec.vr[n]), "n"(n));
+	STVX(0);	STVX(1);	STVX(2);	STVX(3);
+	STVX(4);	STVX(5);	STVX(6);	STVX(7);
+	STVX(8);	STVX(9);	STVX(10);	STVX(11);
+	STVX(12);	STVX(13);	STVX(14);	STVX(15);
+	STVX(16);	STVX(17);	STVX(18);	STVX(19);
+	STVX(20);	STVX(21);	STVX(22);	STVX(23);
+	STVX(24);	STVX(25);	STVX(26);	STVX(27);
+	STVX(28);	STVX(29);	STVX(30);	STVX(31);
+#undef STVX
+
+	__asm __volatile("mfvscr 0; stvewx 0,0,%0" :: "b"(&pcb->pcb_vec.vscr));
+
+	/*
+	 * Disable vector unit again
+	 */
+	isync();
+	mtmsr(msr);
+
+	/*
+	 * Clear the current vec thread and pcb's CPU id
+	 * XXX should this be left clear to allow lazy save/restore ?
+	 */
+	pcb->pcb_veccpu = INT_MAX;
+	PCPU_SET(vecthread, NULL);
+}
+

Modified: stable/7/sys/powerpc/powerpc/genassym.c
==============================================================================
--- stable/7/sys/powerpc/powerpc/genassym.c	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/powerpc/genassym.c	Sat Mar  7 20:39:42 2009	(r189498)
@@ -136,6 +136,7 @@ ASSYM(PCB_USR, offsetof(struct pcb, pcb_
 ASSYM(PCB_ONFAULT, offsetof(struct pcb, pcb_onfault));
 ASSYM(PCB_FLAGS, offsetof(struct pcb, pcb_flags));
 ASSYM(PCB_FPU, PCB_FPU);
+ASSYM(PCB_VEC, PCB_VEC);
 
 ASSYM(TD_PROC, offsetof(struct thread, td_proc));
 ASSYM(TD_PCB, offsetof(struct thread, td_pcb));

Modified: stable/7/sys/powerpc/powerpc/machdep.c
==============================================================================
--- stable/7/sys/powerpc/powerpc/machdep.c	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/powerpc/machdep.c	Sat Mar  7 20:39:42 2009	(r189498)
@@ -103,6 +103,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_object.h>
 #include <vm/vm_pager.h>
 
+#include <machine/altivec.h>
 #include <machine/bat.h>
 #include <machine/cpu.h>
 #include <machine/elf.h>
@@ -252,7 +253,6 @@ extern void	*dsitrap, *dsisize;
 extern void	*decrint, *decrsize;
 extern void     *extint, *extsize;
 extern void	*dblow, *dbsize;
-extern void	*vectrap, *vectrapsize;
 
 void
 powerpc_init(u_int startkernel, u_int endkernel, u_int basekernel, void *mdp)
@@ -343,7 +343,7 @@ powerpc_init(u_int startkernel, u_int en
 	bcopy(&trapcode, (void *)EXC_SC,   (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_TRC,  (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_FPA,  (size_t)&trapsize);
-	bcopy(&vectrap,  (void *)EXC_VEC,  (size_t)&vectrapsize);
+	bcopy(&trapcode, (void *)EXC_VEC,  (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_VECAST, (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_THRM, (size_t)&trapsize);
 	bcopy(&trapcode, (void *)EXC_BPT,  (size_t)&trapsize);
@@ -652,7 +652,21 @@ grab_mcontext(struct thread *td, mcontex
 		memcpy(mcp->mc_fpreg, pcb->pcb_fpu.fpr, 32*sizeof(double));
 	}
 
-	/* XXX Altivec context ? */
+	/*
+	 * Repeat for Altivec context
+	 */
+
+	if (pcb->pcb_flags & PCB_VEC) {
+		KASSERT(td == curthread,
+			("get_mcontext: fp save not curthread"));
+		critical_enter();
+		save_vec(td);
+		critical_exit();
+		mcp->mc_flags |= _MC_AV_VALID;
+		mcp->mc_vscr  = pcb->pcb_vec.vscr;
+		mcp->mc_vrsave =  pcb->pcb_vec.vrsave;
+		memcpy(mcp->mc_avec, pcb->pcb_vec.vr, sizeof(mcp->mc_avec));
+	}
 
 	mcp->mc_len = sizeof(*mcp);
 
@@ -706,7 +720,17 @@ set_mcontext(struct thread *td, const mc
 		memcpy(pcb->pcb_fpu.fpr, mcp->mc_fpreg, 32*sizeof(double));
 	}
 
-	/* XXX Altivec context? */
+	if (mcp->mc_flags & _MC_AV_VALID) {
+		if ((pcb->pcb_flags & PCB_VEC) != PCB_VEC) {
+			critical_enter();
+			enable_vec(td);
+			critical_exit();
+		}
+		pcb->pcb_vec.vscr = mcp->mc_vscr;
+		pcb->pcb_vec.vrsave = mcp->mc_vrsave;
+		memcpy(pcb->pcb_vec.vr, mcp->mc_avec, sizeof(mcp->mc_avec));
+	}
+
 
 	return (0);
 }

Modified: stable/7/sys/powerpc/powerpc/swtch.S
==============================================================================
--- stable/7/sys/powerpc/powerpc/swtch.S	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/powerpc/swtch.S	Sat Mar  7 20:39:42 2009	(r189498)
@@ -94,8 +94,16 @@ ENTRY(cpu_switch)
 	andi.	%r6, %r6, PCB_FPU
 	beq	.L1
 	bl	save_fpu
-	mr	%r3,%r14		/* restore old thread ptr */
+
 .L1:
+	lwz	%r6,PCB_FLAGS(%r5)
+	/* Save Altivec context if needed */
+	andi.	%r6, %r6, PCB_VEC
+	beq	.L2
+	bl	save_vec
+	
+.L2:
+	mr	%r3,%r14		/* restore old thread ptr */
 	bl	pmap_deactivate		/* Deactivate the current pmap */
 
 	mr	%r3,%r15		/* Get new thread ptr */
@@ -108,12 +116,20 @@ ENTRY(cpu_switch)
 
 	lwz	%r6, PCB_FLAGS(%r17)	/* Restore FPU context if needed */
 	andi.	%r6, %r6, PCB_FPU
-	beq	.L2
+	beq	.L3
 	mr	%r3,%r15		/* Pass curthread to enable_fpu */
 	bl	enable_fpu
 
+.L3:
+	lwz	%r6, PCB_FLAGS(%r17)
+	/* Restore Altivec context if needed */
+	andi.	%r6, %r6, PCB_VEC
+	beq	.L4
+	mr	%r3,%r15		/* Pass curthread to enable_vec */
+	bl	enable_vec
+
 	/* thread to restore is in r3 */
-.L2:
+.L4:
 	mr	%r3,%r17		/* Recover PCB ptr */
 	lmw	%r12,PCB_CONTEXT(%r3)	/* Load the non-volatile GP regs */
 	mr	%r2,%r12

Modified: stable/7/sys/powerpc/powerpc/trap.c
==============================================================================
--- stable/7/sys/powerpc/powerpc/trap.c	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/powerpc/trap.c	Sat Mar  7 20:39:42 2009	(r189498)
@@ -65,6 +65,7 @@ __FBSDID("$FreeBSD$");
 #include <vm/vm_map.h>
 #include <vm/vm_page.h>
 
+#include <machine/altivec.h>
 #include <machine/cpu.h>
 #include <machine/db_machdep.h>
 #include <machine/fpu.h>
@@ -188,24 +189,16 @@ trap(struct trapframe *frame)
 			enable_fpu(td);
 			break;
 
-#ifdef	ALTIVEC
 		case EXC_VEC:
-			if ((vecthread = PCPU_GET(vecthread)) != NULL) {
-				KASSERT(vecthread != td,
-				    ("altivec already enabled"));
-				save_vec(vecthread);
-			}
-			PCPU_SET(vecthread, td);
-			td->td_pcb->pcb_veccpu = PCPU_GET(cpuid);
+			KASSERT((td->td_pcb->pcb_flags & PCB_VEC) != PCB_VEC,
+			    ("Altivec already enabled for thread"));
 			enable_vec(td);
-			frame->srr1 |= PSL_VEC;
 			break;
-#else
-		case EXC_VEC:
+
 		case EXC_VECAST:
+			printf("Vector assist exception!\n");
 			sig = SIGILL;
 			break;
-#endif /* ALTIVEC */
 
 		case EXC_ALI:
 			if (fix_unaligned(td, frame) != 0)

Modified: stable/7/sys/powerpc/powerpc/trap_subr.S
==============================================================================
--- stable/7/sys/powerpc/powerpc/trap_subr.S	Sat Mar  7 19:54:30 2009	(r189497)
+++ stable/7/sys/powerpc/powerpc/trap_subr.S	Sat Mar  7 20:39:42 2009	(r189498)
@@ -79,13 +79,13 @@
 /*
  * FRAME_SETUP assumes:
  *	SPRG1		SP (1)
+ * 	SPRG3		trap type
  *	savearea	r28-r31,DAR,DSISR   (DAR & DSISR only for DSI traps)
  *	r28		LR
  *	r29		CR
  *	r30		scratch
  *	r31		scratch
  *	r1		kernel stack
- *	LR		trap type (from calling address, mask with 0xff00)
  *	SRR0/1		as at start of trap
  */
 #define	FRAME_SETUP(savearea)						\
@@ -146,8 +146,7 @@
 	lwz	%r31,(savearea+CPUSAVE_SRR1)(%r2); /* saved SRR1 */	\
 	mfxer	%r3;							\
 	mfctr	%r4;							\
-	mflr	%r5;							\
-	andi.	%r5,%r5,0xff00;		/* convert LR to exc # */	\
+	mfsprg3	%r5;							\
 	stw	%r3, FRAME_XER+8(1);	/* save xer/ctr/exc */		\
 	stw	%r4, FRAME_CTR+8(1);					\
 	stw	%r5, FRAME_EXC+8(1);					\
@@ -245,18 +244,10 @@ GLOBAL(dbstk)
 	.globl	CNAME(trapcode),CNAME(trapsize)
 CNAME(trapcode):
 	mtsprg1	%r1			/* save SP */
-	GET_CPUINFO(%r1)
-	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28-r31 */
-	stw	%r29,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)
-	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)
-	stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)
-	mfsprg1	%r1			/* restore SP, in case of branch */
-	mflr	%r28			/* save LR */
-	mfcr	%r29			/* save CR */
-/* Test whether we already had PR set */
-	mfsrr1	%r31
-	mtcr	%r31
-	bla	s_trap			/* LR & 0xff00 is exception # */
+	mflr	%r1			/* Save the old LR in r1 */
+	mtsprg2 %r1			/* And then in SPRG2 */
+	li	%r1, 0x20		/* How to get the vector from LR */
+	bla	generictrap		/* LR & SPRG3 is exception # */
 CNAME(trapsize) = .-CNAME(trapcode)
 
 /*
@@ -277,10 +268,15 @@ CNAME(alitrap):
 	mfsprg1	%r1			/* restore SP, in case of branch */
 	mflr	%r28			/* save LR */
 	mfcr	%r29			/* save CR */
-/* Test whether we already had PR set */
+
+	/* Put our exception vector in SPRG3 */
+	li	%r31, EXC_ALI
+	mtsprg3	%r31
+
+	/* Test whether we already had PR set */
 	mfsrr1	%r31
 	mtcr	%r31
-	bla	s_trap			/* LR & 0xff00 is exception # */
+	bla	s_trap
 CNAME(alisize) = .-CNAME(alitrap)
 
 /*
@@ -345,6 +341,11 @@ CNAME(dsisize) = .-CNAME(dsitrap)
  * Preamble code for DSI/ISI traps
  */
 disitrap:
+	/* Write the trap vector to SPRG3 by computing LR & 0xff00 */
+	mflr	%r1
+	andi.	%r1,%r1,0xff00
+	mtsprg3	%r1
+	
 	GET_CPUINFO(%r1)
 	lwz	%r30,(PC_DISISAVE+CPUSAVE_R28)(%r1)
 	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)
@@ -397,6 +398,47 @@ realtrap:
 	mtcr	%r1
 	mfsprg1	%r1			/* restore SP (might have been
 					   overwritten) */
+	bf	17,k_trap		/* branch if PSL_PR is false */
+	GET_CPUINFO(%r1)
+	lwz	%r1,PC_CURPCB(%r1)
+	RESTORE_KERN_SRS(%r30,%r31)	/* enable kernel mapping */
+	ba s_trap
+
+/*
+ * generictrap does some standard setup for trap handling to minimize
+ * the code that need be installed in the actual vectors. It expects
+ * the following conditions.
+ * 
+ * R1 - Trap vector = LR & (0xff00 | R1)
+ * SPRG1 - Original R1 contents
+ * SPRG2 - Original LR
+ */
+
+generictrap:
+	/* Save R1 for computing the exception vector */
+	mtsprg3 %r1
+
+	/* Save interesting registers */
+	GET_CPUINFO(%r1)
+	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28-r31 */
+	stw	%r29,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)
+	stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)
+	stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)
+	mfsprg1	%r1			/* restore SP, in case of branch */
+	mfsprg2	%r28			/* save LR */
+	mfcr	%r29			/* save CR */
+
+	/* Compute the exception vector from the link register */
+	mfsprg3 %r31
+	ori	%r31,%r31,0xff00
+	mflr	%r30
+	and	%r30,%r30,%r31
+	mtsprg3	%r30
+
+	/* Test whether we already had PR set */
+	mfsrr1	%r31
+	mtcr	%r31
+
 s_trap:
 	bf	17,k_trap		/* branch if PSL_PR is false */
 	GET_CPUINFO(%r1)
@@ -445,14 +487,6 @@ CNAME(asttrapexit):
 	FRAME_LEAVE(PC_TEMPSAVE)
 	rfi
 
-/*
- *     Temporary: vector-unavailable traps are directed to vector-assist traps
- */
-	.globl  CNAME(vectrap),CNAME(vectrapsize)
-CNAME(vectrap):
-	ba	EXC_VECAST
-CNAME(vectrapsize) = .-CNAME(vectrap)
-
 #if defined(KDB)
 /*
  * Deliberate entry to dbtrap
@@ -480,6 +514,14 @@ CNAME(ppc_db_trap):
  * Now the kdb trap catching code.
  */
 dbtrap:
+	/* Write the trap vector to SPRG3 by computing LR & 0xff00 */
+	mflr	%r1
+	andi.	%r1,%r1,0xff00
+	mtsprg3	%r1
+
+	lis	%r1,(tmpstk+TMPSTKSZ-16)@ha	/* get new SP */
+	addi	%r1,%r1,(tmpstk+TMPSTKSZ-16)@l
+
 	FRAME_SETUP(PC_DBSAVE)
 /* Call C trap code: */
 	addi	%r3,%r1,8
@@ -517,16 +559,19 @@ CNAME(dblow):
 	mfcr	%r29			/* save CR in r29 */
 	mfsrr1	%r1
 	mtcr	%r1
-	GET_CPUINFO(%r1)
 	bf	17,1f			/* branch if privileged */
-	stw	%r28,(PC_TEMPSAVE+CPUSAVE_R28)(%r1)	/* free r28 */
-        mfsprg2	%r28				/* r29 holds cr ... */
-        stw	%r28,(PC_TEMPSAVE+CPUSAVE_R29)(%r1)	/* free r29 */
-        stw	%r30,(PC_TEMPSAVE+CPUSAVE_R30)(%r1)	/* free r30 */
-        stw	%r31,(PC_TEMPSAVE+CPUSAVE_R31)(%r1)	/* free r31 */
-        mflr	%r28					/* save LR */
-        bla     u_trap
+
+	/* Unprivileged case */
+	mtcr	%r29			/* put the condition register back */
+	mfsprg2	%r29			/* ... and r29 */
+	mflr	%r1			/* save LR */
+	mtsprg2	%r1			/* And then in SPRG2 */
+	li	%r1, 0			/* How to get the vector from LR */
+
+	bla	generictrap		/* and we look like a generic trap */
 1:
+	/* Privileged, so drop to KDB */
+	GET_CPUINFO(%r1)
 	stw	%r28,(PC_DBSAVE+CPUSAVE_R28)(%r1)	/* free r28 */
         mfsprg2	%r28				/* r29 holds cr...  */
         stw	%r28,(PC_DBSAVE+CPUSAVE_R29)(%r1)	/* free r29 */



Want to link to this message? Use this URL: <https://mail-archive.FreeBSD.org/cgi/mid.cgi?200903072039.n27Kdg2q062234>