24 files changed, 973 insertions, 198 deletions

diff --git a/cpu/mips/cache.S b/cpu/mips/cache.S
index 443240e..89ada71 100644
--- a/cpu/mips/cache.S
+++ b/cpu/mips/cache.S
@@ -1,5 +1,5 @@
 /*
- *  Cache-handling routined for MIPS 4K CPUs
+ *  Cache-handling routined for MIPS CPUs
  *
  *  Copyright (c) 2003	Wolfgang Denk <wd@denx.de>
  *
@@ -24,15 +24,32 @@
 
 #include <config.h>
 #include <version.h>
+#include <asm/asm.h>
 #include <asm/regdef.h>
 #include <asm/mipsregs.h>
 #include <asm/addrspace.h>
 #include <asm/cacheops.h>
 
-	/* 16KB is the maximum size of instruction and data caches on
-	 * MIPS 4K.
-	 */
-#define MIPS_MAX_CACHE_SIZE	0x4000
+#define RA		t8
+
+/*
+ * 16kB is the maximum size of instruction and data caches on MIPS 4K,
+ * 64kB is on 4KE, 24K, 5K, etc. Set bigger size for convenience.
+ *
+ * Note that the above size is the maximum size of primary cache. U-Boot
+ * doesn't have L2 cache support for now.
+ */
+#define MIPS_MAX_CACHE_SIZE	0x10000
+
+#define INDEX_BASE	KSEG0
+
+	.macro	cache_op op addr
+	.set	push
+	.set	noreorder
+	.set	mips3
+	cache	\op, 0(\addr)
+	.set	pop
+	.endm
 
 /*
  * cacheop macro to automate cache operations
@@ -103,6 +120,77 @@
 #define icacheop(kva, n, cacheSize, cacheLineSize, op) \
    icacheopn(kva, n, cacheSize, cacheLineSize, 1, (op))
 
+	.macro	f_fill64 dst, offset, val
+	LONG_S	\val, (\offset +  0 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  1 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  2 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  3 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  4 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  5 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  6 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  7 * LONGSIZE)(\dst)
+#if LONGSIZE == 4
+	LONG_S	\val, (\offset +  8 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset +  9 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 10 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 11 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 12 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 13 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 14 * LONGSIZE)(\dst)
+	LONG_S	\val, (\offset + 15 * LONGSIZE)(\dst)
+#endif
+	.endm
+
+/*
+ * mips_init_icache(uint PRId, ulong icache_size, unchar icache_linesz)
+ */
+LEAF(mips_init_icache)
+	blez	a1, 9f
+	mtc0	zero, CP0_TAGLO
+	/* clear tag to invalidate */
+	PTR_LI		t0, INDEX_BASE
+	PTR_ADDU	t1, t0, a1
+1:	cache_op	Index_Store_Tag_I t0
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 1b
+	/* fill once, so data field parity is correct */
+	PTR_LI		t0, INDEX_BASE
+2:	cache_op	Fill t0
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 2b
+	/* invalidate again - prudent but not strictly neccessary */
+	PTR_LI		t0, INDEX_BASE
+1:	cache_op	Index_Store_Tag_I t0
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 1b
+9:	jr	ra
+	END(mips_init_icache)
+
+/*
+ * mips_init_dcache(uint PRId, ulong dcache_size, unchar dcache_linesz)
+ */
+LEAF(mips_init_dcache)
+	blez	a1, 9f
+	mtc0	zero, CP0_TAGLO
+	/* clear all tags */
+	PTR_LI		t0, INDEX_BASE
+	PTR_ADDU	t1, t0, a1
+1:	cache_op	Index_Store_Tag_D t0
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 1b
+	/* load from each line (in cached space) */
+	PTR_LI		t0, INDEX_BASE
+2:	LONG_L		zero, 0(t0)
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 2b
+	/* clear all tags */
+	PTR_LI		t0, INDEX_BASE
+1:	cache_op	Index_Store_Tag_D t0
+	PTR_ADDU	t0, a2
+	bne		t0, t1, 1b
+9:	jr	ra
+	END(mips_init_dcache)
+
 /*******************************************************************************
 *
 * mips_cache_reset - low level initialisation of the primary caches
@@ -119,10 +207,8 @@
 * RETURNS: N/A
 *
 */
-	.globl	mips_cache_reset
-	.ent	mips_cache_reset
-mips_cache_reset:
-
+NESTED(mips_cache_reset, 0, ra)
+	move	RA, ra
 	li	t2, CFG_ICACHE_SIZE
 	li	t3, CFG_DCACHE_SIZE
 	li	t4, CFG_CACHELINE_SIZE
@@ -130,27 +216,14 @@ mips_cache_reset:
 
 	li	v0, MIPS_MAX_CACHE_SIZE
 
-	/* Now clear that much memory starting from zero.
-	 */
-
-	li	a0, KSEG1
-	addu	a1, a0, v0
-2:
-	sw	zero, 0(a0)
-	sw	zero, 4(a0)
-	sw	zero, 8(a0)
-	sw	zero, 12(a0)
-	sw	zero, 16(a0)
-	sw	zero, 20(a0)
-	sw	zero, 24(a0)
-	sw	zero, 28(a0)
-	addu	a0, 32
-	bltu	a0, a1, 2b
-
-	/* Set invalid tag.
+	/*
+	 * Now clear that much memory starting from zero.
 	 */
-
-	mtc0	zero, CP0_TAGLO
+	PTR_LI		a0, KSEG1
+	PTR_ADDU	a1, a0, v0
+2:	PTR_ADDIU	a0, 64
+	f_fill64	a0, -64, zero
+	bne		a0, a1, 2b
 
 	/*
 	 * The caches are probably in an indeterminate state,
@@ -158,48 +231,26 @@ mips_cache_reset:
 	 * invalidate, load/fill, invalidate for each line.
 	 */
 
-	/* Assume bottom of RAM will generate good parity for the cache.
-	 */
-
-	li	a0, K0BASE
-	move	a2, t2		# icacheSize
-	move	a3, t4		# icacheLineSize
-	move	a1, a2
-	icacheopn(a0,a1,a2,a3,121,(Index_Store_Tag_I,Fill))
-
-	/* To support Orion/R4600, we initialise the data cache in 3 passes.
-	 */
-
-	/* 1: initialise dcache tags.
+	/*
+	 * Assume bottom of RAM will generate good parity for the cache.
 	 */
 
-	li	a0, K0BASE
-	move	a2, t3		# dcacheSize
-	move	a3, t5		# dcacheLineSize
-	move	a1, a2
-	icacheop(a0,a1,a2,a3,Index_Store_Tag_D)
-
-	/* 2: fill dcache.
+	/*
+	 * Initialize the I-cache first,
 	 */
+	move	a1, t2
+	move	a2, t4
+	bal	mips_init_icache
 
-	li	a0, K0BASE
-	move	a2, t3		# dcacheSize
-	move	a3, t5		# dcacheLineSize
-	move	a1, a2
-	icacheopn(a0,a1,a2,a3,1lw,(dummy))
-
-	/* 3: clear dcache tags.
+	/*
+	 * then initialize D-cache.
 	 */
+	move	a1, t3
+	move	a2, t5
+	bal	mips_init_dcache
 
-	li	a0, K0BASE
-	move	a2, t3		# dcacheSize
-	move	a3, t5		# dcacheLineSize
-	move	a1, a2
-	icacheop(a0,a1,a2,a3,Index_Store_Tag_D)
-
-	j	ra
-
-	.end	mips_cache_reset
+	jr	RA
+	END(mips_cache_reset)
 
 /*******************************************************************************
 *
@@ -208,15 +259,15 @@ mips_cache_reset:
 * RETURNS: 0 - cache disabled; 1 - cache enabled
 *
 */
