ppc: Loose GOT access in IRQ

Using the GOT in IRQ handlers requires r14 to be -ffixed-r14.
Avoid this by relocatate transfer_to_handler too.
This will allow to free up r14 later on.

Signed-off-by: Joakim Tjernlund <Joakim.Tjernlund@transmode.se>

15 files changed, 108 insertions, 407 deletions

diff --git a/cpu/74xx_7xx/start.S b/cpu/74xx_7xx/start.S
index 23381a3..efcd95f 100644
--- a/cpu/74xx_7xx/start.S
+++ b/cpu/74xx_7xx/start.S
@@ -132,28 +132,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	/* No FPU on MPC8xx.  This exception is not supposed to happen.
 	*/
@@ -837,23 +824,6 @@ trap_init:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	sync
-	isync
-
-	blr
-
 #ifdef CONFIG_SYS_INIT_RAM_LOCK
 lock_ram_in_cache:
 	/* Allocate Initial RAM in data cache.
diff --git a/cpu/mpc512x/start.S b/cpu/mpc512x/start.S
index 4edc8e9..46c5efa 100644
--- a/cpu/mpc512x/start.S
+++ b/cpu/mpc512x/start.S
@@ -127,30 +127,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 /* Floating Point Unit unavailable exception */
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
@@ -723,17 +708,3 @@ trap_init:
 
 	mtlr	r4			/* restore link register    */
 	blr
-
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr	*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...	*/
-	add	r0, r0, r3		/*  ... += dest_addr	*/
-	stw	r0, 4(r7)
-
-	blr
diff --git a/cpu/mpc5xx/start.S b/cpu/mpc5xx/start.S
index 85ea7a8..be980a1 100644
--- a/cpu/mpc5xx/start.S
+++ b/cpu/mpc5xx/start.S
@@ -217,28 +217,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	/* FPU on MPC5xx available. We will use it later.
 	*/
@@ -563,24 +550,6 @@ trap_init:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	sync
-	isync
-
-	blr
-
-
 #if defined(CONFIG_PATI)
 /* Program the PLL */
 pll_prog_code_start:
diff --git a/cpu/mpc5xxx/start.S b/cpu/mpc5xxx/start.S
index eb42939..075a7dd 100644
--- a/cpu/mpc5xxx/start.S
+++ b/cpu/mpc5xxx/start.S
@@ -215,30 +215,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 
@@ -797,17 +782,3 @@ trap_init:
 
 	mtlr	r4			/* restore link register    */
 	blr
-
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	blr
diff --git a/cpu/mpc8220/start.S b/cpu/mpc8220/start.S
index af9472d..5218c85 100644
--- a/cpu/mpc8220/start.S
+++ b/cpu/mpc8220/start.S
@@ -176,30 +176,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16	    /* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25	    /* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16	    /* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25	    /* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 
@@ -762,17 +747,3 @@ trap_init:
 
 	mtlr	r4		    /* restore link register	    */
 	blr
-
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)	    /* hdlr ...			    */
-	add	r0, r0, r3	    /*	... += dest_addr	    */
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)	    /* int_return ...		    */
-	add	r0, r0, r3	    /*	... += dest_addr	    */
-	stw	r0, 4(r7)
-
-	blr
diff --git a/cpu/mpc824x/start.S b/cpu/mpc824x/start.S
index 750457b..7f40648 100644
--- a/cpu/mpc824x/start.S
+++ b/cpu/mpc824x/start.S
@@ -227,28 +227,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = EXC_OFF_PROGRAM
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	/* No FPU on MPC8xx. This exception is not supposed to happen.
 	*/
@@ -695,20 +682,6 @@ trap_init:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	blr
-
 	/* Setup the BAT registers.
 	 */
 setup_bats:
diff --git a/cpu/mpc8260/start.S b/cpu/mpc8260/start.S
index edb95e6..7d80af5 100644
--- a/cpu/mpc8260/start.S
+++ b/cpu/mpc8260/start.S
@@ -286,30 +286,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 
@@ -1033,17 +1018,3 @@ trap_init:
 
 	mtlr	r4			/* restore link register    */
 	blr
