Merge with /home/wd/git/u-boot/custodian/u-boot-mpc85xx

9 files changed, 567 insertions, 331 deletions

diff --git a/cpu/mpc85xx/Makefile b/cpu/mpc85xx/Makefile
index ff67dcd..32091fa 100644
--- a/cpu/mpc85xx/Makefile
+++ b/cpu/mpc85xx/Makefile
@@ -30,7 +30,7 @@ LIB	= $(obj)lib$(CPU).a
 
 START	= start.o resetvec.o
 COBJS	= traps.o cpu.o cpu_init.o speed.o interrupts.o \
-	  pci.o serial_scc.o commproc.o ether_fcc.o spd_sdram.o
+	  pci.o serial_scc.o commproc.o ether_fcc.o spd_sdram.o qe_io.o
 
 SRCS	:= $(START:.o=.S) $(SOBJS:.o=.S) $(COBJS:.o=.c)
 OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
diff --git a/cpu/mpc85xx/cpu.c b/cpu/mpc85xx/cpu.c
index 1d791c9..08e0468 100644
--- a/cpu/mpc85xx/cpu.c
+++ b/cpu/mpc85xx/cpu.c
@@ -280,7 +280,7 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	if (p != NULL)
 		*p = cpu_to_be32(clock);
 
-#if defined(CONFIG_TSEC1)
+#if defined(CONFIG_HAS_ETH0)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@24000/mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enetaddr, 6);
@@ -308,6 +308,17 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@26000/local-mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enet2addr, 6);
+
+#ifdef CONFIG_UEC_ETH
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet2addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@2000/local-mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet2addr, 6);
+
+#endif
 #endif
 
 #if defined(CONFIG_HAS_ETH3)
@@ -318,6 +329,17 @@ ft_cpu_setup(void *blob, bd_t *bd)
 	p = ft_get_prop(blob, "/" OF_SOC "/ethernet@27000/local-mac-address", &len);
 	if (p)
 		memcpy(p, bd->bi_enet3addr, 6);
+
+#ifdef CONFIG_UEC_ETH
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet3addr, 6);
+
+	p = ft_get_prop(blob, "/" OF_QE "/ucc@3000/local-mac-address", &len);
+	if (p)
+		memcpy(p, bd->bi_enet3addr, 6);
+
+#endif
 #endif
 
 }
diff --git a/cpu/mpc85xx/cpu_init.c b/cpu/mpc85xx/cpu_init.c
index 9517146..7b99610 100644
--- a/cpu/mpc85xx/cpu_init.c
+++ b/cpu/mpc85xx/cpu_init.c
@@ -1,4 +1,6 @@
 /*
+ * Copyright 2007 Freescale Semiconductor.
+ *
  * (C) Copyright 2003 Motorola Inc.
  * Modified by Xianghua Xiao, X.Xiao@motorola.com
  *
@@ -32,6 +34,29 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+#ifdef CONFIG_QE
+extern qe_iop_conf_t qe_iop_conf_tab[];
+extern void qe_config_iopin(u8 port, u8 pin, int dir,
+				int open_drain, int assign);
+extern void qe_init(uint qe_base);
+extern void qe_reset(void);
+
+static void config_qe_ioports(void)
+{
+	u8      port, pin;
+	int     dir, open_drain, assign;
+	int     i;
+
+	for (i = 0; qe_iop_conf_tab[i].assign != QE_IOP_TAB_END; i++) {
+		port		= qe_iop_conf_tab[i].port;
+		pin		= qe_iop_conf_tab[i].pin;
+		dir		= qe_iop_conf_tab[i].dir;
+		open_drain	= qe_iop_conf_tab[i].open_drain;
+		assign		= qe_iop_conf_tab[i].assign;
+		qe_config_iopin(port, pin, dir, open_drain, assign);
+	}
+}
+#endif
 
 #ifdef CONFIG_CPM2
 static void config_8560_ioports (volatile immap_t * immr)
@@ -133,15 +158,18 @@ void cpu_init_f (void)
 #endif
 
 	/* now restrict to preliminary range */
+	/* if cs1 is already set via debugger, leave cs0/cs1 alone */
+	if (! memctl->br1 & 1) {
 #if defined(CFG_BR0_PRELIM) && defined(CFG_OR0_PRELIM)
-	memctl->br0 = CFG_BR0_PRELIM;
-	memctl->or0 = CFG_OR0_PRELIM;
+		memctl->br0 = CFG_BR0_PRELIM;
+		memctl->or0 = CFG_OR0_PRELIM;
 #endif
 
 #if defined(CFG_BR1_PRELIM) && defined(CFG_OR1_PRELIM)
-	memctl->or1 = CFG_OR1_PRELIM;
-	memctl->br1 = CFG_BR1_PRELIM;
+		memctl->or1 = CFG_OR1_PRELIM;
+		memctl->br1 = CFG_BR1_PRELIM;
 #endif
+	}
 
 #if defined(CFG_BR2_PRELIM) && defined(CFG_OR2_PRELIM)
 	memctl->or2 = CFG_OR2_PRELIM;
@@ -176,6 +204,11 @@ void cpu_init_f (void)
 #if defined(CONFIG_CPM2)
 	m8560_cpm_reset();
 #endif
+#ifdef CONFIG_QE
+	/* Config QE ioports */
+	config_qe_ioports();
+#endif
+
 }
 
 
@@ -185,16 +218,25 @@ void cpu_init_f (void)
  * The newer 8548, etc, parts have twice as much cache, but
  * use the same bit-encoding as the older 8555, etc, parts.
  *