-	.globl	dcache_status
-	.ent	dcache_status
-dcache_status:
-
-	mfc0	v0, CP0_CONFIG
-	andi	v0, v0, 1
-	j	ra
-
-	.end	dcache_status
+LEAF(dcache_status)
+	mfc0	t0, CP0_CONFIG
+	li	t1, CONF_CM_UNCACHED
+	andi	t0, t0, CONF_CM_CMASK
+	move	v0, zero
+	beq	t0, t1, 2f
+	li	v0, 1
+2:	jr	ra
+	END(dcache_status)
 
 /*******************************************************************************
 *
@@ -225,19 +276,16 @@ dcache_status:
 * RETURNS: N/A
 *
 */
-	.globl	dcache_disable
-	.ent	dcache_disable
-dcache_disable:
-
+LEAF(dcache_disable)
 	mfc0	t0, CP0_CONFIG
 	li	t1, -8
 	and	t0, t0, t1
 	ori	t0, t0, CONF_CM_UNCACHED
 	mtc0	t0, CP0_CONFIG
 	j	ra
+	END(dcache_disable)
 
-	.end	dcache_disable
-
+#ifdef CFG_INIT_RAM_LOCK_MIPS
 /*******************************************************************************
 *
 * mips_cache_lock - lock RAM area pointed to by a0 in cache.
@@ -263,3 +311,4 @@ mips_cache_lock:
 	j	ra
 
 	.end	mips_cache_lock
+#endif /* CFG_INIT_RAM_LOCK_MIPS */
diff --git a/cpu/mips/cpu.c b/cpu/mips/cpu.c
index 7559ac6..8b43d8e 100644
--- a/cpu/mips/cpu.c
+++ b/cpu/mips/cpu.c
@@ -23,24 +23,45 @@
 
 #include <common.h>
 #include <command.h>
-#include <asm/inca-ip.h>
 #include <asm/mipsregs.h>
+#include <asm/cacheops.h>
+#include <asm/reboot.h>
+
+#define cache_op(op,addr)						\
+	__asm__ __volatile__(						\
+	"	.set	push					\n"	\
+	"	.set	noreorder				\n"	\
+	"	.set	mips3\n\t				\n"	\
+	"	cache	%0, %1					\n"	\
+	"	.set	pop					\n"	\
+	:								\
+	: "i" (op), "R" (*(unsigned char *)(addr)))
+
+void __attribute__((weak)) _machine_restart(void)
+{
+}
 
 int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char *argv[])
 {
-#if defined(CONFIG_INCA_IP)
-	*INCA_IP_WDT_RST_REQ = 0x3f;
-#elif defined(CONFIG_PURPLE) || defined(CONFIG_TB0229)
-	void (*f)(void) = (void *) 0xbfc00000;
+	_machine_restart();
 
-	f();
-#endif
 	fprintf(stderr, "*** reset failed ***\n");
 	return 0;
 }
 
 void flush_cache(ulong start_addr, ulong size)
 {
+	unsigned long lsize = CFG_CACHELINE_SIZE;
+	unsigned long addr = start_addr & ~(lsize - 1);
+	unsigned long aend = (start_addr + size - 1) & ~(lsize - 1);
+
+	while (1) {
+		cache_op(Hit_Writeback_Inv_D, start_addr);
+		cache_op(Hit_Invalidate_I, start_addr);
+		if (addr == aend)
+			break;
+		addr += lsize;
+	}
 }
 
 void write_one_tlb(int index, u32 pagemask, u32 hi, u32 low0, u32 low1)
diff --git a/cpu/mips/start.S b/cpu/mips/start.S
index c92b162..baac2ce 100644
--- a/cpu/mips/start.S
+++ b/cpu/mips/start.S
@@ -27,6 +27,30 @@
 #include <asm/regdef.h>
 #include <asm/mipsregs.h>
 
+	/*
+	 * For the moment disable interrupts, mark the kernel mode and
+	 * set ST0_KX so that the CPU does not spit fire when using
+	 * 64-bit addresses.
+	 */
+	.macro	setup_c0_status set clr
+	.set	push
+	mfc0	t0, CP0_STATUS
+	or	t0, ST0_CU0 | \set | 0x1f | \clr
+	xor	t0, 0x1f | \clr
+	mtc0	t0, CP0_STATUS
+	.set	noreorder
+	sll	zero, 3				# ehb
+	.set	pop
+	.endm
+
+	.macro	setup_c0_status_reset
+#ifdef CONFIG_64BIT
+	setup_c0_status ST0_KX 0
+#else
+	setup_c0_status 0 0
+#endif
+	.endm
+
 #define RVECENT(f,n) \
    b f; nop
 #define XVECENT(f,bev) \
@@ -211,19 +235,11 @@ reset:
 	mtc0	zero, CP0_WATCHLO
 	mtc0	zero, CP0_WATCHHI
 
-	/* STATUS register */
-#ifdef  CONFIG_TB0229
-	li	k0, ST0_CU0
-#else
-	mfc0	k0, CP0_STATUS
-#endif
-	li	k1, ~ST0_IE
-	and	k0, k1
-	mtc0	k0, CP0_STATUS
-
-	/* CAUSE register */
+	/* WP(Watch Pending), SW0/1 should be cleared. */
 	mtc0	zero, CP0_CAUSE
 
+	setup_c0_status_reset
+
 	/* Init Timer */
 	mtc0	zero, CP0_COUNT
 	mtc0	zero, CP0_COMPARE
@@ -240,14 +256,6 @@ reset:
 1:
 	lw	gp, 0(ra)
 
-#ifdef CONFIG_INCA_IP
-	/* Disable INCA-IP Watchdog.
-	 */
-	la	t9, disable_incaip_wdt
-	jalr	t9
-	nop
-#endif
-
 	/* Initialize any external memory.
 	 */
 	la	t9, lowlevel_init
@@ -267,10 +275,12 @@ reset:
 
 	/* Set up temporary stack.
 	 */
+#ifdef CFG_INIT_RAM_LOCK_MIPS
 	li	a0, CFG_INIT_SP_OFFSET
 	la	t9, mips_cache_lock
 	jalr	t9
 	nop
+#endif
 
 	li	t0, CFG_SDRAM_BASE + CFG_INIT_SP_OFFSET
 	la	sp, 0(t0)
diff --git a/cpu/mpc5xxx/cpu.c b/cpu/mpc5xxx/cpu.c
index e4d6168..ace1653 100644
--- a/cpu/mpc5xxx/cpu.c
+++ b/cpu/mpc5xxx/cpu.c
@@ -114,12 +114,14 @@ unsigned long get_tbclk (void)
 
 /* ------------------------------------------------------------------------- */
 
-#ifdef CONFIG_OF_LIBFDT
+#if defined(CONFIG_OF_LIBFDT) && defined (CONFIG_OF_BOARD_SETUP)
 void ft_cpu_setup(void *blob, bd_t *bd)
 {
 	int div = in_8((void*)CFG_MBAR + 0x204) & 0x0020 ? 8 : 4;
 	char * cpu_path = "/cpus/" OF_CPU;
+#ifdef CONFIG_MPC5xxx_FEC
 	char * eth_path = "/" OF_SOC "/ethernet@3000";
+#endif
 
 	do_fixup_by_path_u32(blob, cpu_path, "timebase-frequency", OF_TBCLK, 1);
 	do_fixup_by_path_u32(blob, cpu_path, "bus-frequency", bd->bi_busfreq, 1);
@@ -127,7 +129,9 @@ void ft_cpu_setup(void *blob, bd_t *bd)
 	do_fixup_by_path_u32(blob, "/" OF_SOC, "bus-frequency", bd->bi_ipbfreq, 1);
 	do_fixup_by_path_u32(blob, "/" OF_SOC, "system-frequency",
 				bd->bi_busfreq*div, 1);
+#ifdef CONFIG_MPC5xxx_FEC
 	do_fixup_by_path(blob, eth_path, "mac-address", bd->bi_enetaddr, 6, 0);
 	do_fixup_by_path(blob, eth_path, "local-mac-address", bd->bi_enetaddr, 6, 0);
+#endif
 }
 #endif
