summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--cpu/mpc85xx/mp.c32
1 files changed, 29 insertions, 3 deletions
diff --git a/cpu/mpc85xx/mp.c b/cpu/mpc85xx/mp.c
index 2df55c7..fa65bed 100644
--- a/cpu/mpc85xx/mp.c
+++ b/cpu/mpc85xx/mp.c
@@ -25,6 +25,7 @@
#include <ioports.h>
#include <lmb.h>
#include <asm/io.h>
+#include <asm/mmu.h>
#include "mp.h"
DECLARE_GLOBAL_DATA_PTR;
@@ -209,8 +210,33 @@ void setup_mp(void)
ulong fixup = (ulong)&__secondary_start_page;
u32 bootpg = determine_mp_bootpg();
- memcpy((void *)bootpg, (void *)fixup, 4096);
- flush_cache(bootpg, 4096);
+ /* look for the tlb covering the reset page, there better be one */
+ int i = find_tlb_idx((void *)0xfffff000, 1);
- pq3_mp_up(bootpg);
+ /* we found a match */
+ if (i != -1) {
+ /* map reset page to bootpg so we can copy code there */
+ disable_tlb(i);
+
+ set_tlb(1, 0xfffff000, bootpg, /* tlb, epn, rpn */
+ MAS3_SX|MAS3_SW|MAS3_SR, MAS2_M, /* perms, wimge */
+ 0, i, BOOKE_PAGESZ_4K, 1); /* ts, esel, tsize, iprot */
+
+ memcpy((void *)0xfffff000, (void *)fixup, 4096);
+ flush_cache(0xfffff000, 4096);
+
+ disable_tlb(i);
+
+ /* setup reset page back to 1:1, we'll use HW boot translation
+ * to map this where we want
+ */
+ set_tlb(1, 0xfffff000, 0xfffff000, /* tlb, epn, rpn */
+ MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I, /* perms, wimge */
+ 0, i, BOOKE_PAGESZ_4K, 1); /* ts, esel, tsize, iprot */
+
+ pq3_mp_up(bootpg);
+ } else {
+ puts("WARNING: No reset page TLB. "
+ "Skipping secondary core setup\n");
+ }
}