1 files changed, 101 insertions, 1 deletions
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 8e000d3..54be37c 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -1361,7 +1361,7 @@ ppcSync:
 relocate_code:
 #if defined(CONFIG_440EP) || defined(CONFIG_440GR) || \
     defined(CONFIG_440EPX) || defined(CONFIG_440GRX) || \
-    defined(CONFIG_440SPE)
+    defined(CONFIG_440SP) || defined(CONFIG_440SPE)
 	/*
 	 * On some 440er platforms the cache is enabled in the first TLB (Boot-CS)
 	 * to speed up the boot process. Now this cache needs to be disabled.
@@ -1856,3 +1856,103 @@ pll_wait:
 				     /* execution will continue from the poweron */
 				     /* vector of 0xfffffffc */
 #endif /* CONFIG_405EP */
+
+#if defined(CONFIG_440)
+#define function_prolog(func_name)      .text; \
+					.align 2; \
+					.globl func_name; \
+					func_name:
+#define function_epilog(func_name)      .type func_name,@function; \
+					.size func_name,.-func_name
+
+/*----------------------------------------------------------------------------+
+| mttlb3.
++----------------------------------------------------------------------------*/
+	function_prolog(mttlb3)
+	TLBWE(4,3,2)
+	blr
+	function_epilog(mttlb3)
+
+/*----------------------------------------------------------------------------+
+| mftlb3.
++----------------------------------------------------------------------------*/
+	function_prolog(mftlb3)
+	TLBRE(3,3,2)
+	blr
+	function_epilog(mftlb3)
+
+/*----------------------------------------------------------------------------+
+| mttlb2.
++----------------------------------------------------------------------------*/
+	function_prolog(mttlb2)
+	TLBWE(4,3,1)
+	blr
+	function_epilog(mttlb2)
+
+/*----------------------------------------------------------------------------+
+| mftlb2.
++----------------------------------------------------------------------------*/
+	function_prolog(mftlb2)
+	TLBRE(3,3,1)
+	blr
+	function_epilog(mftlb2)
+
+/*----------------------------------------------------------------------------+
+| mttlb1.
++----------------------------------------------------------------------------*/
+	function_prolog(mttlb1)
+	TLBWE(4,3,0)
+	blr
+	function_epilog(mttlb1)
+
+/*----------------------------------------------------------------------------+
+| mftlb1.
++----------------------------------------------------------------------------*/
+	function_prolog(mftlb1)
+	TLBRE(3,3,0)
+	blr
+	function_epilog(mftlb1)
+
+/*----------------------------------------------------------------------------+
+| dcbz_area.
++----------------------------------------------------------------------------*/
+	function_prolog(dcbz_area)
+	rlwinm. r5,r4,0,27,31
+	rlwinm  r5,r4,27,5,31
+	beq     ..d_ra2
+	addi    r5,r5,0x0001
+..d_ra2:mtctr   r5
+..d_ag2:dcbz    r0,r3
+	addi    r3,r3,32
+	bdnz    ..d_ag2
+	sync
+	blr
+	function_epilog(dcbz_area)
+
+/*----------------------------------------------------------------------------+
+| dflush.  Assume 32K at vector address is cachable.
++----------------------------------------------------------------------------*/
+	function_prolog(dflush)
+	mfmsr   r9
+	rlwinm  r8,r9,0,15,13
+	rlwinm  r8,r8,0,17,15
+	mtmsr   r8
+	addi    r3,r0,0x0000
+	mtspr   dvlim,r3
+	mfspr   r3,ivpr
+	addi    r4,r0,1024
+	mtctr   r4
+..dflush_loop:
+	lwz     r6,0x0(r3)
+	addi    r3,r3,32
+	bdnz    ..dflush_loop
+	addi    r3,r3,-32
+	mtctr   r4
+..ag:   dcbf    r0,r3
+	addi    r3,r3,-32
+	bdnz    ..ag
+	sync
+	mtmsr   r9
+	blr
+	function_epilog(dflush)
+#endif /* CONFIG_440 */