diff --git a/cpu/mpc8260/cpu.c b/cpu/mpc8260/cpu.c
index 55e61a1..414759e 100644
--- a/cpu/mpc8260/cpu.c
+++ b/cpu/mpc8260/cpu.c
@@ -300,7 +300,7 @@ void watchdog_reset (void)
 #endif /* CONFIG_WATCHDOG */
 
 /* ------------------------------------------------------------------------- */
-#if defined(CONFIG_OF_LIBFDT)
+#if defined(CONFIG_OF_LIBFDT) && defined (CONFIG_OF_BOARD_SETUP)
 void ft_cpu_setup (void *blob, bd_t *bd)
 {
 	char * cpu_path = "/cpus/" OF_CPU;
diff --git a/cpu/mpc83xx/Makefile b/cpu/mpc83xx/Makefile
index 94a3cb8..fcb6a52 100644
--- a/cpu/mpc83xx/Makefile
+++ b/cpu/mpc83xx/Makefile
@@ -28,9 +28,20 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o
-COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o \
-	  spd_sdram.o ecc.o qe_io.o pci.o fdt.o
 
+COBJS-y += traps.o
+COBJS-y += cpu.o
+COBJS-y += cpu_init.o
+COBJS-y += speed.o
+COBJS-y += interrupts.o
+COBJS-y += spd_sdram.o
+COBJS-y += ecc.o
+COBJS-$(CONFIG_QE) += qe_io.o
+COBJS-$(CONFIG_FSL_SERDES) += serdes.o
+COBJS-$(CONFIG_83XX_GENERIC_PCI) += pci.o
+COBJS-$(CONFIG_OF_LIBFDT) += fdt.o
+
+COBJS	:= $(COBJS-y)
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
 START	:= $(addprefix $(obj),$(START))
diff --git a/cpu/mpc83xx/cpu_init.c b/cpu/mpc83xx/cpu_init.c
index e643037..fba5b02 100644
--- a/cpu/mpc83xx/cpu_init.c
+++ b/cpu/mpc83xx/cpu_init.c
@@ -79,6 +79,12 @@ void cpu_init_f (volatile immap_t * im)
 			  (CFG_ACR_RPTCNT << ACR_RPTCNT_SHIFT);
 #endif
 
+#ifdef CFG_SPCR_OPT
+	/* Optimize transactions between CSB and other devices */
+	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_OPT) |
+			   (CFG_SPCR_OPT << SPCR_OPT_SHIFT);
+#endif
+
 #ifdef CFG_SPCR_TSECEP
 	/* all eTSEC's Emergency priority */
 	im->sysconf.spcr = (im->sysconf.spcr & ~SPCR_TSECEP) |
diff --git a/cpu/mpc83xx/fdt.c b/cpu/mpc83xx/fdt.c
index 6f55932..b39f678 100644
--- a/cpu/mpc83xx/fdt.c
+++ b/cpu/mpc83xx/fdt.c
@@ -24,9 +24,6 @@
  */
 
 #include <common.h>
-
-#if defined(CONFIG_OF_LIBFDT)
-
 #include <libfdt.h>
 #include <fdt_support.h>
 
@@ -49,6 +46,14 @@ void ft_cpu_setup(void *blob, bd_t *bd)
 		"clock-frequency", gd->core_clk, 1);
 	do_fixup_by_prop_u32(blob, "device_type", "soc", 4,
 		"bus-frequency", bd->bi_busfreq, 1);
+	do_fixup_by_compat_u32(blob, "fsl,soc",
+		"bus-frequency", bd->bi_busfreq, 1);
+	do_fixup_by_compat_u32(blob, "fsl,soc",
+		"clock-frequency", bd->bi_busfreq, 1);
+	do_fixup_by_compat_u32(blob, "fsl,immr",
+		"bus-frequency", bd->bi_busfreq, 1);
+	do_fixup_by_compat_u32(blob, "fsl,immr",
+		"clock-frequency", bd->bi_busfreq, 1);
 #ifdef CONFIG_QE
 	ft_qe_setup(blob);
 #endif
@@ -68,4 +73,3 @@ void ft_cpu_setup(void *blob, bd_t *bd)
 
 	fdt_fixup_memory(blob, (u64)bd->bi_memstart, (u64)bd->bi_memsize);
 }
-#endif /* CONFIG_OF_LIBFDT */
diff --git a/cpu/mpc83xx/pci.c b/cpu/mpc83xx/pci.c
index 18558db..adabf7a 100644
--- a/cpu/mpc83xx/pci.c
+++ b/cpu/mpc83xx/pci.c
@@ -33,7 +33,6 @@
 
 #include <asm/mpc8349_pci.h>
 
-#ifdef CONFIG_83XX_GENERIC_PCI
 #define MAX_BUSES 2
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -209,4 +208,3 @@ void ft_pci_setup(void *blob, bd_t *bd)
 	}
 }
 #endif /* CONFIG_OF_LIBFDT */
-#endif /* CONFIG_83XX_GENERIC_PCI */
diff --git a/cpu/mpc83xx/qe_io.c b/cpu/mpc83xx/qe_io.c
index 8b3937a..ce91a07 100644
--- a/cpu/mpc83xx/qe_io.c
+++ b/cpu/mpc83xx/qe_io.c
@@ -25,7 +25,6 @@
 #include "asm/io.h"
 #include "asm/immap_83xx.h"
 
-#if defined(CONFIG_QE)
 #define	NUM_OF_PINS	32
 void qe_config_iopin(u8 port, u8 pin, int dir, int open_drain, int assign)
 {
@@ -81,5 +80,3 @@ void qe_config_iopin(u8 port, u8 pin, int dir, int open_drain, int assign)
 		out_be32(&par_io->ioport[port].ppar1, pin_2bit_assign | tmp_val);
 	}
 }
