Index: sys/conf/ldscript.powerpc64
===================================================================
--- sys/conf/ldscript.powerpc64
+++ sys/conf/ldscript.powerpc64
@@ -13,7 +13,6 @@
 }
 SECTIONS
 {
-
   /* Low-address wrapper for bootloaders (kexec/kboot) that can't parse ELF */
   . = kernbase - 0x100;
   .kboot : { *(.text.kboot) } :text
Index: sys/powerpc/aim/aim_machdep.c
===================================================================
--- sys/powerpc/aim/aim_machdep.c
+++ sys/powerpc/aim/aim_machdep.c
@@ -158,6 +158,11 @@
 extern void	*imisstrap, *imisssize;
 extern void	*dlmisstrap, *dlmisssize;
 extern void	*dsmisstrap, *dsmisssize;
+#ifdef __powerpc64__
+extern void	*cpu_reset_handler;
+extern void	*cpu_wakeup_handler;
+extern void	*power_save_sequence;
+#endif
 
 extern void *ap_pcpu;
 extern void __restartkernel(vm_offset_t, vm_offset_t, vm_offset_t, void *, uint32_t, register_t offset, register_t msr);
@@ -391,6 +396,15 @@
 	/* Set TOC base so that the interrupt code can get at it */
 	*((void **)TRAP_GENTRAP) = &generictrap;
 	*((register_t *)TRAP_TOCBASE) = toc;
+	/*
+         * Set up special support function addresses.
+         * These functions do not use C calling conventions,
+	 * they operate on magic addresses in the trap vector.
+	 */
+	*((void **)TRAP_ADDR_CPU_RESET) = &cpu_reset_handler;
+	*((void **)TRAP_ADDR_CPU_WAKEUP) = &cpu_wakeup_handler;
+	*((void **)TRAP_ADDR_POWER_SAVE) = &power_save_sequence;
+
 	#else
 	/* Set branch address for trap code */
 	if (cpu_features & PPC_FEATURE_64)
Index: sys/powerpc/aim/trap_subr64.S
===================================================================
--- sys/powerpc/aim/trap_subr64.S
+++ sys/powerpc/aim/trap_subr64.S
@@ -48,6 +48,17 @@
 	ori	r,r,TRAP_TOCBASE;	/* Magic address for TOC */	\
 	ld	r,0(r)
 
+/*
+ * 8 bytes - Load DMAP address 0 into a register.
+ * Setup for accessing trap area memory with the MMU on or off.
+ * Optimized for low instruction count instead of flexibility.
+ * Note: This assumes the least significant 48 bits of
+ * DMAP_BASE_ADDRESS are 0!
+ */
+#define DMAP_ZERO(r)	\
+	lis	r,DMAP_BASE_ADDRESS@highesta;	/* To real-mode alias/dmap */ \
+	sldi	r,r,32
+
 /*
  * Restore SRs for a pmap
  *
@@ -317,29 +328,27 @@
 	 * processor is waking up from power saving mode
 	 * It is software reset when 46:47 = 0b00
 	 */
-	mfsrr1	%r9			/* Load SRR1 into r9 */
-	andis.	%r9,%r9,0x3		/* Logic AND with 46:47 bits */
-	beq	2f			/* Branch if software reset */
-	bl	1f
-	.llong	cpu_wakeup_handler
+	DMAP_ZERO(%r2)			/* 0x100 and 0x104 */
+	mfsrr1	%r9			/* 0x108 Load SRR1 into r9 */
+	andis.	%r9,%r9,0x3		/* 0x10c Logic AND with 46:47 bits */
+
+	beq	2f			/* 0x110 Branch if software reset */
+	ld	%r9,TRAP_ADDR_CPU_WAKEUP(%r2) /* 0x114 */
+	b	1f			/* 0x118 */
 
 	/* It is software reset */
 
 	/* Explicitly set MSR[SF] */
-2:	mfmsr	%r9
-	li	%r8,1
-	insrdi	%r9,%r8,1,0
-	mtmsrd	%r9
-	isync
-
-	bl	1f
-	.llong	cpu_reset_handler /* Make sure to maintain 8-byte alignment */
+2:	mfmsr	%r9			/* 0x11c */
+	li	%r8,1			/* 0x120 */
+	insrdi	%r9,%r8,1,0		/* 0x124 */
+	mtmsrd	%r9			/* 0x128 */
+	isync				/* 0x12c */
 
-1:	mflr	%r9
-	ld	%r9,0(%r9)
-	mtlr	%r9
+	ld	%r9,TRAP_ADDR_CPU_RESET(%r2)	/* 0x130 */
 
-	blr
+1:	mtlr	%r9				/* 0x134 */
+	blr					/* 0x138 */
 CNAME(rstcodeend):
 
 cpu_reset_handler:
@@ -471,38 +480,39 @@
 	.globl	CNAME(slbtrap),CNAME(slbtrapend)
 	.p2align 3
 CNAME(slbtrap):
+	/* 0x00 */
 	mtsprg1	%r1			/* save SP */
 	GET_CPUINFO(%r1)
 	std	%r2,(PC_SLBSAVE+16)(%r1)
 	mfcr	%r2			/* save CR */
+	/* 0x10 */
 	std	%r2,(PC_SLBSAVE+104)(%r1)
 	mfsrr1	%r2			/* test kernel mode */
 	mtcr	%r2
 	bf	17,2f			/* branch if PSL_PR is false */
+	/* 0x20 */
 	/* User mode */
 	ld	%r2,(PC_SLBSAVE+104)(%r1) /* Restore CR */
 	mtcr	%r2
 	ld	%r2,(PC_SLBSAVE+16)(%r1) /* Restore R2 */
 	mflr	%r1			/* Save the old LR in r1 */
+	/* 0x30 */
 	mtsprg2 %r1			/* And then in SPRG2 */
-					/* 52 bytes so far */
-	bl	1f
-	.llong	generictrap
-1:	mflr	%r1
-	ld	%r1,0(%r1)
+	DMAP_ZERO(%r1)			/* two instructions */
+	ld	%r1,TRAP_GENTRAP(%r1)
+	/* 0x40 */ 
 	mtlr	%r1
 	li	%r1, 0x80		/* How to get the vector from LR */
 	blrl				/* Branch to generictrap */
-					/* 84 bytes */
 2:	mflr	%r2			/* Save the old LR in r2 */
-	nop
-	bl	3f			/* Begin dance to jump to kern_slbtrap*/
-	.llong	kern_slbtrap
-3:	mflr	%r1
-	ld	%r1,0(%r1)
+	/* 0x50 */
+	DMAP_ZERO(%r1)			/* two instructions */
+	ld	%r1,TRAP_GENTRAP(%r1)
+	addi    %r1,%r1,(kern_slbtrap-generictrap)
+	/* 0x60 */
 	mtlr	%r1
 	GET_CPUINFO(%r1)
-	blrl				/* 124 bytes -- 4 to spare */
+	blrl				/* must fit in 128 bytes! */
 CNAME(slbtrapend):
 
 kern_slbtrap:
@@ -641,14 +651,9 @@
 	mflr	%r28			/* save LR */
 	mfcr	%r29			/* save CR */
 
-	/* Begin dance to branch to s_trap in a bit */
-	b	1f
-	.p2align 3
-1:	nop
-	bl	1f
-	.llong	s_trap
-1:	mflr	%r31
-	ld	%r31,0(%r31)
+	DMAP_ZERO(%r31)			/* two instructions */
+	ld	%r31,TRAP_GENTRAP(%r31)
+	addi	%r31,%r31,(s_trap - generictrap)
 	mtlr	%r31
 
 	/* Put our exception vector in SPRG3 */
@@ -681,10 +686,9 @@
 	mfsrr1	%r31			/* test kernel mode */
 	mtcr	%r31
 	mflr	%r28			/* save LR (SP already saved) */
-	bl	1f			/* Begin branching to disitrap */
-	.llong	disitrap
-1:	mflr	%r1
-	ld	%r1,0(%r1)
+	DMAP_ZERO(%r1)
+	ld	%r1,TRAP_GENTRAP(%r1)
+	addi	%r1,%r1,(disitrap-generictrap)
 	mtlr	%r1
 	blrl				/* Branch to generictrap */
 CNAME(dsiend):
@@ -966,11 +970,9 @@
         std	%r30,(PC_DBSAVE+CPUSAVE_R30)(%r1)	/* free r30 */
         std	%r31,(PC_DBSAVE+CPUSAVE_R31)(%r1)	/* free r31 */
         mflr	%r28					/* save LR */
-	nop						/* alignment */
-	bl	9f					/* Begin branch */
-	.llong	dbtrap
-9:	mflr	%r1
-	ld	%r1,0(%r1)
+	DMAP_ZERO(%r1)
+	ld	%r1,TRAP_GENTRAP(%r1)
+	addi	%r1,%r1,(dbtrap-generictrap)
 	mtlr	%r1
 	blrl				/* Branch to generictrap */
 CNAME(dbend):
Index: sys/powerpc/include/trap.h
===================================================================
--- sys/powerpc/include/trap.h
+++ sys/powerpc/include/trap.h
@@ -146,9 +146,12 @@
 /* DTrace trap opcode. */
 #define EXC_DTRACE	0x7ffff808
 
-/* Magic pointer to store TOC base and other info for trap handlers on ppc64 */
-#define TRAP_GENTRAP	0x1f0
-#define TRAP_TOCBASE	0x1f8
+/* Magic pointers to store TOC base and other info for trap handlers on ppc64 */
+#define TRAP_ADDR_POWER_SAVE	0x1c8	/* &power_save_sequence */
+#define TRAP_ADDR_CPU_RESET	0x1d0	/* &cpu_reset_handler */
+#define TRAP_ADDR_CPU_WAKEUP	0x1d8	/* &cpu_wakeup_handler */
+#define TRAP_GENTRAP		0x1f0	/* &generictrap */
+#define TRAP_TOCBASE		0x1f8	/* Located at end of EXC_RST region. */
 
 #ifndef LOCORE
 struct	trapframe;
Index: sys/powerpc/powerpc/cpu_subr64.S
===================================================================
--- sys/powerpc/powerpc/cpu_subr64.S
+++ sys/powerpc/powerpc/cpu_subr64.S
@@ -30,6 +30,7 @@
 #include "assym.inc"
 
 #include <machine/asm.h>
+#include <machine/trap.h>
 
 	.globl	CNAME(power_save_sequence)
 	.p2align 3
@@ -65,11 +66,9 @@
 	std	%r1,PCB_SP(%r3)		/* Save the stack pointer */
 	std	%r2,PCB_TOC(%r3)	/* Save the TOC pointer */
 
-	/* Set where we want to jump */
-	bl	1f
-	.llong power_save_sequence	/* Remember about 8 byte alignment */
-1:	mflr	%r3
-	ld	%r3,0(%r3)
+	lis	%r3,DMAP_BASE_ADDRESS@highesta;	/* To real-mode alias/dmap */
+	sldi	%r3,%r3,32
+	ld	%r3,TRAP_ADDR_POWER_SAVE(%r3)
 	mtsrr0	%r3
 
 	/* Set MSR */