- * FIXME: Use PVR_VER(pvr) == 1 test here instead of SVR_VER()?
  */
 
 int cpu_init_r(void)
 {
+#if defined(CONFIG_CLEAR_LAW0) || defined(CONFIG_L2_CACHE)
+	volatile immap_t    *immap = (immap_t *)CFG_IMMR;
+#endif
+#ifdef CONFIG_CLEAR_LAW0
+	volatile ccsr_local_ecm_t *ecm = &immap->im_local_ecm;
+
+	/* clear alternate boot location LAW (used for sdram, or ddr bank) */
+	ecm->lawar0 = 0;
+#endif
+
 #if defined(CONFIG_L2_CACHE)
-	volatile immap_t *immap = (immap_t *)CFG_IMMR;
 	volatile ccsr_l2cache_t *l2cache = &immap->im_l2cache;
 	volatile uint cache_ctl;
 	uint svr, ver;
+	uint l2srbar;
 
 	svr = get_svr();
 	ver = SVR_VER(svr);
@@ -204,33 +246,55 @@ int cpu_init_r(void)
 
 	switch (cache_ctl & 0x30000000) {
 	case 0x20000000:
-		if (ver == SVR_8548 || ver == SVR_8548_E) {
+		if (ver == SVR_8548 || ver == SVR_8548_E ||
+		    ver == SVR_8544) {
 			printf ("L2 cache 512KB:");
+			/* set L2E=1, L2I=1, & L2SRAM=0 */
+			cache_ctl = 0xc0000000;
 		} else {
 			printf ("L2 cache 256KB:");
+			/* set L2E=1, L2I=1, & L2BLKSZ=2 (256 Kbyte) */
+			cache_ctl = 0xc8000000;
 		}
 		break;
-	case 0x00000000:
 	case 0x10000000:
+		printf ("L2 cache 256KB:");
+		if (ver == SVR_8544 || ver == SVR_8544_E) {
+			cache_ctl = 0xc0000000; /* set L2E=1, L2I=1, & L2SRAM=0 */
+		}
+		break;
 	case 0x30000000:
+	case 0x00000000:
 	default:
 		printf ("L2 cache unknown size (0x%08x)\n", cache_ctl);
 		return -1;
 	}
 
-	asm("msync;isync");
-	l2cache->l2ctl = 0x68000000; /* invalidate */
-	cache_ctl = l2cache->l2ctl;
-	asm("msync;isync");
-
-	l2cache->l2ctl = 0xa8000000; /* enable 256KB L2 cache */
-	cache_ctl = l2cache->l2ctl;
-	asm("msync;isync");
-
-	printf(" enabled\n");
+	if (l2cache->l2ctl & 0x80000000) {
+		printf(" already enabled.");
+		l2srbar = l2cache->l2srbar0;
+#ifdef CFG_INIT_L2_ADDR
+		if (l2cache->l2ctl & 0x00010000 && l2srbar >= CFG_FLASH_BASE) {
+			l2srbar = CFG_INIT_L2_ADDR;
+			l2cache->l2srbar0 = l2srbar;
+			printf("  Moving to 0x%08x", CFG_INIT_L2_ADDR);
+		}
+#endif /* CFG_INIT_L2_ADDR */
+		puts("\n");
+	} else {
+		asm("msync;isync");
+		l2cache->l2ctl = cache_ctl; /* invalidate & enable */
+		asm("msync;isync");
+		printf(" enabled\n");
+	}
 #else
 	printf("L2 cache: disabled\n");
 #endif
+#ifdef CONFIG_QE
+	uint qe_base = CFG_IMMR + 0x00080000; /* QE immr base */
+	qe_init(qe_base);
+	qe_reset();
+#endif
 
 	return 0;
 }
diff --git a/cpu/mpc85xx/interrupts.c b/cpu/mpc85xx/interrupts.c
index dc246dc..bf737d6 100644
--- a/cpu/mpc85xx/interrupts.c
+++ b/cpu/mpc85xx/interrupts.c
@@ -89,6 +89,39 @@ int interrupt_init (void)
 	mtspr(SPRN_TCR, TCR_PIE);
 	set_dec (decrementer_count);
 	set_msr (get_msr () | MSR_EE);
+
+#ifdef CONFIG_INTERRUPTS
+	volatile ccsr_pic_t *pic = &immr->im_pic;
+
+	pic->iivpr1 = 0x810002;	/* 50220 enable ecm interrupts */
+	debug("iivpr1@%x = %x\n",&pic->iivpr1, pic->iivpr1);
+
+	pic->iivpr2 = 0x810002;	/* 50240 enable ddr interrupts */
+	debug("iivpr2@%x = %x\n",&pic->iivpr2, pic->iivpr2);
+
+	pic->iivpr3 = 0x810003;	/* 50260 enable lbc interrupts */
+	debug("iivpr3@%x = %x\n",&pic->iivpr3, pic->iivpr3);
+
+#ifdef CONFIG_PCI1
+	pic->iivpr8 = 0x810008;	/* enable pci1 interrupts */
+	debug("iivpr8@%x = %x\n",&pic->iivpr8, pic->iivpr8);
+#endif
+#if defined(CONFIG_PCI2) || defined(CONFIG_PCIE2)
+	pic->iivpr9 = 0x810009;	/* enable pci1 interrupts */
+	debug("iivpr9@%x = %x\n",&pic->iivpr9, pic->iivpr9);
+#endif
+#ifdef CONFIG_PCIE1
+	pic->iivpr10 = 0x81000a;	/* enable pcie1 interrupts */
+	debug("iivpr10@%x = %x\n",&pic->iivpr10, pic->iivpr10);
+#endif
+#ifdef CONFIG_PCIE3
+	pic->iivpr11 = 0x81000b;	/* enable pcie3 interrupts */
+	debug("iivpr11@%x = %x\n",&pic->iivpr11, pic->iivpr11);
+#endif
+
+	pic->ctpr=0;		/* 40080 clear current task priority register */
+#endif
+
 	return (0);
 }
 
diff --git a/cpu/mpc85xx/pci.c b/cpu/mpc85xx/pci.c
index 3c1a323..db09e45 100644
--- a/cpu/mpc85xx/pci.c
+++ b/cpu/mpc85xx/pci.c
@@ -142,7 +142,7 @@ pci_mpc85xx_init(struct pci_controller *board_hose)
 		u8 header_type;
 
 		pci_hose_read_config_byte(hose,
-					  PCI_BDF(0,17,0),
+					  PCI_BDF(0,BRIDGE_ID,0),
 					  PCI_HEADER_TYPE,
 					  &header_type);
 	}
