1 files changed, 66 insertions, 53 deletions

diff --git a/cpu/mpc86xx/start.S b/cpu/mpc86xx/start.S
index c83310a..f163521 100644
--- a/cpu/mpc86xx/start.S
+++ b/cpu/mpc86xx/start.S
@@ -235,17 +235,8 @@ in_flash:
 	bl	enable_ext_addr
 
 	/* setup the bats */
-	bl	setup_bats
-	sync
-
-#if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
-	/* setup ccsrbar */
-	bl	setup_ccsrbar
-#endif
+	bl	early_bats
 
-	/* setup the law entries */
-	bl	law_entry
-	sync
 	/*
 	 * Cache must be enabled here for stack-in-cache trick.
 	 * This means we need to enable the BATS.
@@ -282,6 +273,16 @@ in_flash:
 
 	GET_GOT			/* initialize GOT access	*/
 
+	/* setup the rest of the bats */
+	bl      setup_bats
+	bl      clear_tlbs
+	sync
+
+#if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
+	/* setup ccsrbar */
+	bl      setup_ccsrbar
+#endif
+
 	/* run low-level CPU init code	   (from Flash) */
 	bl	cpu_init_f
 	sync
@@ -359,6 +360,7 @@ invalidate_bats:
 
 
 	/* setup_bats - set them up to some initial state */
+	/* Skip any BATS setup in early_bats */
 	.globl	setup_bats
 setup_bats:
 
@@ -454,42 +456,6 @@ setup_bats:
 	mtspr	DBAT4U, r3
 	isync
 
-	/* IBAT 5 */
-	addis	r4, r0, CFG_IBAT5L@h
-	ori	r4, r4, CFG_IBAT5L@l
-	addis	r3, r0, CFG_IBAT5U@h
-	ori	r3, r3, CFG_IBAT5U@l
-	mtspr	IBAT5L, r4
-	mtspr	IBAT5U, r3
-	isync
-
-	/* DBAT 5 */
-	addis	r4, r0, CFG_DBAT5L@h
-	ori	r4, r4, CFG_DBAT5L@l
-	addis	r3, r0, CFG_DBAT5U@h
-	ori	r3, r3, CFG_DBAT5U@l
-	mtspr	DBAT5L, r4
-	mtspr	DBAT5U, r3
-	isync
-
-	/* IBAT 6 */
-	addis	r4, r0, CFG_IBAT6L@h
-	ori	r4, r4, CFG_IBAT6L@l
-	addis	r3, r0, CFG_IBAT6U@h
-	ori	r3, r3, CFG_IBAT6U@l
-	mtspr	IBAT6L, r4
-	mtspr	IBAT6U, r3
-	isync
-
-	/* DBAT 6 */
-	addis	r4, r0, CFG_DBAT6L@h
-	ori	r4, r4, CFG_DBAT6L@l
-	addis	r3, r0, CFG_DBAT6U@h
-	ori	r3, r3, CFG_DBAT6U@l
-	mtspr	DBAT6L, r4
-	mtspr	DBAT6U, r3
-	isync
-
 	/* IBAT 7 */
 	addis	r4, r0, CFG_IBAT7L@h
 	ori	r4, r4, CFG_IBAT7L@l
@@ -508,18 +474,65 @@ setup_bats:
 	mtspr	DBAT7U, r3
 	isync
 
-1:
-	addis	r3, 0, 0x0000
-	addis	r5, 0, 0x4	/* upper bound of 0x00040000 for 7400/750 */
+	sync
+	blr
+
+/*
+ * early_bats:
+ *
+ * Set up bats needed early on - this is usually the BAT for the
+ * stack-in-cache and the Flash
+ */
+	.globl  early_bats
+early_bats:
+	/* IBAT 5 */
+	lis   	r4, CFG_IBAT5L@h
+	ori     r4, r4, CFG_IBAT5L@l
+	lis   	r3, CFG_IBAT5U@h
+	ori     r3, r3, CFG_IBAT5U@l
+	mtspr   IBAT5L, r4
+	mtspr   IBAT5U, r3
+	isync
+
+	/* DBAT 5 */
+	lis   	r4, CFG_DBAT5L@h
+	ori     r4, r4, CFG_DBAT5L@l
+	lis   	r3, CFG_DBAT5U@h
+	ori     r3, r3, CFG_DBAT5U@l
+	mtspr   DBAT5L, r4
+	mtspr   DBAT5U, r3
+	isync
+
+	/* IBAT 6 */
+	lis   	r4, CFG_IBAT6L@h
+	ori     r4, r4, CFG_IBAT6L@l
+	lis	r3, CFG_IBAT6U@h
+	ori     r3, r3, CFG_IBAT6U@l
+	mtspr   IBAT6L, r4
+	mtspr   IBAT6U, r3
 	isync
 
+	/* DBAT 6 */
+	lis   	r4, CFG_DBAT6L@h
+	ori     r4, r4, CFG_DBAT6L@l
+	lis   	r3, CFG_DBAT6U@h
+	ori     r3, r3, CFG_DBAT6U@l
+	mtspr   DBAT6L, r4
+	mtspr   DBAT6U, r3
+	isync
+	blr
+
+	.globl clear_tlbs
+clear_tlbs:
+	addis   r3, 0, 0x0000
+	addis   r5, 0, 0x4
+	isync
 tlblp:
-	tlbie	r3
+	tlbie   r3
 	sync
-	addi	r3, r3, 0x1000
-	cmp	0, 0, r3, r5
+	addi    r3, r3, 0x1000
+	cmp     0, 0, r3, r5
 	blt tlblp
-
 	blr
 
 	.globl enable_addr_trans