-
-#endif /* CONFIG_QE */
diff --git a/cpu/mpc83xx/serdes.c b/cpu/mpc83xx/serdes.c
new file mode 100644
index 0000000..020c4c8
--- /dev/null
+++ b/cpu/mpc83xx/serdes.c
@@ -0,0 +1,145 @@
+/*
+ * Freescale SerDes initialization routine
+ *
+ * Copyright (C) 2007 Freescale Semicondutor, Inc. All rights reserved.
+ * Copyright (C) 2008 MontaVista Software, Inc. All rights reserved.
+ *
+ * Author: Li Yang <leoli@freescale.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <config.h>
+#include <common.h>
+#include <asm/io.h>
+#include <asm/fsl_serdes.h>
+
+/* SerDes registers */
+#define FSL_SRDSCR0_OFFS		0x0
+#define FSL_SRDSCR0_DPP_1V2		0x00008800
+#define FSL_SRDSCR1_OFFS		0x4
+#define FSL_SRDSCR1_PLLBW		0x00000040
+#define FSL_SRDSCR2_OFFS		0x8
+#define FSL_SRDSCR2_VDD_1V2		0x00800000
+#define FSL_SRDSCR2_SEIC_MASK		0x00001c1c
+#define FSL_SRDSCR2_SEIC_SATA		0x00001414
+#define FSL_SRDSCR2_SEIC_PEX		0x00001010
+#define FSL_SRDSCR2_SEIC_SGMII		0x00000101
+#define FSL_SRDSCR3_OFFS		0xc
+#define FSL_SRDSCR3_KFR_SATA		0x10100000
+#define FSL_SRDSCR3_KPH_SATA		0x04040000
+#define FSL_SRDSCR3_SDFM_SATA_PEX	0x01010000
+#define FSL_SRDSCR3_SDTXL_SATA		0x00000505
+#define FSL_SRDSCR4_OFFS		0x10
+#define FSL_SRDSCR4_PROT_SATA		0x00000808
+#define FSL_SRDSCR4_PROT_PEX		0x00000101
+#define FSL_SRDSCR4_PROT_SGMII		0x00000505
+#define FSL_SRDSCR4_PLANE_X2		0x01000000
+#define FSL_SRDSRSTCTL_OFFS		0x20
+#define FSL_SRDSRSTCTL_RST		0x80000000
+#define FSL_SRDSRSTCTL_SATA_RESET	0xf
+
+void fsl_setup_serdes(u32 offset, char proto, char rfcks, char vdd)
+{
+	void *regs = (void *)CFG_IMMR + offset;
+	u32 tmp;
+
+	/* 1.0V corevdd */
+	if (vdd) {
+		/* DPPE/DPPA = 0 */
+		tmp = in_be32(regs + FSL_SRDSCR0_OFFS);
+		tmp &= ~FSL_SRDSCR0_DPP_1V2;
+		out_be32(regs + FSL_SRDSCR0_OFFS, tmp);
+
+		/* VDD = 0 */
+		tmp = in_be32(regs + FSL_SRDSCR2_OFFS);
+		tmp &= ~FSL_SRDSCR2_VDD_1V2;
+		out_be32(regs + FSL_SRDSCR2_OFFS, tmp);
+	}
+
+	/* protocol specific configuration */
+	switch (proto) {
+	case FSL_SERDES_PROTO_SATA:
+		/* Set and clear reset bits */
+		tmp = in_be32(regs + FSL_SRDSRSTCTL_OFFS);
+		tmp |= FSL_SRDSRSTCTL_SATA_RESET;
+		out_be32(regs + FSL_SRDSRSTCTL_OFFS, tmp);
+		udelay(1000);
+		tmp &= ~FSL_SRDSRSTCTL_SATA_RESET;
+		out_be32(regs + FSL_SRDSRSTCTL_OFFS, tmp);
+
+		/* Configure SRDSCR1 */
+		tmp = in_be32(regs + FSL_SRDSCR1_OFFS);
+		tmp &= ~FSL_SRDSCR1_PLLBW;
+		out_be32(regs + FSL_SRDSCR1_OFFS, tmp);
+
+		/* Configure SRDSCR2 */
+		tmp = in_be32(regs + FSL_SRDSCR2_OFFS);
+		tmp &= ~FSL_SRDSCR2_SEIC_MASK;
+		tmp |= FSL_SRDSCR2_SEIC_SATA;
+		out_be32(regs + FSL_SRDSCR2_OFFS, tmp);
+
+		/* Configure SRDSCR3 */
+		tmp = FSL_SRDSCR3_KFR_SATA | FSL_SRDSCR3_KPH_SATA |
+			FSL_SRDSCR3_SDFM_SATA_PEX |
+			FSL_SRDSCR3_SDTXL_SATA;
+		out_be32(regs + FSL_SRDSCR3_OFFS, tmp);
+
+		/* Configure SRDSCR4 */
+		tmp = rfcks | FSL_SRDSCR4_PROT_SATA;
+		out_be32(regs + FSL_SRDSCR4_OFFS, tmp);
+		break;
+	case FSL_SERDES_PROTO_PEX:
+	case FSL_SERDES_PROTO_PEX_X2:
+		/* Configure SRDSCR1 */
+		tmp = in_be32(regs + FSL_SRDSCR1_OFFS);
+		tmp |= FSL_SRDSCR1_PLLBW;
+		out_be32(regs + FSL_SRDSCR1_OFFS, tmp);
+
+		/* Configure SRDSCR2 */
+		tmp = in_be32(regs + FSL_SRDSCR2_OFFS);
+		tmp &= ~FSL_SRDSCR2_SEIC_MASK;
+		tmp |= FSL_SRDSCR2_SEIC_PEX;
+		out_be32(regs + FSL_SRDSCR2_OFFS, tmp);
+
+		/* Configure SRDSCR3 */
+		tmp = FSL_SRDSCR3_SDFM_SATA_PEX;
+		out_be32(regs + FSL_SRDSCR3_OFFS, tmp);
+
+		/* Configure SRDSCR4 */
+		tmp = rfcks | FSL_SRDSCR4_PROT_PEX;
+		if (proto == FSL_SERDES_PROTO_PEX_X2)
+			tmp |= FSL_SRDSCR4_PLANE_X2;
+		out_be32(regs + FSL_SRDSCR4_OFFS, tmp);
+		break;
+	case FSL_SERDES_PROTO_SGMII:
+		/* Configure SRDSCR1 */
+		tmp = in_be32(regs + FSL_SRDSCR1_OFFS);
+		tmp &= ~FSL_SRDSCR1_PLLBW;
+		out_be32(regs + FSL_SRDSCR1_OFFS, tmp);
+
+		/* Configure SRDSCR2 */
+		tmp = in_be32(regs + FSL_SRDSCR2_OFFS);
+		tmp &= ~FSL_SRDSCR2_SEIC_MASK;
+		tmp |= FSL_SRDSCR2_SEIC_SGMII;
+		out_be32(regs + FSL_SRDSCR2_OFFS, tmp);
+
+		/* Configure SRDSCR3 */
+		out_be32(regs + FSL_SRDSCR3_OFFS, 0);
+
+		/* Configure SRDSCR4 */
+		tmp = rfcks | FSL_SRDSCR4_PROT_SGMII;
+		out_be32(regs + FSL_SRDSCR4_OFFS, tmp);
+		break;
+	default:
+		return;
+	}
+
+	/* Do a software reset */
+	tmp = in_be32(regs + FSL_SRDSRSTCTL_OFFS);
+	tmp |= FSL_SRDSRSTCTL_RST;
+	out_be32(regs + FSL_SRDSRSTCTL_OFFS, tmp);
+}
diff --git a/cpu/mpc85xx/Makefile b/cpu/mpc85xx/Makefile
index 2205dca..adbc585 100644
--- a/cpu/mpc85xx/Makefile
+++ b/cpu/mpc85xx/Makefile
@@ -29,6 +29,9 @@ include $(TOPDIR)/config.mk
 LIB	= $(obj)lib$(CPU).a
 
 START	= start.o resetvec.o
+SOBJS-$(CONFIG_MP) += release.o
+SOBJS	= $(SOBJS-y)
+COBJS-$(CONFIG_MP) += mp.o
 COBJS-$(CONFIG_OF_LIBFDT) += fdt.o
 COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o tlb.o \
 	  pci.o serial_scc.o commproc.o ether_fcc.o spd_sdram.o qe_io.o \
diff --git a/cpu/mpc85xx/cpu.c b/cpu/mpc85xx/cpu.c
index ac8b018..dcd8817 100644
--- a/cpu/mpc85xx/cpu.c
+++ b/cpu/mpc85xx/cpu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004,2007 Freescale Semiconductor, Inc.
+ * Copyright 2004,2007,2008 Freescale Semiconductor, Inc.
  * (C) Copyright 2002, 2003 Motorola Inc.
  * Xianghua Xiao (X.Xiao@motorola.com)
  *
@@ -30,6 +30,41 @@
 #include <command.h>
 #include <asm/cache.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