-
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	blr
diff --git a/cpu/mpc83xx/start.S b/cpu/mpc83xx/start.S
index ee4b862..5a50b09 100644
--- a/cpu/mpc83xx/start.S
+++ b/cpu/mpc83xx/start.S
@@ -338,30 +338,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	rlwimi	r20,r23,0,25,25		/* copy IP bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 
@@ -1086,19 +1071,6 @@ trap_init:
 	mtlr	r4			/* restore link register    */
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr	*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...	*/
-	add	r0, r0, r3		/*  ... += dest_addr	*/
-	stw	r0, 4(r7)
-
-	blr
 #endif /* !CONFIG_NAND_SPL */
 
 #ifdef CONFIG_SYS_INIT_RAM_LOCK
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 7e60e67..d73c5c2 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -415,28 +415,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + _START_OFFSET
-	.long	int_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x0700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + _START_OFFSET
-	.long	int_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	/* No FPU on MPC85xx.  This exception is not supposed to happen.
 	*/
@@ -1089,20 +1076,6 @@ trap_init:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0,0(r7)		/* hdlr ...			*/
-	add	r0,r0,r3		/*  ... += dest_addr		*/
-	stw	r0,0(r7)
-
-	lwz	r0,4(r7)		/* int_return ...		*/
-	add	r0,r0,r3		/*  ... += dest_addr		*/
-	stw	r0,4(r7)
-
-	blr
-
 .globl unlock_ram_in_cache
 unlock_ram_in_cache:
 	/* invalidate the INIT_RAM section */
diff --git a/cpu/mpc86xx/start.S b/cpu/mpc86xx/start.S
index e65f1c0..eaa2657 100644
--- a/cpu/mpc86xx/start.S
+++ b/cpu/mpc86xx/start.S
@@ -121,28 +121,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
 
@@ -848,23 +835,6 @@ trap_init:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	sync
-	isync
-
-	blr
-
 .globl enable_ext_addr
 enable_ext_addr:
 	mfspr	r0, HID0
diff --git a/cpu/mpc8xx/start.S b/cpu/mpc8xx/start.S
index e84326e..24e9053 100644
--- a/cpu/mpc8xx/start.S
+++ b/cpu/mpc8xx/start.S
@@ -231,28 +231,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 	/* No FPU on MPC8xx.  This exception is not supposed to happen.
 	*/
@@ -694,20 +681,3 @@ trap_init:
 
 	mtlr	r4			/* restore link register	*/
 	blr
-
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	sync
-	isync
-
-	blr
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 46f65aa..2eb21fa 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -574,28 +574,15 @@ Alignment:
 	mfspr	r5,DSISR
 	stw	r5,_DSISR(r21)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + _START_OFFSET
-	.long	int_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(Alignment, AlignmentException, MSR_KERNEL, COPY_EE)
 
 /* Program check exception */
 	. = 0x700
 ProgramCheck:
 	EXCEPTION_PROLOG(SRR0, SRR1)
 	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + _START_OFFSET
-	.long	int_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(ProgramCheck, ProgramCheckException,
+		MSR_KERNEL, COPY_EE)
 
 #ifdef CONFIG_440
 	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
@@ -1742,20 +1729,6 @@ __440_msr_continue:
 	mtlr	r4			/* restore link register	*/
 	blr
 
-	/*
-	 * Function: relocate entries for one exception vector
-	 */
-trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
-
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
-
-	blr
-
 #if defined(CONFIG_440)
 /*----------------------------------------------------------------------------+
 | dcbz_area.
diff --git a/include/ppc_asm.tmpl b/include/ppc_asm.tmpl
index 0019d46..fa5c5f6 100644
--- a/include/ppc_asm.tmpl
+++ b/include/ppc_asm.tmpl
@@ -257,46 +257,43 @@
  * OFFSET values only; they must be relocated first before they can
  * be used!
  */
