85xx: Additional fixes and cleanup of MP code

[u-boot] / cpu / mpc85xx / mp.c

diff --git a/cpu/mpc85xx/mp.c b/cpu/mpc85xx/mp.c
index d3727b0b2c03f2c5ab8819a944b5a0d1d4f8747f..a527cf3047ad76eeb9f1904ab1c6091922554cb0 100644 (file)
--- a/cpu/mpc85xx/mp.c
+++ b/cpu/mpc85xx/mp.c
@@ -23,6 +23,7 @@
 #include <common.h>
 #include <asm/processor.h>
 #include <ioports.h>
+#include <lmb.h>
 #include <asm/io.h>
 #include "mp.h"
 
@@ -102,6 +103,10 @@ int cpu_release(int nr, int argc, char *argv[])
        }
 
        table[BOOT_ENTRY_ADDR_UPPER] = (u32)(boot_addr >> 32);
+
+       /* ensure all table updates complete before final address write */
+       eieio();
+
        table[BOOT_ENTRY_ADDR_LOWER] = (u32)(boot_addr & 0xffffffff);
 
        return 0;
@@ -152,8 +157,8 @@ static void pq3_mp_up(unsigned long bootpg)
        /* wait for everyone */
        while (timeout) {
                int i;
-               for (i = 1; i < CONFIG_NR_CPUS; i++) {
-                       if (table[i * NUM_BOOT_ENTRY])
+               for (i = 0; i < CONFIG_NR_CPUS; i++) {
+                       if (table[i * NUM_BOOT_ENTRY + BOOT_ENTRY_ADDR_LOWER])
                                cpu_up_mask |= (1 << i);
                };
 
@@ -164,6 +169,10 @@ static void pq3_mp_up(unsigned long bootpg)
                timeout--;
        }
 
+       if (timeout == 0)
+               printf("CPU up timeout. CPU up mask is %x should be %x\n",
+                       cpu_up_mask, up);
+
        /* enable time base at the platform */
        if (whoami)
                devdisr |= MPC85xx_DEVDISR_TB1;
@@ -177,6 +186,19 @@ static void pq3_mp_up(unsigned long bootpg)
        out_be32(&gur->devdisr, devdisr);
 }
 
+void cpu_mp_lmb_reserve(struct lmb *lmb)
+{
+       u32 bootpg;
+
+       /* if we have 4G or more of memory, put the boot page at 4Gb-4k */
+       if ((u64)gd->ram_size > 0xfffff000)
+               bootpg = 0xfffff000;
+       else
+               bootpg = gd->ram_size - 4096;
+
+       lmb_reserve(lmb, bootpg, 4096);
+}
+
 void setup_mp(void)
 {
        extern ulong __secondary_start_page;