+struct cpu_type {
+	char name[15];
+	u32 soc_ver;
+};
+
+#define CPU_TYPE_ENTRY(x) {#x, SVR_##x}
+
+struct cpu_type cpu_type_list [] = {
+	CPU_TYPE_ENTRY(8533),
+	CPU_TYPE_ENTRY(8533_E),
+	CPU_TYPE_ENTRY(8540),
+	CPU_TYPE_ENTRY(8541),
+	CPU_TYPE_ENTRY(8541_E),
+	CPU_TYPE_ENTRY(8543),
+	CPU_TYPE_ENTRY(8543_E),
+	CPU_TYPE_ENTRY(8544),
+	CPU_TYPE_ENTRY(8544_E),
+	CPU_TYPE_ENTRY(8545),
+	CPU_TYPE_ENTRY(8545_E),
+	CPU_TYPE_ENTRY(8547_E),
+	CPU_TYPE_ENTRY(8548),
+	CPU_TYPE_ENTRY(8548_E),
+	CPU_TYPE_ENTRY(8555),
+	CPU_TYPE_ENTRY(8555_E),
+	CPU_TYPE_ENTRY(8560),
+	CPU_TYPE_ENTRY(8567),
+	CPU_TYPE_ENTRY(8567_E),
+	CPU_TYPE_ENTRY(8568),
+	CPU_TYPE_ENTRY(8568_E),
+	CPU_TYPE_ENTRY(8572),
+	CPU_TYPE_ENTRY(8572_E),
+};
+
 int checkcpu (void)
 {
 	sys_info_t sysinfo;
@@ -39,47 +74,26 @@ int checkcpu (void)
 	uint fam;
 	uint ver;
 	uint major, minor;
+	int i;
 	u32 ddr_ratio;
 	volatile ccsr_gur_t *gur = (void *)(CFG_MPC85xx_GUTS_ADDR);
 
 	svr = get_svr();
-	ver = SVR_VER(svr);
+	ver = SVR_SOC_VER(svr);
 	major = SVR_MAJ(svr);
 	minor = SVR_MIN(svr);
 
 	puts("CPU:   ");
-	switch (ver) {
-	case SVR_8540:
-		puts("8540");
-		break;
-	case SVR_8541:
-		puts("8541");
-		break;
-	case SVR_8555:
-		puts("8555");
-		break;
-	case SVR_8560:
-		puts("8560");
-		break;
-	case SVR_8548:
-		puts("8548");
-		break;
-	case SVR_8548_E:
-		puts("8548_E");
-		break;
-	case SVR_8544:
-		puts("8544");
-		break;
-	case SVR_8544_E:
-		puts("8544_E");
-		break;
-	case SVR_8568_E:
-		puts("8568_E");
-		break;
-	default:
+
+	for (i = 0; i < ARRAY_SIZE(cpu_type_list); i++)
+		if (cpu_type_list[i].soc_ver == ver) {
+			puts(cpu_type_list[i].name);
+			break;
+		}
+
+	if (i == ARRAY_SIZE(cpu_type_list))
 		puts("Unknown");
-		break;
-	}
+
 	printf(", Version: %d.%d, (0x%08x)\n", major, minor, svr);
 
 	pvr = get_pvr();
@@ -108,13 +122,16 @@ int checkcpu (void)
 	ddr_ratio = ((gur->porpllsr) & 0x00003e00) >> 9;
 	switch (ddr_ratio) {
 	case 0x0:
-		printf("       DDR:%4lu MHz, ", sysinfo.freqDDRBus / 2000000);
+		printf("       DDR:%4lu MHz (%lu MT/s data rate), ",
+		sysinfo.freqDDRBus / 2000000, sysinfo.freqDDRBus / 1000000);
 		break;
 	case 0x7:
-		printf("       DDR:%4lu MHz (Synchronous), ", sysinfo.freqDDRBus / 2000000);
+		printf("       DDR:%4lu MHz (%lu MT/s data rate) (Synchronous), ",
+		sysinfo.freqDDRBus / 2000000, sysinfo.freqDDRBus / 1000000);
 		break;
 	default:
-		printf("       DDR:%4lu MHz (Asynchronous), ", sysinfo.freqDDRBus / 2000000);
+		printf("       DDR:%4lu MHz (%lu MT/s data rate) (Asynchronous), ",
+		sysinfo.freqDDRBus / 2000000, sysinfo.freqDDRBus / 1000000);
 		break;
 	}
 
@@ -142,10 +159,9 @@ int checkcpu (void)
 		printf("LBC: unknown (lcrr: 0x%08x)\n", lcrr);
 	}
 
-	if (ver == SVR_8560) {
-		printf("CPM:  %lu Mhz\n",
-		       sysinfo.freqSystemBus / 1000000);
-	}
+#ifdef CONFIG_CPM2
+	printf("CPM:  %lu Mhz\n", sysinfo.freqSystemBus / 1000000);
+#endif
 
 	puts("L1:    D-cache 32 kB enabled\n       I-cache 32 kB enabled\n");
 
@@ -190,11 +206,7 @@ int do_reset (cmd_tbl_t *cmdtp, bd_t *bd, int flag, int argc, char *argv[])
  */
 unsigned long get_tbclk (void)
 {
-
-	sys_info_t  sys_info;
-
-	get_sys_info(&sys_info);
-	return ((sys_info.freqSystemBus + 7L) / 8L);
+	return (gd->bus_clk + 4UL)/8UL;
 }
 
 
diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index c0ff1d5..fce0c48 100644
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -33,6 +33,7 @@
 #include <asm/io.h>
 #include <asm/mmu.h>
 #include <asm/fsl_law.h>
+#include "mp.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -127,12 +128,12 @@ void config_8560_ioports (volatile ccsr_cpm_t * cpm)
 /* We run cpu_init_early_f in AS = 1 */
 void cpu_init_early_f(void)
 {
-	set_tlb(0, CFG_CCSRBAR, CFG_CCSRBAR,
+	set_tlb(0, CFG_CCSRBAR, CFG_CCSRBAR_PHYS,
 		MAS3_SX|MAS3_SW|MAS3_SR, MAS2_I|MAS2_G,
 		1, 0, BOOKE_PAGESZ_4K, 0);
 
 	/* set up CCSR if we want it moved */
-#if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
+#if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR_PHYS)
 	{
 		u32 temp;
 
@@ -141,7 +142,7 @@ void cpu_init_early_f(void)
 			1, 1, BOOKE_PAGESZ_4K, 0);
 
 		temp = in_be32((volatile u32 *)CFG_CCSRBAR_DEFAULT);
-		out_be32((volatile u32 *)CFG_CCSRBAR_DEFAULT, CFG_CCSRBAR >> 12);
+		out_be32((volatile u32 *)CFG_CCSRBAR_DEFAULT, CFG_CCSRBAR_PHYS >> 12);
 
 		temp = in_be32((volatile u32 *)CFG_CCSRBAR);
 	}
@@ -328,5 +329,8 @@ int cpu_init_r(void)
 	qe_reset();
 #endif
 
+#if defined(CONFIG_MP)
+	setup_mp();
+#endif
 	return 0;
 }
diff --git a/cpu/mpc85xx/fdt.c b/cpu/mpc85xx/fdt.c
index a6b014c..bde6d1e 100644
--- a/cpu/mpc85xx/fdt.c
+++ b/cpu/mpc85xx/fdt.c
@@ -28,6 +28,54 @@
 #include <fdt_support.h>
 
 extern void ft_qe_setup(void *blob);