diff --git a/cpu/mpc85xx/qe_io.c b/cpu/mpc85xx/qe_io.c
new file mode 100644
index 0000000..8878bc5
--- /dev/null
+++ b/cpu/mpc85xx/qe_io.c
@@ -0,0 +1,85 @@
+/*
+ * Copyright (C) 2006 Freescale Semiconductor, Inc.
+ *
+ * Dave Liu <daveliu@freescale.com>
+ * based on source code of Shlomi Gridish
+ *
+ * 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/errno.h"
+#include "asm/io.h"
+#include "asm/immap_85xx.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)
+{
+	u32			pin_2bit_mask;
+	u32			pin_2bit_dir;
+	u32			pin_2bit_assign;
+	u32			pin_1bit_mask;
+	u32			tmp_val;
+	volatile immap_t	*im = (volatile immap_t *)CFG_IMMR;
+	volatile par_io_t	*par_io = (volatile par_io_t *)
+						&(im->im_gur.qe_par_io);
+
+	/* Caculate pin location and 2bit mask and dir */
+	pin_2bit_mask = (u32)(0x3 << (NUM_OF_PINS-(pin%(NUM_OF_PINS/2)+1)*2));
+	pin_2bit_dir = (u32)(dir << (NUM_OF_PINS-(pin%(NUM_OF_PINS/2)+1)*2));
+
+	/* Setup the direction */
+	tmp_val = (pin > (NUM_OF_PINS/2) - 1) ? \
+		in_be32(&par_io[port].cpdir2) :
+		in_be32(&par_io[port].cpdir1);
+
+	if (pin > (NUM_OF_PINS/2) -1) {
+		out_be32(&par_io[port].cpdir2, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cpdir2, pin_2bit_dir | tmp_val);
+	} else {
+		out_be32(&par_io[port].cpdir1, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cpdir1, pin_2bit_dir | tmp_val);
+	}
+
+	/* Calculate pin location for 1bit mask */
+	pin_1bit_mask = (u32)(1 << (NUM_OF_PINS - (pin+1)));
+
+	/* Setup the open drain */
+	tmp_val = in_be32(&par_io[port].cpodr);
+	if (open_drain)
+		out_be32(&par_io[port].cpodr, pin_1bit_mask | tmp_val);
+	else
+		out_be32(&par_io[port].cpodr, ~pin_1bit_mask & tmp_val);
+
+	/* Setup the assignment */
+	tmp_val = (pin > (NUM_OF_PINS/2) - 1) ?
+		in_be32(&par_io[port].cppar2):
+		in_be32(&par_io[port].cppar1);
+	pin_2bit_assign = (u32)(assign
+				<< (NUM_OF_PINS - (pin%(NUM_OF_PINS/2)+1)*2));
+
+	/* Clear and set 2 bits mask */
+	if (pin > (NUM_OF_PINS/2) - 1) {
+		out_be32(&par_io[port].cppar2, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cppar2, pin_2bit_assign | tmp_val);
+	} else {
+		out_be32(&par_io[port].cppar1, ~pin_2bit_mask & tmp_val);
+		out_be32(&par_io[port].cppar1, pin_2bit_assign | tmp_val);
+	}
+}
+
+#endif /* CONFIG_QE */
diff --git a/cpu/mpc85xx/spd_sdram.c b/cpu/mpc85xx/spd_sdram.c
index 3777f49..5dc223a 100644
--- a/cpu/mpc85xx/spd_sdram.c
+++ b/cpu/mpc85xx/spd_sdram.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004 Freescale Semiconductor.
+ * Copyright 2004, 2007 Freescale Semiconductor.
  * (C) Copyright 2003 Motorola Inc.
  * Xianghua Xiao (X.Xiao@motorola.com)
  *
@@ -173,11 +173,10 @@ spd_sdram(void)
 {
 	volatile immap_t *immap = (immap_t *)CFG_IMMR;
 	volatile ccsr_ddr_t *ddr = &immap->im_ddr;
-	volatile ccsr_gur_t *gur = &immap->im_gur;
 	spd_eeprom_t spd;
 	unsigned int n_ranks;
 	unsigned int rank_density;
-	unsigned int odt_rd_cfg, odt_wr_cfg;
+	unsigned int odt_rd_cfg, odt_wr_cfg, ba_bits;
 	unsigned int odt_cfg, mode_odt_enable;
 	unsigned int refresh_clk;
 #ifdef MPC85xx_DDR_SDRAM_CLK_CNTL
@@ -189,7 +188,7 @@ spd_sdram(void)
 	unsigned int max_data_rate, effective_data_rate;
 	unsigned int busfreq;
 	unsigned sdram_cfg;
-	unsigned int memsize;
+	unsigned int memsize = 0;
 	unsigned char caslat, caslat_ctrl;
 	unsigned int trfc, trfc_clk, trfc_low, trfc_high;
 	unsigned int trcd_clk;
@@ -204,6 +203,46 @@ spd_sdram(void)
 	unsigned int mode_caslat;
 	unsigned char sdram_type;
 	unsigned char d_init;
+	unsigned int bnds;
+
+	/*
+	 * Skip configuration if already configured.
+	 * memsize is determined from last configured chip select.
+	 */
+	if (ddr->cs0_config & 0x80000000) {
+		debug(" cs0 already configured, bnds=%x\n",ddr->cs0_bnds);
+		bnds = 0xfff & ddr->cs0_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+	if (ddr->cs1_config & 0x80000000) {
+		debug(" cs1 already configured, bnds=%x\n",ddr->cs1_bnds);
+		bnds = 0xfff & ddr->cs1_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4; /* assume ordered bnds */
+		}
+	}
+	if (ddr->cs2_config & 0x80000000) {
+		debug(" cs2 already configured, bnds=%x\n",ddr->cs2_bnds);
+		bnds = 0xfff & ddr->cs2_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+	if (ddr->cs3_config & 0x80000000) {
+		debug(" cs3 already configured, bnds=%x\n",ddr->cs3_bnds);
+		bnds = 0xfff & ddr->cs3_bnds;
+		if (bnds < 0xff) { /* do not add if at top of 4G */
+			memsize = (bnds + 1) << 4;
+		}
+	}
+
+	if (memsize) {
+		printf("       Reusing current %dMB configuration\n",memsize);
+		memsize = setup_laws_and_tlbs(memsize);
+		return memsize << 20;
+	}
 
 	/*
 	 * Read SPD information.
@@ -262,6 +301,7 @@ spd_sdram(void)
 		return 0;
 	}
 
+#ifdef CONFIG_MPC8548
 	/*
 	 * Adjust DDR II IO voltage biasing.
 	 * Only 8548 rev 1 needs the fix
@@ -269,9 +309,11 @@ spd_sdram(void)
 	if ((SVR_VER(get_svr()) == SVR_8548_E) &&
 			(SVR_MJREV(get_svr()) == 1) &&
 			(spd.mem_type == SPD_MEMTYPE_DDR2)) {
+		volatile ccsr_gur_t *gur = &immap->im_gur;
 		gur->ddrioovcr = (0x80000000	/* Enable */
 				  | 0x10000000);/* VSEL to 1.8V */
 	}
