85xx start.S cleanup and exception support

From: Ed Swarthout <Ed.Swarthout@freescale.com>

Support external interrupts from platform to eliminate system hangs.
Define CONFIG_INTERRUPTS board configure option to enable.
Enable ecm, ddr, lbc, and pci/pcie error interrupts in PIC.

Remove extra cpu initialization redundant with hardware initialization.
Whitespace cleanup.

Define and use _START_OFFSET consistent with other processors using
ppc_asm.tmpl

Move additional code from .text to boot page to make room for
exception vectors at start of image.

Handle Machine Check, External and Critical exceptions.

Fix e500 machine check error determination in traps.c

TEXT_BASE can now be 0xfffc_0000 - which cuts binary image in half.

Signed-off-by: Ed Swarthout <Ed.Swarthout@freescale.com>
Acked-by: Andy Fleming <afleming@freescale.com>

3 files changed, 319 insertions, 298 deletions

diff --git a/cpu/mpc85xx/interrupts.c b/cpu/mpc85xx/interrupts.c
index dc246dca02..bf737d6228 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/start.S b/cpu/mpc85xx/start.S
index 77c155c5bd..9dfd38d6a0 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 */
@@ -382,35 +260,56 @@ _start:
 	mtspr	DBCR0,r0
 #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 */
+	/* Jump out the last 4K page and continue to 'normal' start */
+	bl	3f
+	b	_start_cont
+
+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:
+
+/* L1 DCache is used for initial RAM */
+
 	/* 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
 	/* 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,in_flash - _start + _START_OFFSET
 	mtlr	r3
 	blr
-
+	.global	in_flash
 in_flash:
 #endif	/* CFG_RAMBOOT */
 
@@ -424,26 +323,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 +349,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 +366,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 +380,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 +393,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 +467,7 @@ _end_back:
 _end_of_vectors:
 
 
-	. = 0x2100
+	. = . + (0x100 - ( . & 0xff ))	/* align for debug */
 
 /*
  * This code finishes saving the registers to the exception frame
@@ -655,26 +552,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 +626,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 +636,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 +656,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 +667,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 +840,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 +858,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 +924,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 +971,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 +994,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 +1050,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 +1064,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 9cd621c3ac..efc80c7aee 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)