+#ifdef CONFIG_MP
+#include "mp.h"
+DECLARE_GLOBAL_DATA_PTR;
+
+void ft_fixup_cpu(void *blob, u64 memory_limit)
+{
+	int off;
+	ulong spin_tbl_addr = get_spin_addr();
+	u32 bootpg, id = get_my_id();
+
+	/* 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;
+
+	off = fdt_node_offset_by_prop_value(blob, -1, "device_type", "cpu", 4);
+	while (off != -FDT_ERR_NOTFOUND) {
+		u32 *reg = (u32 *)fdt_getprop(blob, off, "reg", 0);
+
+		if (reg) {
+			if (*reg == id) {
+				fdt_setprop_string(blob, off, "status", "okay");
+			} else {
+				u32 val = *reg * SIZE_BOOT_ENTRY + spin_tbl_addr;
+				val = cpu_to_fdt32(val);
+				fdt_setprop_string(blob, off, "status",
+								"disabled");
+				fdt_setprop_string(blob, off, "enable-method",
+								"spin-table");
+				fdt_setprop(blob, off, "cpu-release-addr",
+						&val, sizeof(val));
+			}
+		} else {
+			printf ("cpu NULL\n");
+		}
+		off = fdt_node_offset_by_prop_value(blob, off,
+				"device_type", "cpu", 4);
+	}
+
+	/* Reserve the boot page so OSes dont use it */
+	if ((u64)bootpg < memory_limit) {
+		off = fdt_add_mem_rsv(blob, bootpg, (u64)4096);
+		if (off < 0)
+			printf("%s: %s\n", __FUNCTION__, fdt_strerror(off));
+	}
+}
+#endif
 
 void ft_cpu_setup(void *blob, bd_t *bd)
 {
@@ -62,4 +110,8 @@ void ft_cpu_setup(void *blob, bd_t *bd)
 #endif
 
 	fdt_fixup_memory(blob, (u64)bd->bi_memstart, (u64)bd->bi_memsize);
+
+#ifdef CONFIG_MP
+	ft_fixup_cpu(blob, (u64)bd->bi_memstart + (u64)bd->bi_memsize);
+#endif
 }
