[U-Boot] [PATCH] 85xx: Fix mapping of 0xfffffxxx when CONFIG_MP

[Peter Tyser]

Previously, boot page translation was enabled while U-Boot executed.
This resulted in the address range 0xfffff000 - 0xffffffff being
translated to SDRAM which made the 0xfffffxxx address space unusable for
other peripherals.

This change disables boot page translation after the secondary CPU cores
have been initialized which allows the 0xfffffxxx address space to be
properly accessed.

Signed-off-by: Peter Tyser <ptyser at xes-inc.com>
---
This was tested on the XPedite5370 which has flash mapped in the
0xfffffxxx adddress space.  I verified the flash was accessible
as expected and Linux properly brought up 2 cores.

I wasn't sure how the MPC8572 handled caching with respect to the boot
page translation.  I didn't add any cache flushes/invalidates, but they
may be necessary if the 0xfffffxxx range is not mapped as uncachable.
Anyone at Freescale have any comments on this?

Best,
Peter

 cpu/mpc85xx/mp.c      |   14 ++++++++++++--
 cpu/mpc85xx/release.S |   30 +++++++++++++++++++++++++-----
 2 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/cpu/mpc85xx/mp.c b/cpu/mpc85xx/mp.c
index 76f02a4..0019cec 100644
--- a/cpu/mpc85xx/mp.c
+++ b/cpu/mpc85xx/mp.c
@@ -129,7 +129,7 @@ ulong get_spin_addr(void)
 
 	ulong addr =
 		(ulong)&__spin_table - (ulong)&__secondary_start_page;
-	addr += 0xfffff000;
+	addr += determine_mp_bootpg();
 
 	return addr;
 }
@@ -137,7 +137,8 @@ ulong get_spin_addr(void)
 static void pq3_mp_up(unsigned long bootpg)
 {
 	u32 up, cpu_up_mask, whoami;
-	u32 *table = (u32 *)get_spin_addr();
+	/* The table is at 0xfffffxxx due to boot page translation below */
+	u32 *table = (u32 *)(0xfffff000 | get_spin_addr());
 	volatile u32 bpcr;
 	volatile ccsr_local_ecm_t *ecm = (void *)(CONFIG_SYS_MPC85xx_ECM_ADDR);
 	volatile ccsr_gur_t *gur = (void *)(CONFIG_SYS_MPC85xx_GUTS_ADDR);
@@ -146,6 +147,8 @@ static void pq3_mp_up(unsigned long bootpg)
 	int timeout = 10;
 
 	whoami = in_be32(&pic->whoami);
+
+	/* Translate 0xfffffxxx 'bootpg' address range to SDRAM */
 	out_be32(&ecm->bptr, 0x80000000 | (bootpg >> 12));
 
 	/* disable time base at the platform */
@@ -194,6 +197,9 @@ static void pq3_mp_up(unsigned long bootpg)
 
 	devdisr &= ~(MPC85xx_DEVDISR_TB0 | MPC85xx_DEVDISR_TB1);
 	out_be32(&gur->devdisr, devdisr);
+
+	/* Disable translation of 0xfffffxxx 'bootpg' */
+	out_be32(&ecm->bptr, 0x0);
 }
 
 void cpu_mp_lmb_reserve(struct lmb *lmb)
@@ -206,9 +212,13 @@ void cpu_mp_lmb_reserve(struct lmb *lmb)
 void setup_mp(void)
 {
 	extern ulong __secondary_start_page;
+	extern ulong __bootpg_addr;
 	ulong fixup = (ulong)&__secondary_start_page;
 	u32 bootpg = determine_mp_bootpg();
 
+	/* Store the bootpg's SDRAM address for use by secondary CPU cores */
+	__bootpg_addr = bootpg;
+
 	memcpy((void *)bootpg, (void *)fixup, 4096);
 	flush_cache(bootpg, 4096);
 
diff --git a/cpu/mpc85xx/release.S b/cpu/mpc85xx/release.S
index fbefc2c..6799633 100644
--- a/cpu/mpc85xx/release.S
+++ b/cpu/mpc85xx/release.S
@@ -114,23 +114,38 @@ __secondary_start_page:
 	stw	r3,ENTRY_R6_UPPER(r10)
 	stw	r3,ENTRY_R6_LOWER(r10)
 
+	/* load r13 with the address of the 'bootpg' in SDRAM */
+	lis	r13,toreset(__bootpg_addr)@h
+	ori	r13,r13,toreset(__bootpg_addr)@l
+	lwz	r13,0(r13)
+
 	/* setup mapping for AS = 1, and jump there */
 	lis	r11,(MAS0_TLBSEL(1)|MAS0_ESEL(1))@h
 	mtspr	SPRN_MAS0,r11
 	lis	r11,(MAS1_VALID|MAS1_IPROT)@h
 	ori	r11,r11,(MAS1_TS|MAS1_TSIZE(BOOKE_PAGESZ_4K))@l
 	mtspr	SPRN_MAS1,r11
-	lis	r11,(0xfffff000|MAS2_I)@h
-	ori	r11,r11,(0xfffff000|MAS2_I)@l
+	oris	r11,r13,(MAS2_I)@h
+	ori	r11,r13,(MAS2_I)@l
 	mtspr	SPRN_MAS2,r11
-	lis	r11,(0xfffff000|MAS3_SX|MAS3_SW|MAS3_SR)@h
-	ori	r11,r11,(0xfffff000|MAS3_SX|MAS3_SW|MAS3_SR)@l
+	oris	r11,r13,(MAS3_SX|MAS3_SW|MAS3_SR)@h
+	ori	r11,r13,(MAS3_SX|MAS3_SW|MAS3_SR)@l
 	mtspr	SPRN_MAS3,r11
 	tlbwe
 
 	bl	1f
 1:	mflr	r11
-	addi	r11,r11,28
+	/*
+	 * OR in 0xfff to create a mask of the bootpg SDRAM address.  We use
+	 * this mask to fixup the cpu spin table and the address that we want
+	 * to jump to, eg change them from 0xfffffxxx to 0x7ffffxxx if the
+	 * bootpg is at 0x7ffff000 in SDRAM.
+	 */
+	ori	r13,r13,0xfff
+	and	r11, r11, r13
+	and	r10, r10, r13
+
+	addi	r11,r11,(2f-1b)
 	mfmsr	r13
 	ori	r12,r13,MSR_IS|MSR_DS at l
 
@@ -200,6 +215,11 @@ __secondary_start_page:
 	mtspr	SPRN_SRR1,r13
 	rfi
 
+	/* Allocate some space for the SDRAM address of the bootpg */
+	.globl __bootpg_addr
+__bootpg_addr:
+	.long	0
+
 	.align L1_CACHE_SHIFT
 	.globl __spin_table
 __spin_table:
-- 
1.6.2.1