Save and restore the trap vectors when doing OF calls on pSeries machines.

It turned out that on pSeries machines the call into OF modified the trap
vectors and this made further behaviour unpredictable.

With this commit I'm now able to boot multi user on a network booted
environment on my IntelliStation 285. This is a POWER5+ machine.

Discussed with:		nwhitehorn
MFC after:	1 week

3 files changed, 38 insertions, 0 deletions

diff --git a/sys/powerpc/aim/machdep.c b/sys/powerpc/aim/machdep.c
index 51b9798c131d..dd22bb72e860 100644
--- a/sys/powerpc/aim/machdep.c
+++ b/sys/powerpc/aim/machdep.c
@@ -123,6 +123,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/spr.h>
 #include <machine/trap.h>
 #include <machine/vmparam.h>
+#include <machine/ofw_machdep.h>
 
 #include <ddb/ddb.h>
 
@@ -249,6 +250,7 @@ extern void	*dblow, *dbsize;
 extern void	*imisstrap, *imisssize;
 extern void	*dlmisstrap, *dlmisssize;
 extern void	*dsmisstrap, *dsmisssize;
+char 		save_trap_init[0x2f00];		/* EXC_LAST */
 
 uintptr_t
 powerpc_init(vm_offset_t startkernel, vm_offset_t endkernel,
@@ -273,6 +275,9 @@ powerpc_init(vm_offset_t startkernel, vm_offset_t endkernel,
 	trap_offset = 0;
 	cacheline_warn = 0;
 
+	/* Save trap vectors. */
+	ofw_save_trap_vec(save_trap_init);
+
 #ifdef WII
 	/*
 	 * The Wii loader doesn't pass us any environment so, mdp
diff --git a/sys/powerpc/include/ofw_machdep.h b/sys/powerpc/include/ofw_machdep.h
index b85ebe121660..023fa4c56252 100644
--- a/sys/powerpc/include/ofw_machdep.h
+++ b/sys/powerpc/include/ofw_machdep.h
@@ -47,5 +47,6 @@ void OF_reboot(void);
 
 void ofw_mem_regions(struct mem_region **, int *, struct mem_region **, int *);
 void ofw_quiesce(void); /* Must be called before VM is up! */
+void ofw_save_trap_vec(char *);
 
 #endif /* _MACHINE_OFW_MACHDEP_H_ */
diff --git a/sys/powerpc/ofw/ofw_machdep.c b/sys/powerpc/ofw/ofw_machdep.c
index 344865f12a5c..3b2f84bfb5c7 100644
--- a/sys/powerpc/ofw/ofw_machdep.c
+++ b/sys/powerpc/ofw/ofw_machdep.c
@@ -59,6 +59,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/md_var.h>
 #include <machine/platform.h>
 #include <machine/ofw_machdep.h>
+#include <machine/trap.h>
 
 static struct mem_region OFmem[PHYS_AVAIL_SZ], OFavail[PHYS_AVAIL_SZ];
 static struct mem_region OFfree[PHYS_AVAIL_SZ];
@@ -70,10 +71,31 @@ extern register_t ofmsr[5];
 extern void	*openfirmware_entry;
 static void	*fdt;
 int		ofw_real_mode;
+extern char     save_trap_init[0x2f00];          /* EXC_LAST */
+char            save_trap_of[0x2f00];            /* EXC_LAST */
 
 int		ofwcall(void *);
 static int	openfirmware(void *args);
 
+__inline void
+ofw_save_trap_vec(char *save_trap_vec)
+{
+	if (apple_hacks)
+                return;
+
+	bcopy((void *)EXC_RST, save_trap_vec, EXC_LAST - EXC_RST);
+}
+
+static __inline void
+ofw_restore_trap_vec(char *restore_trap_vec)
+{
+	if (apple_hacks)
+                return;
+
+	bcopy(restore_trap_vec, (void *)EXC_RST, EXC_LAST - EXC_RST);
+	__syncicache(EXC_RSVD, EXC_LAST - EXC_RSVD);
+}
+
 /*
  * Saved SPRG0-3 from OpenFirmware. Will be restored prior to the callback.
  */
@@ -524,6 +546,12 @@ openfirmware_core(void *args)
 
 	ofw_sprg_prepare();
 
+	/* Save trap vectors */
+	ofw_save_trap_vec(save_trap_of);
+
+	/* Restore initially saved trap vectors */
+	ofw_restore_trap_vec(save_trap_init);
+
 #if defined(AIM) && !defined(__powerpc64__)
 	/*
 	 * Clear battable[] translations
@@ -535,6 +563,10 @@ openfirmware_core(void *args)
 #endif
 
 	result = ofwcall(args);
+
+	/* Restore trap vecotrs */
+	ofw_restore_trap_vec(save_trap_of);
+
 	ofw_sprg_restore();
 
 	intr_restore(oldmsr);