diff --git a/cpu/mpc85xx/mp.c b/cpu/mpc85xx/mp.c
new file mode 100644
index 0000000..7b10fba
--- /dev/null
+++ b/cpu/mpc85xx/mp.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright 2008 Freescale Semiconductor.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/processor.h>
+#include <ioports.h>
+#include <lmb.h>
+#include <asm/io.h>
+#include "mp.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+u32 get_my_id()
+{
+	return mfspr(SPRN_PIR);
+}
+
+int cpu_reset(int nr)
+{
+	volatile ccsr_pic_t *pic = (void *)(CFG_MPC85xx_PIC_ADDR);
+	out_be32(&pic->pir, 1 << nr);
+	(void)in_be32(&pic->pir);
+	out_be32(&pic->pir, 0x0);
+
+	return 0;
+}
+
+int cpu_status(int nr)
+{
+	u32 *table, id = get_my_id();
+
+	if (nr == id) {
+		table = (u32 *)get_spin_addr();
+		printf("table base @ 0x%08x\n", table);
+	} else {
+		table = (u32 *)get_spin_addr() + nr * NUM_BOOT_ENTRY;
+		printf("Running on cpu %d\n", id);
+		printf("\n");
+		printf("table @ 0x%08x:\n", table);
+		printf("   addr - 0x%08x\n", table[BOOT_ENTRY_ADDR_LOWER]);
+		printf("   pir  - 0x%08x\n", table[BOOT_ENTRY_PIR]);
+		printf("   r3   - 0x%08x\n", table[BOOT_ENTRY_R3_LOWER]);
+		printf("   r6   - 0x%08x\n", table[BOOT_ENTRY_R6_LOWER]);
+	}
+
+	return 0;
+}
+
+static u8 boot_entry_map[4] = {
+	0,
+	BOOT_ENTRY_PIR,
+	BOOT_ENTRY_R3_LOWER,
+	BOOT_ENTRY_R6_LOWER,
+};
+
+int cpu_release(int nr, int argc, char *argv[])
+{
+	u32 i, val, *table = (u32 *)get_spin_addr() + nr * NUM_BOOT_ENTRY;
+	u64 boot_addr;
+
+	if (nr == get_my_id()) {
+		printf("Invalid to release the boot core.\n\n");
+		return 1;
+	}
+
+	if (argc != 4) {
+		printf("Invalid number of arguments to release.\n\n");
+		return 1;
+	}
+
+#ifdef CFG_64BIT_STRTOUL
+	boot_addr = simple_strtoull(argv[0], NULL, 16);
+#else
+	boot_addr = simple_strtoul(argv[0], NULL, 16);
+#endif
+
+	/* handle pir, r3, r6 */
+	for (i = 1; i < 4; i++) {
+		if (argv[i][0] != '-') {
+			u8 entry = boot_entry_map[i];
+			val = simple_strtoul(argv[i], NULL, 16);
+			table[entry] = val;
+		}
+	}
+
+	table[BOOT_ENTRY_ADDR_UPPER] = (u32)(boot_addr >> 32);
+	table[BOOT_ENTRY_ADDR_LOWER] = (u32)(boot_addr & 0xffffffff);
+
+	return 0;
+}
+
+ulong get_spin_addr(void)
+{
+	extern ulong __secondary_start_page;
+	extern ulong __spin_table;
+
+	ulong addr =
+		(ulong)&__spin_table - (ulong)&__secondary_start_page;
+	addr += 0xfffff000;
+
+	return addr;
+}
+
+static void pq3_mp_up(unsigned long bootpg)
+{
+	u32 up, cpu_up_mask, whoami;
+	u32 *table = (u32 *)get_spin_addr();
+	volatile u32 bpcr;
+	volatile ccsr_local_ecm_t *ecm = (void *)(CFG_MPC85xx_ECM_ADDR);
+	volatile ccsr_gur_t *gur = (void *)(CFG_MPC85xx_GUTS_ADDR);
+	volatile ccsr_pic_t *pic = (void *)(CFG_MPC85xx_PIC_ADDR);
+	u32 devdisr;
+	int timeout = 10;
+
+	whoami = in_be32(&pic->whoami);
+	out_be32(&ecm->bptr, 0x80000000 | (bootpg >> 12));
+
+	/* disable time base at the platform */
+	devdisr = in_be32(&gur->devdisr);
+	if (whoami)
+		devdisr |= MPC85xx_DEVDISR_TB0;
+	else
+		devdisr |= MPC85xx_DEVDISR_TB1;
+	out_be32(&gur->devdisr, devdisr);
+
+	/* release the hounds */
+	up = ((1 << CONFIG_NR_CPUS) - 1);
+	bpcr = in_be32(&ecm->eebpcr);
+	bpcr |= (up << 24);
+	out_be32(&ecm->eebpcr, bpcr);
+	asm("sync; isync; msync");
+
+	cpu_up_mask = 1 << whoami;
+	/* wait for everyone */
+	while (timeout) {
+		int i;
+		for (i = 1; i < CONFIG_NR_CPUS; i++) {
+			if (table[i * NUM_BOOT_ENTRY])
+				cpu_up_mask |= (1 << i);
+		};
+
+		if ((cpu_up_mask & up) == up)
+			break;
+
+		udelay(100);
+		timeout--;
+	}
+
+	/* enable time base at the platform */
+	if (whoami)
+		devdisr |= MPC85xx_DEVDISR_TB1;
+	else
+		devdisr |= MPC85xx_DEVDISR_TB0;
+	out_be32(&gur->devdisr, devdisr);
+	mtspr(SPRN_TBWU, 0);
+	mtspr(SPRN_TBWL, 0);
+
+	devdisr &= ~(MPC85xx_DEVDISR_TB0 | MPC85xx_DEVDISR_TB1);
+	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;
+	ulong fixup = (ulong)&__secondary_start_page;
+	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;
+
+	memcpy((void *)bootpg, (void *)fixup, 4096);
+	flush_cache(bootpg, 4096);
+
+	pq3_mp_up(bootpg);
+}
diff --git a/cpu/mpc85xx/mp.h b/cpu/mpc85xx/mp.h
new file mode 100644
index 0000000..4329286
--- /dev/null
+++ b/cpu/mpc85xx/mp.h
@@ -0,0 +1,20 @@
+#ifndef __MPC85XX_MP_H_
+#define __MPC85XX_MP_H_
+
+ulong get_spin_addr(void);
+void setup_mp(void);
+u32 get_my_id(void);
+void cpu_mp_lmb_reserve(struct lmb *lmb);
+
+#define BOOT_ENTRY_ADDR_UPPER	0
+#define BOOT_ENTRY_ADDR_LOWER	1
+#define BOOT_ENTRY_R3_UPPER	2
+#define BOOT_ENTRY_R3_LOWER	3
+#define BOOT_ENTRY_RESV		4
+#define BOOT_ENTRY_PIR		5
+#define BOOT_ENTRY_R6_UPPER	6
+#define BOOT_ENTRY_R6_LOWER	7
+#define NUM_BOOT_ENTRY		8
+#define SIZE_BOOT_ENTRY		(NUM_BOOT_ENTRY * sizeof(u32))
+
+#endif
diff --git a/cpu/mpc85xx/release.S b/cpu/mpc85xx/release.S
new file mode 100644
index 0000000..3b7366f
--- /dev/null
+++ b/cpu/mpc85xx/release.S
@@ -0,0 +1,182 @@
+#include <config.h>
+#include <mpc85xx.h>
+#include <version.h>
+
+#define _LINUX_CONFIG_H 1	/* avoid reading Linux autoconf.h file	*/
+
+#include <ppc_asm.tmpl>
+#include <ppc_defs.h>
+
+#include <asm/cache.h>
+#include <asm/mmu.h>
+
+/* To boot secondary cpus, we need a place for them to start up.
+ * Normally, they start at 0xfffffffc, but that's usually the
+ * firmware, and we don't want to have to run the firmware again.
+ * Instead, the primary cpu will set the BPTR to point here to
+ * this page.  We then set up the core, and head to
+ * start_secondary.  Note that this means that the code below
+ * must never exceed 1023 instructions (the branch at the end
+ * would then be the 1024th).
+ */
+	.globl	__secondary_start_page
+	.align	12
+__secondary_start_page:
+/* First do some preliminary setup */
+	lis	r3, HID0_EMCP@h		/* enable machine check */
+	ori	r3,r3,HID0_TBEN@l	/* enable Timebase */
+#ifdef CONFIG_PHYS_64BIT
+	ori	r3,r3,HID0_ENMAS7@l	/* enable MAS7 updates */
+#endif
+	mtspr	SPRN_HID0,r3
+
+	li	r3,(HID1_ASTME|HID1_ABE)@l	/* Addr streaming & broadcast */
+	mtspr	SPRN_HID1,r3
+
+	/* Enable branch prediction */
+	li	r3,0x201
+	mtspr	SPRN_BUCSR,r3
+
+	/* Enable/invalidate the I-Cache */
+	mfspr	r0,SPRN_L1CSR1
+	ori	r0,r0,(L1CSR1_ICFI|L1CSR1_ICE)
+	mtspr	SPRN_L1CSR1,r0
+	isync
+
+	/* Enable/invalidate the D-Cache */
+	mfspr	r0,SPRN_L1CSR0
+	ori	r0,r0,(L1CSR0_DCFI|L1CSR0_DCE)
+	msync
+	isync
+	mtspr	SPRN_L1CSR0,r0
+	isync
+
+#define toreset(x) (x - __secondary_start_page + 0xfffff000)
+
+	/* get our PIR to figure out our table entry */
+	lis	r3,toreset(__spin_table)@h
+	ori	r3,r3,toreset(__spin_table)@l
+
+	/* r10 has the base address for the entry */
+	mfspr	r0,SPRN_PIR
+	mr	r4,r0
+	slwi	r8,r4,5
+	add	r10,r3,r8
+
+#define EPAPR_MAGIC		(0x45504150)
+#define ENTRY_ADDR_UPPER	0
+#define ENTRY_ADDR_LOWER	4
+#define ENTRY_R3_UPPER		8
+#define ENTRY_R3_LOWER		12
+#define ENTRY_RESV		16
+#define ENTRY_PIR		20
+#define ENTRY_R6_UPPER		24
+#define ENTRY_R6_LOWER		28
+#define ENTRY_SIZE		32
+
+	/* setup the entry */
+	li	r3,0
+	li	r8,1
+	stw	r0,ENTRY_PIR(r10)
+	stw	r3,ENTRY_ADDR_UPPER(r10)
+	stw	r8,ENTRY_ADDR_LOWER(r10)
+	stw	r3,ENTRY_R3_UPPER(r10)
+	stw	r4,ENTRY_R3_LOWER(r10)
+	stw	r3,ENTRY_R6_UPPER(r10)
+	stw	r3,ENTRY_R6_LOWER(r10)
+
+	/* 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
+	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
+	mtspr	SPRN_MAS3,r11
+	tlbwe
+
+	bl	1f
+1:	mflr	r11
+	addi	r11,r11,28
+	mfmsr	r13
+	ori	r12,r13,MSR_IS|MSR_DS@l
+
+	mtspr	SPRN_SRR0,r11
+	mtspr	SPRN_SRR1,r12
+	rfi
+
+	/* spin waiting for addr */
+2:
+	lwz	r4,ENTRY_ADDR_LOWER(r10)
+	andi.	r11,r4,1
+	bne	2b
+
+	/* get the upper bits of the addr */
+	lwz	r11,ENTRY_ADDR_UPPER(r10)
+
+	/* setup branch addr */
+	mtspr	SPRN_SRR0,r4
+
+	/* mark the entry as released */
+	li	r8,3
+	stw	r8,ENTRY_ADDR_LOWER(r10)
+
+	/* mask by ~64M to setup our tlb we will jump to */
+	rlwinm	r12,r4,0,0,5
+
+	/* setup r3, r4, r5, r6, r7, r8, r9 */
+	lwz	r3,ENTRY_R3_LOWER(r10)
+	li	r4,0
+	li	r5,0
+	lwz	r6,ENTRY_R6_LOWER(r10)
+	lis	r7,(64*1024*1024)@h
+	li	r8,0
+	li	r9,0
+
+	/* load up the pir */
+	lwz	r0,ENTRY_PIR(r10)
+	mtspr	SPRN_PIR,r0
+	mfspr	r0,SPRN_PIR
+	stw	r0,ENTRY_PIR(r10)
+
+/*
+ * Coming here, we know the cpu has one TLB mapping in TLB1[0]
+ * which maps 0xfffff000-0xffffffff one-to-one.  We set up a
+ * second mapping that maps addr 1:1 for 64M, and then we jump to
+ * addr
+ */
+	lis	r10,(MAS0_TLBSEL(1)|MAS0_ESEL(0))@h
+	mtspr	SPRN_MAS0,r10
+	lis	r10,(MAS1_VALID|MAS1_IPROT)@h
+	ori	r10,r10,(MAS1_TSIZE(BOOKE_PAGESZ_64M))@l
+	mtspr	SPRN_MAS1,r10
+	/* WIMGE = 0b00000 for now */
+	mtspr	SPRN_MAS2,r12
+	ori	r12,r12,(MAS3_SX|MAS3_SW|MAS3_SR)
+	mtspr	SPRN_MAS3,r12
+#ifdef CONFIG_ENABLE_36BIT_PHYS
+	mtspr	SPRN_MAS7,r11
+#endif
+	tlbwe
+
+/* Now we have another mapping for this page, so we jump to that
+ * mapping
+ */
+	mtspr	SPRN_SRR1,r13
+	rfi
+
+	.align 3
+	.globl __spin_table
+__spin_table:
+	.space CONFIG_NR_CPUS*ENTRY_SIZE
+
+	/* Fill in the empty space.  The actual reset vector is
+	 * the last word of the page */
+__secondary_start_code_end:
+	.space 4092 - (__secondary_start_code_end - __secondary_start_page)
+__secondary_reset_vector:
+	b	__secondary_start_page
diff --git a/cpu/mpc85xx/speed.c b/cpu/mpc85xx/speed.c
index 952f30c..d90d397 100644
--- a/cpu/mpc85xx/speed.c
+++ b/cpu/mpc85xx/speed.c
@@ -48,6 +48,8 @@ void get_sys_info (sys_info_t * sysInfo)
 	 * overflow for processor speeds above 2GHz */
 	half_freqSystemBus = sysInfo->freqSystemBus/2;
 	sysInfo->freqProcessor = e500_ratio*half_freqSystemBus;