+#endif
 
 	/*
 	 * Determine the size of each Rank in bytes.
@@ -299,9 +341,14 @@ spd_sdram(void)
 #endif
 	}
 
+	ba_bits = 0;
+	if (spd.nbanks == 0x8)
+		ba_bits = 1;
+
 	ddr->cs0_config = ( 1 << 31
 			    | (odt_rd_cfg << 20)
 			    | (odt_wr_cfg << 16)
+			    | (ba_bits << 14)
 			    | (spd.nrow_addr - 12) << 8
 			    | (spd.ncol_addr - 8) );
 	debug("\n");
@@ -645,13 +692,10 @@ spd_sdram(void)
 	 */
 	cpo = 0;
 	if (spd.mem_type == SPD_MEMTYPE_DDR2) {
-		if (effective_data_rate == 266 || effective_data_rate == 333) {
+		if (effective_data_rate <= 333) {
 			cpo = 0x7;		/* READ_LAT + 5/4 */
-		} else if (effective_data_rate == 400) {
-			cpo = 0x9;		/* READ_LAT + 7/4 */
 		} else {
-			/* Pure speculation */
-			cpo = 0xb;
+			cpo = 0x9;		/* READ_LAT + 7/4 */
 		}
 	}
 
@@ -858,7 +902,12 @@ spd_sdram(void)
 	if (spd.mem_type == SPD_MEMTYPE_DDR)
 		clk_adjust = 0x6;
 	else
+#ifdef CONFIG_MPC8568
+		/* Empirally setting clk_adjust */
+		clk_adjust = 0x6;
+#else
 		clk_adjust = 0x7;
+#endif
 
 	ddr->sdram_clk_cntl = (0
 			       | 0x80000000
diff --git a/cpu/mpc85xx/start.S b/cpu/mpc85xx/start.S
index 77c155c..2c98c2a 100644
--- a/cpu/mpc85xx/start.S
+++ b/cpu/mpc85xx/start.S
@@ -1,7 +1,6 @@
 /*
- * Copyright 2004 Freescale Semiconductor.
+ * Copyright 2004, 2007 Freescale Semiconductor.
  * Copyright (C) 2003  Motorola,Inc.
- * Xianghua Xiao<X.Xiao@motorola.com>
  *
  * See file CREDITS for list of people who contributed to this
  * project.
@@ -46,7 +45,7 @@
 #endif
 
 #undef	MSR_KERNEL
-#define MSR_KERNEL ( MSR_ME  )	/* Machine Check */
+#define MSR_KERNEL ( MSR_ME )	/* Machine Check */
 
 /*
  * Set up GOT: Global Offset Table
@@ -80,110 +79,37 @@
  *
  */
 
-    .section .bootpg,"ax"
-    .globl _start_e500
+	.section .bootpg,"ax"
+	.globl _start_e500
 
 _start_e500:
-	mfspr	r0, PVR
-	lis	r1, PVR_85xx_REV1@h
-	ori	r1, r1, PVR_85xx_REV1@l
-	cmpw	r0, r1
-	bne	1f
 
-	/* Semi-bogus errata fixup for Rev 1 */
-	li	r0,0x2000
-	mtspr	977,r0
+/* clear registers/arrays not reset by hardware */
 
-	/*
-	 * Before invalidating MMU L1/L2, read TLB1 Entry 0 and then
-	 * write it back immediately to fixup a Rev 1 bug (Errata CPU4)
-	 * for this initial TLB1 entry 0, otherwise the TLB1 entry 0
-	 * will be invalidated (incorrectly).
-	 */
-	lis	r2,0x1000
-	mtspr	MAS0,r2
-	tlbre
-	tlbwe
-	isync
-
-1:
-	/*
-	 * Clear and set up some registers.
-	 * Note: Some registers need strict synchronization by
-	 * sync/mbar/msync/isync when being "mtspr".
-	 * BookE: isync before PID,tlbivax,tlbwe
-	 * BookE: isync after MSR,PID; msync_isync after tlbivax & tlbwe
-	 * E500:  msync,isync before L1CSR0
-	 * E500:  isync after BBEAR,BBTAR,BUCSR,DBCR0,DBCR1,HID0,HID1,
-	 *	  L1CSR0, L1CSR1, MAS[0,1,2,3,4,6],MMUCSR0, PID[0,1,2],
-	 *	  SPEFCSR
-	 */
-
-	/* invalidate d-cache */
-	mfspr	r0,L1CSR0
-	ori	r0,r0,0x0002
-	msync
-	isync
-	mtspr	L1CSR0,r0
-	isync
-
-	/* disable d-cache */
-	li	r0,0x0
-	mtspr	L1CSR0,r0
-
-	/* invalidate i-cache */
-	mfspr	r0,L1CSR1
-	ori	r0,r0,0x0002
-	mtspr	L1CSR1,r0
-	isync
-
-	/* disable i-cache */
-	li	r0,0x0
-	mtspr	L1CSR1,r0
-	isync
-
-	/* clear registers */
-	li	r0,0
-	mtspr	SRR0,r0
-	mtspr	SRR1,r0
-	mtspr	CSRR0,r0
-	mtspr	CSRR1,r0
-	mtspr	MCSRR0,r0
-	mtspr	MCSRR1,r0
-
-	mtspr	ESR,r0
-	mtspr	MCSR,r0
-	mtspr	DEAR,r0
-
-	/* not needed and conflicts with some debuggers */
-	/* mtspr	DBCR0,r0 */
-	mtspr	DBCR1,r0
-	mtspr	DBCR2,r0
-	/* not needed and conflicts with some debuggers */
-	/* mtspr	IAC1,r0 */
-	/* mtspr	IAC2,r0 */
-	mtspr	DAC1,r0
-	mtspr	DAC2,r0
+	/* L1 */
+	li	r0,2
+	mtspr	L1CSR0,r0	/* invalidate d-cache */
+	mtspr	L1CSR1,r0 	/* invalidate i-cache */
 
 	mfspr	r1,DBSR
 	mtspr	DBSR,r1		/* Clear all valid bits */
 
-	mtspr	PID0,r0
-	mtspr	PID1,r0
-	mtspr	PID2,r0
-	mtspr	TCR,r0
+	/*
+	 *	Enable L1 Caches early
+	 *
+	 */
 
-	mtspr	BUCSR,r0	/* disable branch prediction */
-	mtspr	MAS4,r0
-	mtspr	MAS6,r0
-#if defined(CONFIG_ENABLE_36BIT_PHYS)
-	mtspr	MAS7,r0
-#endif
+	lis	r2,L1CSR0_CPE@H	/* enable parity */
+	ori	r2,r2,L1CSR0_DCE
+	mtspr	L1CSR0,r2	/* enable L1 Dcache */
 	isync
+	mtspr	L1CSR1,r2	/* enable L1 Icache */
+	isync
+	msync
 
 	/* Setup interrupt vectors */
 	lis	r1,TEXT_BASE@h
-	mtspr IVPR, r1
+	mtspr	IVPR,r1
 
 	li	r1,0x0100
 	mtspr	IVOR0,r1	/* 0: Critical input */
@@ -217,26 +143,6 @@ _start_e500:
 	li	r1,0x0f00
 	mtspr	IVOR15,r1	/* 15: Debug */
 
-	/*
-	 * Invalidate MMU L1/L2
-	 *
-	 * Note: There is a fixup earlier for Errata CPU4 on
-	 * Rev 1 parts that must precede this MMU invalidation.
-	 */
-	li	r2, 0x001e
-	mtspr	MMUCSR0, r2
-	isync
-
-	/*
-	 * Invalidate all TLB0 entries.
-	 */
-	li	r3,4
-	li	r4,0
-	tlbivax r4,r3
-	/*
-	 * To avoid REV1 Errata CPU6 issues, make sure
-	 * the instruction following tlbivax is not a store.
-	 */
 
 	/*
 	 * After reset, CCSRBAR is located at CFG_CCSRBAR_DEFAULT, i.e.
@@ -254,14 +160,14 @@ _start_e500:
 	lwzu	r4,0(r5)	/* how many TLB1 entries we actually use */
 	mtctr	r4
 
-0:	lwzu	r0,4(r5)
-	lwzu	r1,4(r5)
-	lwzu	r2,4(r5)
-	lwzu	r3,4(r5)
-	mtspr	MAS0,r0
-	mtspr	MAS1,r1
-	mtspr	MAS2,r2
-	mtspr	MAS3,r3
+0:	lwzu	r6,4(r5)
+	lwzu	r7,4(r5)
+	lwzu	r8,4(r5)
+	lwzu	r9,4(r5)
+	mtspr	MAS0,r6
+	mtspr	MAS1,r7
+	mtspr	MAS2,r8
+	mtspr	MAS3,r9
 	isync
 	msync
 	tlbwe
@@ -271,22 +177,22 @@ _start_e500:
 1:
 #if (CFG_CCSRBAR_DEFAULT != CFG_CCSRBAR)
 	/* Special sequence needed to update CCSRBAR itself */
-	lis	r4, CFG_CCSRBAR_DEFAULT@h
-	ori	r4, r4, CFG_CCSRBAR_DEFAULT@l
+	lis	r4,CFG_CCSRBAR_DEFAULT@h
+	ori	r4,r4,CFG_CCSRBAR_DEFAULT@l
 
-	lis	r5, CFG_CCSRBAR@h
-	ori	r5, r5, CFG_CCSRBAR@l
+	lis	r5,CFG_CCSRBAR@h
+	ori	r5,r5,CFG_CCSRBAR@l
 	srwi	r6,r5,12
-	stw	r6, 0(r4)
+	stw	r6,0(r4)
 	isync
 
-	lis	r5, 0xffff
+	lis	r5,0xffff
 	ori	r5,r5,0xf000
-	lwz	r5, 0(r5)
+	lwz	r5,0(r5)
 	isync
 
-	lis	r3, CFG_CCSRBAR@h
-	lwz	r5, CFG_CCSRBAR@l(r3)
+	lis	r3,CFG_CCSRBAR@h
+	lwz	r5,CFG_CCSRBAR@l(r3)
 	isync
 #endif
 
@@ -300,8 +206,8 @@ _start_e500:
 	lwzu	r5,0(r6)	/* how many windows we actually use */
 	mtctr	r5
 
-	li	r2,0x0c28	/* the first pair is reserved for boot-over-rio-or-pci */
-	li	r1,0x0c30
+	li	r2,0x0c28	/* the first pair is reserved for */
+	li	r1,0x0c30	/* boot-over-rio-or-pci */
 
 0:	lwzu	r4,4(r6)
 	lwzu	r3,4(r6)
@@ -311,31 +217,6 @@ _start_e500:
 	addi	r1,r1,0x0020
 	bdnz	0b
 
-	/* Jump out the last 4K page and continue to 'normal' start */
-1:	bl	3f
-	b	_start
-
-3:	li	r0,0
-	mtspr	SRR1,r0		/* Keep things disabled for now */
-	mflr	r1
-	mtspr	SRR0,r1
-	rfi
-
-/*
- * r3 - 1st arg to board_init(): IMMP pointer
- * r4 - 2nd arg to board_init(): boot flag
- */
-	.text
-	.long	0x27051956		/* U-BOOT Magic Number			*/
-	.globl	version_string
-version_string:
-	.ascii U_BOOT_VERSION
-	.ascii " (", __DATE__, " - ", __TIME__, ")"
-	.ascii CONFIG_IDENT_STRING, "\0"
-
-	. = EXC_OFF_SYS_RESET
-	.globl	_start
-_start:
 	/* Clear and set up some registers. */
 	li	r0,0x0000
 	lis	r1,0xffff
@@ -354,17 +235,14 @@ _start:
 
 	/* Enable Time Base and Select Time Base Clock */
 	lis	r0,HID0_EMCP@h		/* Enable machine check */
-	ori	r0,r0,0x4000		/* time base is processor clock */
 #if defined(CONFIG_ENABLE_36BIT_PHYS)
-	ori	r0,r0,0x0080		/* enable MAS7 updates */
+	ori	r0,r0,(HID0_TBEN|HID0_ENMAS7)@l	/* Enable Timebase & MAS7 */
+#else
+	ori	r0,r0,HID0_TBEN@l	/* enable Timebase */
 #endif
 	mtspr	HID0,r0
 
-#if defined(CONFIG_ADDR_STREAMING)
-	li	r0,0x3000
-#else
-	li	r0,0x1000
-#endif
+	li	r0,(HID1_ASTME|HID1_ABE)@l	/* Addr streaming & broadcast */
 	mtspr	HID1,r0
 
 	/* Enable Branch Prediction */
@@ -383,37 +261,53 @@ _start:
 #endif
 
 /* L1 DCache is used for initial RAM */
-	mfspr	r2, L1CSR0
-	ori	r2, r2, 0x0003
-	oris	r2, r2, 0x0001
-	mtspr	L1CSR0, r2	/* enable/invalidate L1 Dcache */
-	isync
 
 	/* Allocate Initial RAM in data cache.
 	 */
-	lis	r3, CFG_INIT_RAM_ADDR@h
-	ori	r3, r3, CFG_INIT_RAM_ADDR@l
-	li	r2, 512 /* 512*32=16K */
+	lis	r3,CFG_INIT_RAM_ADDR@h
+	ori	r3,r3,CFG_INIT_RAM_ADDR@l
+	li	r2,512 /* 512*32=16K */
 	mtctr	r2
-	li	r0, 0
+	li	r0,0
 1:
-	dcbz	r0, r3
-	dcbtls	0,r0, r3
-	addi	r3, r3, 32
+	dcbz	r0,r3
+	dcbtls	0,r0,r3
+	addi	r3,r3,32
 	bdnz	1b
 
-#ifndef CFG_RAMBOOT
+	/* Jump out the last 4K page and continue to 'normal' start */
+#ifdef CFG_RAMBOOT
+	bl	3f
+	b	_start_cont
+#else
 	/* Calculate absolute address in FLASH and jump there		*/
 	/*--------------------------------------------------------------*/
-	lis	r3, CFG_MONITOR_BASE@h
-	ori	r3, r3, CFG_MONITOR_BASE@l
-	addi	r3, r3, in_flash - _start + EXC_OFF_SYS_RESET
+	lis	r3,CFG_MONITOR_BASE@h
+	ori	r3,r3,CFG_MONITOR_BASE@l
+	addi	r3,r3,_start_cont - _start + _START_OFFSET
 	mtlr	r3
-	blr
+#endif
 
-in_flash:
-#endif	/* CFG_RAMBOOT */
+3:	li	r0,0
+	mtspr	SRR1,r0		/* Keep things disabled for now */
+	mflr	r1
+	mtspr	SRR0,r1
+	rfi
+	isync
 
+	.text
+	.globl	_start
+_start:
+	.long	0x27051956		/* U-BOOT Magic Number */
+	.globl	version_string
+version_string:
+	.ascii U_BOOT_VERSION
+	.ascii " (", __DATE__, " - ", __TIME__, ")"
+	.ascii CONFIG_IDENT_STRING, "\0"
+
+	.align	4
+	.globl	_start_cont
+_start_cont:
 	/* Setup the stack in initial RAM,could be L2-as-SRAM or L1 dcache*/
 	lis	r1,CFG_INIT_RAM_ADDR@h
 	ori	r1,r1,CFG_INIT_SP_OFFSET@l
@@ -424,26 +318,24 @@ in_flash:
 
 	stwu	r1,-8(r1)		/* Save back chain and move SP */
 	lis	r0,RESET_VECTOR@h	/* Address of reset vector */
-	ori	r0,r0, RESET_VECTOR@l
+	ori	r0,r0,RESET_VECTOR@l
 	stwu	r1,-8(r1)		/* Save back chain and move SP */
 	stw	r0,+12(r1)		/* Save return addr (underflow vect) */
 
 	GET_GOT
 	bl	cpu_init_f
-	bl	icache_enable
 	bl	board_init_f
 	isync
 
-/* --FIXME-- machine check with MCSRRn and rfmci */
-
+	. = EXC_OFF_SYS_RESET
 	.globl	_start_of_vectors
 _start_of_vectors:
-#if 0
+
 /* Critical input. */
-	CRIT_EXCEPTION(0x0100, CritcalInput, CritcalInputException)
-#endif
-/* Machine check --FIXME-- Should be MACH_EXCEPTION */
-	CRIT_EXCEPTION(0x0200, MachineCheck, MachineCheckException)
+	CRIT_EXCEPTION(0x0100, CriticalInput, CritcalInputException)
+
+/* Machine check */
+	MCK_EXCEPTION(0x200, MachineCheck, MachineCheckException)
 
 /* Data Storage exception. */
 	STD_EXCEPTION(0x0300, DataStorage, UnknownException)
@@ -452,7 +344,7 @@ _start_of_vectors:
 	STD_EXCEPTION(0x0400, InstStorage, UnknownException)
 
 /* External Interrupt exception. */
-	STD_EXCEPTION(0x0500, ExtInterrupt, UnknownException)
+	STD_EXCEPTION(0x0500, ExtInterrupt, ExtIntException)
 
 /* Alignment exception. */
 	. = 0x0600
@@ -469,8 +361,8 @@ Alignment:
 	mtlr	r6
 	blrl
 .L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	.long	AlignmentException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
 
 /* Program check exception */
 	. = 0x0700
@@ -483,8 +375,8 @@ ProgramCheck:
 	mtlr	r6
 	blrl
 .L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
+	.long	ProgramCheckException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
 
 	/* No FPU on MPC85xx.  This exception is not supposed to happen.
 	*/
@@ -496,23 +388,23 @@ ProgramCheck:
  * r3-... arguments
  */
 SystemCall:
-	addis	r11,r0,0		/* get functions table addr */
-	ori	r11,r11,0		/* Note: this code is patched in trap_init */
-	addis	r12,r0,0		/* get number of functions */
+	addis	r11,r0,0	/* get functions table addr */
+	ori	r11,r11,0	/* Note: this code is patched in trap_init */
+	addis	r12,r0,0	/* get number of functions */
 	ori	r12,r12,0
 
-	cmplw	0, r0, r12
+	cmplw	0,r0,r12
 	bge	1f
 
-	rlwinm	r0,r0,2,0,31		/* fn_addr = fn_tbl[r0] */
+	rlwinm	r0,r0,2,0,31	/* fn_addr = fn_tbl[r0] */
 	add	r11,r11,r0
 	lwz	r11,0(r11)
 
-	li	r20,0xd00-4		/* Get stack pointer */
+	li	r20,0xd00-4	/* Get stack pointer */
 	lwz	r12,0(r20)
-	subi	r12,r12,12		/* Adjust stack pointer */
+	subi	r12,r12,12	/* Adjust stack pointer */
 	li	r0,0xc00+_end_back-SystemCall
-	cmplw	0, r0, r12		/* Check stack overflow */
+	cmplw	0,r0,r12	/* Check stack overflow */
 	bgt	1f
 	stw	r12,0(r20)
 
@@ -570,7 +462,7 @@ _end_back:
 _end_of_vectors:
 
 
-	. = 0x2100
+	. = . + (0x100 - ( . & 0xff ))	/* align for debug */
 
 /*
  * This code finishes saving the registers to the exception frame
@@ -655,26 +547,58 @@ crit_return:
 	REST_GPR(31, r1)
 	lwz	r2,_NIP(r1)	/* Restore environment */
 	lwz	r0,_MSR(r1)
-	mtspr	990,r2		/* SRR2 */
-	mtspr	991,r0		/* SRR3 */
+	mtspr	SPRN_CSRR0,r2
+	mtspr	SPRN_CSRR1,r0
 	lwz	r0,GPR0(r1)
 	lwz	r2,GPR2(r1)
 	lwz	r1,GPR1(r1)
 	SYNC
 	rfci
 
+mck_return:
+	mfmsr	r28		/* Disable interrupts */
+	li	r4,0
+	ori	r4,r4,MSR_EE
+	andc	r28,r28,r4
+	SYNC			/* Some chip revs need this... */
+	mtmsr	r28
+	SYNC
+	lwz	r2,_CTR(r1)
+	lwz	r0,_LINK(r1)
+	mtctr	r2
+	mtlr	r0
+	lwz	r2,_XER(r1)
+	lwz	r0,_CCR(r1)
+	mtspr	XER,r2
+	mtcrf	0xFF,r0
+	REST_10GPRS(3, r1)
+	REST_10GPRS(13, r1)
+	REST_8GPRS(23, r1)
+	REST_GPR(31, r1)
+	lwz	r2,_NIP(r1)	/* Restore environment */
+	lwz	r0,_MSR(r1)
+	mtspr	SPRN_MCSRR0,r2
+	mtspr	SPRN_MCSRR1,r0
+	lwz	r0,GPR0(r1)
+	lwz	r2,GPR2(r1)
+	lwz	r1,GPR1(r1)
+	SYNC
+	rfmci
+
 /* Cache functions.
 */
 invalidate_icache:
 	mfspr	r0,L1CSR1
-	ori	r0,r0,0x0002
+	ori	r0,r0,L1CSR1_ICFI
+	msync
+	isync
 	mtspr	L1CSR1,r0
 	isync
-	blr				/*   entire I cache */
+	blr				/* entire I cache */
 
 invalidate_dcache:
 	mfspr	r0,L1CSR0
-	ori	r0,r0,0x0002
+	ori	r0,r0,L1CSR0_DCFI
 	msync
 	isync
 	mtspr	L1CSR0,r0
@@ -697,9 +621,9 @@ icache_enable:
 	.globl	icache_disable
 icache_disable:
 	mfspr	r0,L1CSR1
-	lis	r1,0xfffffffe@h
-	ori	r1,r1,0xfffffffe@l
-	and	r0,r0,r1
+	lis	r3,0
+	ori	r3,r3,L1CSR1_ICE
+	andc	r0,r0,r3
 	mtspr	L1CSR1,r0
 	isync
 	blr
@@ -707,7 +631,7 @@ icache_disable:
 	.globl	icache_status
 icache_status:
 	mfspr	r3,L1CSR1
-	andi.	r3,r3,1
+	andi.	r3,r3,L1CSR1_ICE
 	blr
 
 	.globl	dcache_enable
@@ -727,12 +651,10 @@ dcache_enable:
 
 	.globl	dcache_disable
 dcache_disable:
-	mfspr	r0,L1CSR0
-	lis	r1,0xfffffffe@h
-	ori	r1,r1,0xfffffffe@l
-	and	r0,r0,r1
-	msync
-	isync
+	mfspr	r3,L1CSR0
+	lis	r4,0
+	ori	r4,r4,L1CSR0_DCE
+	andc	r3,r3,r4
 	mtspr	L1CSR0,r0
 	isync
 	blr
@@ -740,27 +662,27 @@ dcache_disable:
 	.globl	dcache_status
 dcache_status:
 	mfspr	r3,L1CSR0
-	andi.	r3,r3,1
+	andi.	r3,r3,L1CSR0_DCE
 	blr
 
 	.globl get_pir
 get_pir:
-	mfspr	r3, PIR
+	mfspr	r3,PIR
 	blr
 
 	.globl get_pvr
 get_pvr:
-	mfspr	r3, PVR
+	mfspr	r3,PVR
 	blr
 
 	.globl get_svr
 get_svr:
-	mfspr	r3, SVR
+	mfspr	r3,SVR
 	blr
 
 	.globl wr_tcr
 wr_tcr:
-	mtspr	TCR, r3
+	mtspr	TCR,r3
 	blr
 
 /*------------------------------------------------------------------------------- */
@@ -913,16 +835,16 @@ ppcSync:
  */
 	.globl	relocate_code
 relocate_code:
-	mr	r1,  r3		/* Set new stack pointer		*/
-	mr	r9,  r4		/* Save copy of Init Data pointer	*/
-	mr	r10, r5		/* Save copy of Destination Address	*/
+	mr	r1,r3		/* Set new stack pointer		*/
+	mr	r9,r4		/* Save copy of Init Data pointer	*/
+	mr	r10,r5		/* Save copy of Destination Address	*/
 
-	mr	r3,  r5				/* Destination Address	*/
-	lis	r4, CFG_MONITOR_BASE@h		/* Source      Address	*/
-	ori	r4, r4, CFG_MONITOR_BASE@l
+	mr	r3,r5				/* Destination Address	*/
+	lis	r4,CFG_MONITOR_BASE@h		/* Source      Address	*/
+	ori	r4,r4,CFG_MONITOR_BASE@l
 	lwz	r5,GOT(__init_end)
 	sub	r5,r5,r4
-	li	r6, CFG_CACHELINE_SIZE		/* Cache Line Size	*/
+	li	r6,CFG_CACHELINE_SIZE		/* Cache Line Size	*/
 
 	/*
 	 * Fix GOT pointer:
@@ -931,12 +853,12 @@ relocate_code:
 	 *
 	 * Offset:
 	 */
-	sub	r15, r10, r4
+	sub	r15,r10,r4
 
 	/* First our own GOT */
-	add	r14, r14, r15
+	add	r14,r14,r15
 	/* the the one used by the C code */
-	add	r30, r30, r15
+	add	r30,r30,r15
 
 	/*
 	 * Now relocate code
@@ -997,10 +919,10 @@ relocate_code:
  * initialization, now running from RAM.
  */
 
-	addi	r0, r10, in_ram - _start + EXC_OFF_SYS_RESET
+	addi	r0,r10,in_ram - _start + _START_OFFSET
 	mtlr	r0
 	blr				/* NEVER RETURNS! */
-
+	.globl	in_ram
 in_ram:
 
 	/*
@@ -1044,19 +966,19 @@ clear_bss:
 	lwz	r3,GOT(__bss_start)
 	lwz	r4,GOT(_end)
 
-	cmplw	0, r3, r4
+	cmplw	0,r3,r4
 	beq	6f
 
-	li	r0, 0
+	li	r0,0
 5:
-	stw	r0, 0(r3)
-	addi	r3, r3, 4
-	cmplw	0, r3, r4
+	stw	r0,0(r3)
+	addi	r3,r3,4
+	cmplw	0,r3,r4
 	bne	5b
 6:
 
-	mr	r3, r9		/* Init Data pointer		*/
-	mr	r4, r10		/* Destination Address		*/
+	mr	r3,r9		/* Init Data pointer		*/
+	mr	r4,r10		/* Destination Address		*/
 	bl	board_init_r
 
 	/*
@@ -1067,52 +989,54 @@ clear_bss:
 	 */
 	.globl	trap_init
 trap_init:
-	lwz	r7, GOT(_start)
-	lwz	r8, GOT(_end_of_vectors)
+	lwz	r7,GOT(_start_of_vectors)
+	lwz	r8,GOT(_end_of_vectors)
 
-	li	r9, 0x100		/* reset vector always at 0x100 */
+	li	r9,0x100		/* reset vector always at 0x100 */
 
-	cmplw	0, r7, r8
+	cmplw	0,r7,r8
 	bgelr				/* return if r7>=r8 - just in case */
 
 	mflr	r4			/* save link register		*/
 1:
-	lwz	r0, 0(r7)
-	stw	r0, 0(r9)
-	addi	r7, r7, 4
-	addi	r9, r9, 4
-	cmplw	0, r7, r8
+	lwz	r0,0(r7)
+	stw	r0,0(r9)
+	addi	r7,r7,4
+	addi	r9,r9,4
+	cmplw	0,r7,r8
 	bne	1b
 
 	/*
 	 * relocate `hdlr' and `int_return' entries
 	 */
-	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_CriticalInput - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_DataStorage - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_MachineCheck - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_InstStorage - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_DataStorage - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_ExtInterrupt - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_InstStorage - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_ExtInterrupt - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_Alignment - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_ProgramCheck - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_Decrementer - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_FPUnavailable - _start + _START_OFFSET
 	bl	trap_reloc
-	li	r7, .L_IntervalTimer - _start + EXC_OFF_SYS_RESET
-	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
+	li	r7,.L_Decrementer - _start + _START_OFFSET
+	bl	trap_reloc
+	li	r7,.L_IntervalTimer - _start + _START_OFFSET
+	li	r8,_end_of_vectors - _start + _START_OFFSET
 2:
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
+	addi	r7,r7,0x100		/* next exception vector	*/
+	cmplw	0,r7,r8
 	blt	2b
 
 	lis	r7,0x0
-	mtspr	IVPR, r7
+	mtspr	IVPR,r7
 
 	mtlr	r4			/* restore link register	*/
 	blr
@@ -1121,13 +1045,13 @@ trap_init:
 	 * Function: relocate entries for one exception vector
 	 */
 trap_reloc:
-	lwz	r0, 0(r7)		/* hdlr ...			*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 0(r7)
+	lwz	r0,0(r7)		/* hdlr ...			*/
+	add	r0,r0,r3		/*  ... += dest_addr		*/
+	stw	r0,0(r7)
 
-	lwz	r0, 4(r7)		/* int_return ...		*/
-	add	r0, r0, r3		/*  ... += dest_addr		*/
-	stw	r0, 4(r7)
+	lwz	r0,4(r7)		/* int_return ...		*/
+	add	r0,r0,r3		/*  ... += dest_addr		*/
+	stw	r0,4(r7)
 
 	blr
 
@@ -1135,13 +1059,13 @@ trap_reloc:
 .globl unlock_ram_in_cache
 unlock_ram_in_cache:
 	/* invalidate the INIT_RAM section */
-	lis	r3, (CFG_INIT_RAM_ADDR & ~31)@h
-	ori	r3, r3, (CFG_INIT_RAM_ADDR & ~31)@l
-	li	r2,512
-	mtctr	r2
-1:	icbi	r0, r3
-	dcbi	r0, r3
-	addi	r3, r3, 32
+	lis	r3,(CFG_INIT_RAM_ADDR & ~31)@h
+	ori	r3,r3,(CFG_INIT_RAM_ADDR & ~31)@l
+	li	r4,512
+	mtctr	r4
+1:	icbi	r0,r3
+	dcbi	r0,r3
+	addi	r3,r3,32
 	bdnz	1b
 	sync			/* Wait for all icbi to complete on bus */
 	isync
diff --git a/cpu/mpc85xx/traps.c b/cpu/mpc85xx/traps.c
index 9cd621c..efc80c7 100644
--- a/cpu/mpc85xx/traps.c
+++ b/cpu/mpc85xx/traps.c
@@ -1,6 +1,7 @@
 /*
  * linux/arch/ppc/kernel/traps.c
  *
+ * Copyright 2007 Freescale Semiconductor.
  * Copyright (C) 2003 Motorola
  * Modified by Xianghua Xiao(x.xiao@motorola.com)
  *
@@ -145,10 +146,13 @@ CritcalInputException(struct pt_regs *regs)
 	panic("Critical Input Exception");
 }
 
+int machinecheck_count = 0;
+int machinecheck_error = 0;
 void
 MachineCheckException(struct pt_regs *regs)
 {
 	unsigned long fixup;
+	unsigned int mcsr, mcsrr0, mcsrr1, mcar;
 
 	/* Probing PCI using config cycles cause this exception
 	 * when a device is not present.  Catch it and return to
@@ -159,34 +163,62 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 	}
 
+	mcsrr0 = mfspr(SPRN_MCSRR0);
+	mcsrr1 = mfspr(SPRN_MCSRR1);
+	mcsr = mfspr(SPRN_MCSR);
+	mcar = mfspr(SPRN_MCAR);
+
+	machinecheck_count++;
+	machinecheck_error=1;
+
 #if defined(CONFIG_CMD_KGDB)
 	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
 		return;
 #endif
 
 	printf("Machine check in kernel mode.\n");
-	printf("Caused by (from msr): ");
-	printf("regs %p ",regs);
-	switch( regs->msr & 0x000F0000) {
-	case (0x80000000>>12):
-		printf("Machine check signal - probably due to mm fault\n"
-		       "with mmu off\n");
-		break;
-	case (0x80000000>>13):
-		printf("Transfer error ack signal\n");
-		break;
-	case (0x80000000>>14):
-		printf("Data parity signal\n");
-		break;
-	case (0x80000000>>15):
-		printf("Address parity signal\n");
-		break;
-	default:
-		printf("Unknown values in msr\n");
-	}
+	printf("Caused by (from mcsr): ");
+	printf("mcsr = 0x%08x\n", mcsr);
+	if (mcsr & 0x80000000)
+		printf("Machine check input pin\n");
+	if (mcsr & 0x40000000)
+		printf("Instruction cache parity error\n");
+	if (mcsr & 0x20000000)
+		printf("Data cache push parity error\n");
+	if (mcsr & 0x10000000)
+		printf("Data cache parity error\n");
+	if (mcsr & 0x00000080)
+		printf("Bus instruction address error\n");
+	if (mcsr & 0x00000040)
+		printf("Bus Read address error\n");
+	if (mcsr & 0x00000020)
+		printf("Bus Write address error\n");
+	if (mcsr & 0x00000010)
+		printf("Bus Instruction data bus error\n");
+	if (mcsr & 0x00000008)
+		printf("Bus Read data bus error\n");
+	if (mcsr & 0x00000004)
+		printf("Bus Write bus error\n");
+	if (mcsr & 0x00000002)
+		printf("Bus Instruction parity error\n");
+	if (mcsr & 0x00000001)
+		printf("Bus Read parity error\n");
+
 	show_regs(regs);
+	printf("MCSR=0x%08x \tMCSRR0=0x%08x \nMCSRR1=0x%08x \tMCAR=0x%08x\n",
+	       mcsr, mcsrr0, mcsrr1, mcar);
 	print_backtrace((unsigned long *)regs->gpr[1]);
-	panic("machine check");
+	if (machinecheck_count > 10) {
+		panic("machine check count too high\n");
+	}
+
+	if (machinecheck_count > 1) {
+		regs->nip += 4; /* skip offending instruction */
+		printf("Skipping current instr, Returning to 0x%08x\n",
+		       regs->nip);
+	} else {
+		printf("Returning back to 0x%08x\n",regs->nip);
+	}
 }
 
 void
@@ -253,6 +285,33 @@ UnknownException(struct pt_regs *regs)
 	       regs->nip, regs->msr, regs->trap);
 	_exception(0, regs);
 }
+void
+ExtIntException(struct pt_regs *regs)
+{
+	volatile immap_t *immap = (immap_t *)CFG_IMMR;
+	volatile ccsr_pic_t *pic = &immap->im_pic;
+	uint vect;
+
+#if defined(CONFIG_CMD_KGDB)
+	if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+		return;
+#endif
+
+	printf("External Interrupt Exception at PC: %lx, SR: %lx, vector=%lx",
+	       regs->nip, regs->msr, regs->trap);
+	vect = pic->iack0;
+	printf(" irq IACK0@%05x=%d\n",&pic->iack0,vect);
+	show_regs(regs);
+	print_backtrace((unsigned long *)regs->gpr[1]);
+	machinecheck_count++;
+#ifdef EXTINT_NOSKIP
+	printf("Returning back to 0x%08x\n",regs->nip);
+#else
+	regs->nip += 4; /* skip offending instruction */
+	printf("Skipping current instr, Returning to 0x%08x\n",regs->nip);
+#endif
+
+}
 
 void
 DebugException(struct pt_regs *regs)