-#define STD_EXCEPTION(n, label, hdlr)			\
-	. = n;						\
-label:							\
-	EXCEPTION_PROLOG(SRR0, SRR1);			\
-	lwz	r3,GOT(transfer_to_handler);		\
-	mtlr	r3;					\
-	addi	r3,r1,STACK_FRAME_OVERHEAD;		\
-	li	r20,MSR_KERNEL;				\
+#define COPY_EE(d, s)		rlwimi d,s,0,16,16
+#define NOCOPY(d, s)
+#define EXC_XFER_TEMPLATE(label, hdlr, msr, copyee)	\
+	bl	1f;					\
+1:	mflr    r20;					\
+	lwz	r20,(.L_ ## label)-1b+8(r20);		\
+	mtlr	r20;					\
+	li	r20,msr;				\
+	copyee(r20,r23);				\
 	rlwimi	r20,r23,0,25,25;			\
 	blrl;						\
 .L_ ## label :						\
 	.long	hdlr - _start + _START_OFFSET;		\
-	.long	int_return - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET;	\
+	.long	transfer_to_handler - _start + _START_OFFSET
+
+#define STD_EXCEPTION(n, label, hdlr)				\
+	. = n;							\
+label:								\
+	EXCEPTION_PROLOG(SRR0, SRR1);				\
+	addi	r3,r1,STACK_FRAME_OVERHEAD;			\
+	EXC_XFER_TEMPLATE(label, hdlr, MSR_KERNEL, NOCOPY)	\
 
 #define CRIT_EXCEPTION(n, label, hdlr)				\
 	. = n;							\
 label:								\
 	EXCEPTION_PROLOG(CSRR0, CSRR1);				\
-	lwz	r3,GOT(transfer_to_handler);			\
-	mtlr	r3;						\
 	addi	r3,r1,STACK_FRAME_OVERHEAD;			\
-	li	r20,(MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE));	\
-	rlwimi	r20,r23,0,25,25;				\
-	blrl;							\
-.L_ ## label :							\
-	.long	hdlr - _start + _START_OFFSET;			\
-	.long	crit_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(label, hdlr,				\
+	MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE), NOCOPY)		\
 
 #define MCK_EXCEPTION(n, label, hdlr)				\
 	. = n;							\
 label:								\
 	EXCEPTION_PROLOG(MCSRR0, MCSRR1);			\
-	lwz	r3,GOT(transfer_to_handler);			\
-	mtlr	r3;						\
 	addi	r3,r1,STACK_FRAME_OVERHEAD;			\
-	li	r20,(MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE));	\
-	rlwimi	r20,r23,0,25,25;				\
-	blrl;							\
-.L_ ## label :							\
-	.long	hdlr - _start + _START_OFFSET;			\
-	.long	mck_return - _start + _START_OFFSET
+	EXC_XFER_TEMPLATE(label, hdlr,				\
+	MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE), NOCOPY)		\
 
 #endif	/* __PPC_ASM_TMPL__ */
diff --git a/lib_ppc/Makefile b/lib_ppc/Makefile
index 60a7625..334e457 100644
--- a/lib_ppc/Makefile
+++ b/lib_ppc/Makefile
@@ -28,6 +28,7 @@ LIB	= $(obj)lib$(ARCH).a
 SOBJS-y	+= ppccache.o
 SOBJS-y	+= ppcstring.o
 SOBJS-y	+= ticks.o
+SOBJS-y	+= reloc.o
 
 COBJS-y	+= bat_rw.o
 COBJS-y	+= board.o
diff --git a/lib_ppc/reloc.S b/lib_ppc/reloc.S
new file mode 100644
index 0000000..50f9a83
--- /dev/null
+++ b/lib_ppc/reloc.S
@@ -0,0 +1,49 @@
+/*
+ * Copyright (C) 2009 Wolfgang Denk <wd@denx.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <ppc_asm.tmpl>
+
+	.file	"reloc.S"
+
+	.text
+#ifndef CONFIG_NAND_SPL
+	/*
+	 * Function: relocate entries for one exception vector
+	 */
+	.globl trap_reloc
+	.type	trap_reloc, @function
+trap_reloc:
+	lwz	r0, 0(r7)		/* hdlr ...		*/
+	add	r0, r0, r3		/*  ... += dest_addr	*/
+	stw	r0, 0(r7)
+
+	lwz	r0, 4(r7)		/* int_return ...	*/
+	add	r0, r0, r3		/*  ... += dest_addr	*/
+	stw	r0, 4(r7)
+
+	lwz	r0, 8(r7)		/* transfer_to_handler ...*/
+	add	r0, r0, r3		/*  ... += dest_addr	*/
+	stw	r0, 8(r7)
+
+	blr
+	.size	trap_reloc, .-trap_reloc
+#endif