+
+	/* Note: freqDDRBus is the MCLK frequency, not the data rate. */
 	sysInfo->freqDDRBus = sysInfo->freqSystemBus;
 
 #ifdef CONFIG_DDR_CLK_FREQ
@@ -75,6 +77,7 @@ int get_clocks (void)
 	get_sys_info (&sys_info);
 	gd->cpu_clk = sys_info.freqProcessor;
 	gd->bus_clk = sys_info.freqSystemBus;
+	gd->mem_clk = sys_info.freqDDRBus;
 	gd->i2c1_clk = sys_info.freqSystemBus;
 	gd->i2c2_clk = sys_info.freqSystemBus;
 
@@ -96,14 +99,7 @@ int get_clocks (void)
  *********************************************/
 ulong get_bus_freq (ulong dummy)
 {
-	ulong val;
-
-	sys_info_t sys_info;
-
-	get_sys_info (&sys_info);
-	val = sys_info.freqSystemBus;
-
-	return val;
+	return gd->bus_clk;
 }
 
 /********************************************
@@ -112,12 +108,5 @@ ulong get_bus_freq (ulong dummy)
  *********************************************/
 ulong get_ddr_freq (ulong dummy)
 {
-	ulong val;
-
-	sys_info_t sys_info;
-
-	get_sys_info (&sys_info);
-	val = sys_info.freqDDRBus;
-
-	return val;
+	return gd->mem_clk;
 }
diff --git a/cpu/mpc86xx/cpu.c b/cpu/mpc86xx/cpu.c
index bf4e651..3c74764 100644
--- a/cpu/mpc86xx/cpu.c
+++ b/cpu/mpc86xx/cpu.c
@@ -69,7 +69,7 @@ checkcpu(void)
 	printf(", Version: %d.%d, (0x%08x)\n", major, minor, pvr);
 
 	svr = get_svr();
-	ver = SVR_VER(svr);
+	ver = SVR_SOC_VER(svr);
 	major = SVR_MAJ(svr);
 	minor = SVR_MIN(svr);
 
diff --git a/cpu/mpc8xx/Makefile b/cpu/mpc8xx/Makefile
index 223b30c..dbdc2e0 100644
--- a/cpu/mpc8xx/Makefile
+++ b/cpu/mpc8xx/Makefile
@@ -29,7 +29,7 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o kgdb.o
 COBJS	= bedbug_860.o commproc.o cpu.o cpu_init.o	\
-	  fec.o i2c.o interrupts.o lcd.o scc.o		\
+	  fec.o fdt.o i2c.o interrupts.o lcd.o scc.o	\
 	  serial.o speed.o spi.o \
 	  traps.o upatch.o video.o
 SOBJS	= plprcr_write.o
diff --git a/cpu/mpc8xx/cpu.c b/cpu/mpc8xx/cpu.c
index c878352..ec6a3fd 100644
--- a/cpu/mpc8xx/cpu.c
+++ b/cpu/mpc8xx/cpu.c
@@ -634,17 +634,4 @@ void reset_8xx_watchdog (volatile immap_t * immr)
 	immr->im_siu_conf.sc_swsr = 0xaa39;	/* write magic2 */
 # endif /* CONFIG_LWMON */
 }
-
 #endif /* CONFIG_WATCHDOG */
-
-/* ------------------------------------------------------------------------- */
-#if defined(CONFIG_OF_LIBFDT)
-void ft_cpu_setup (void *blob, bd_t *bd)
-{
-	char * cpu_path = "/cpus/" OF_CPU;
-
-	do_fixup_by_path_u32(blob, cpu_path, "bus-frequency", bd->bi_busfreq, 1);
-	do_fixup_by_path_u32(blob, cpu_path, "timebase-frequency", OF_TBCLK, 1);
-	do_fixup_by_path_u32(blob, cpu_path, "clock-frequency", bd->bi_intfreq, 1);
-}
-#endif /* CONFIG_OF_LIBFDT */
diff --git a/cpu/mpc8xx/fdt.c b/cpu/mpc8xx/fdt.c
new file mode 100644
index 0000000..567094a
--- /dev/null
+++ b/cpu/mpc8xx/fdt.c
@@ -0,0 +1,46 @@
+/*
+ * Copyright 2008 (C) Bryan O'Donoghue
+ *
+ * Code copied & edited from Freescale mpc85xx stuff.
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <libfdt.h>
+#include <fdt_support.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+void ft_cpu_setup(void *blob, bd_t *bd)
+{
+	do_fixup_by_prop_u32(blob, "device_type", "cpu", 4,
+		"timebase-frequency", get_tbclk(), 1);
+	do_fixup_by_prop_u32(blob, "device_type", "cpu", 4,
+		"bus-frequency", bd->bi_busfreq, 1);
+	do_fixup_by_prop_u32(blob, "device_type", "cpu", 4,
+		"clock-frequency", bd->bi_intfreq, 1);
+	do_fixup_by_compat_u32(blob, "fsl,cpm-brg", "clock-frequency",
+		gd->brg_clk, 1);
+
+	/* Fixup ethernet MAC addresses */
+	fdt_fixup_ethernet(blob, bd);
+
+	fdt_fixup_memory(blob, (u64)bd->bi_memstart, (u64)bd->bi_memsize);
+}
diff --git a/cpu/mpc8xx/speed.c b/cpu/mpc8xx/speed.c
index 11b0893..070babc 100644
--- a/cpu/mpc8xx/speed.c
+++ b/cpu/mpc8xx/speed.c
@@ -174,6 +174,27 @@ unsigned long measure_gclk(void)
 
 #endif
 
+void get_brgclk(uint sccr)
+{
+	uint divider = 0;
+
+	switch((sccr&SCCR_DFBRG11)>>11){
+		case 0:
+			divider = 1;
+			break;
+		case 1:
+			divider = 4;
+			break;
+		case 2:
+			divider = 16;
+			break;
+		case 3:
+			divider = 64;
+			break;
+	}
+	gd->brg_clk = gd->cpu_clk/divider;
+}
+
 #if !defined(CONFIG_8xx_CPUCLK_DEFAULT)
 
 /*
@@ -223,6 +244,8 @@ int get_clocks (void)
 		gd->bus_clk = gd->cpu_clk / 2;
 	}
 
+	get_brgclk(sccr);
+
 	return (0);
 }
 
@@ -254,6 +277,8 @@ int get_clocks_866 (void)
 	gd->cpu_clk = measure_gclk ();
 #endif
 
+	get_brgclk(immr->im_clkrst.car_sccr);
+
 	/* if cpu clock <= 66 MHz then set bus division factor to 1,
 	 * otherwise set it to 2
 	 */