ppc4xx: Clean up 440 exceptions handling

- Introduced dedicated switches for building 440 and 405 images required
  for 440-specific machine instructions like 'rfmci' etc.

- Exception vectors moved to the proper location (_start moved away from
  the critical exception handler space, which it occupied)

- CriticalInput now serviced (with default handler)

- MachineCheck properly serviced (added a dedicated handler and return
  subroutine)

- Overall cleanup of exceptions declared with STD_EXCEPTION macro (unused,
  unhandled and those not relevant for 4xx were eliminated)

- Eliminated Linux leftovers, removed dead code

Signed-off-by: Grzegorz Bernacki <gjb@semihalf.com>
Signed-off-by: Rafal Jaworowski <raj@semihalf.com>
Signed-off-by: Stefan Roese <sr@denx.de>

21 files changed, 319 insertions, 267 deletions

diff --git a/cpu/ppc4xx/config.mk b/cpu/ppc4xx/config.mk
index 119e061..e7fc3f63 100644
--- a/cpu/ppc4xx/config.mk
+++ b/cpu/ppc4xx/config.mk
@@ -22,5 +22,13 @@
 #
 
 PLATFORM_RELFLAGS += -fPIC -ffixed-r14 -meabi -fno-strict-aliasing
+PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -msoft-float
 
-PLATFORM_CPPFLAGS += -DCONFIG_4xx -ffixed-r2 -ffixed-r29 -mstring -Wa,-m405 -mcpu=405 -msoft-float
+cfg=$(shell grep configs $(TOPDIR)/include/config.h | sed 's/.*<\(configs.*\)>/\1/')
+is440=$(shell grep CONFIG_440 $(TOPDIR)/include/$(cfg))
+
+ifneq (,$(findstring CONFIG_440,$(is440)))
+PLATFORM_CPPFLAGS += -Wa,-m440 -mcpu=440
+else
+PLATFORM_CPPFLAGS += -Wa,-m405 -mcpu=405
+endif
diff --git a/cpu/ppc4xx/start.S b/cpu/ppc4xx/start.S
index 78d0042..e135220 100644
--- a/cpu/ppc4xx/start.S
+++ b/cpu/ppc4xx/start.S
@@ -294,11 +294,13 @@ skip_debug_init:
 	mtspr	ivor7,r1	/* Floating point unavailable */
 	li	r1,0x0c00
 	mtspr	ivor8,r1	/* System call */
-	li	r1,0x1000
-	mtspr	ivor10,r1	/* Decrementer (PIT for 440) */
-	li	r1,0x1400
-	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x0a00
+	mtspr   ivor9,r1	/* Auxiliary Processor unavailable */
+	li	r1,0x0900
+	mtspr	ivor10,r1	/* Decrementer */
 	li	r1,0x1300
+	mtspr	ivor13,r1	/* Data TLB error */
+	li	r1,0x1400
 	mtspr	ivor14,r1	/* Instr TLB error */
 	li	r1,0x2000
 	mtspr	ivor15,r1	/* Debug */
@@ -503,11 +505,81 @@ version_string:
 	.ascii " (", __DATE__, " - ", __TIME__, ")"
 	.ascii CONFIG_IDENT_STRING, "\0"
 
-/*
- * Maybe this should be moved somewhere else because the current
- * location (0x100) is where the CriticalInput Execption should be.
- */
 	. = EXC_OFF_SYS_RESET
+	.globl	_start_of_vectors
+_start_of_vectors:
+
+/* Critical input. */
+	CRIT_EXCEPTION(0x100, CritcalInput, UnknownException)
+
+#ifdef CONFIG_440
+/* Machine check */
+        MCK_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#else
+        CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
+#endif /* CONFIG_440 */
+
+/* Data Storage exception. */
+	STD_EXCEPTION(0x300, DataStorage, UnknownException)
+
+/* Instruction Storage exception. */
+	STD_EXCEPTION(0x400, InstStorage, UnknownException)
+
+/* External Interrupt exception. */
+	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
+
+/* Alignment exception. */
+	. = 0x600
+Alignment:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	mfspr	r4,DAR
+	stw	r4,_DAR(r21)
+	mfspr	r5,DSISR
+	stw	r5,_DSISR(r21)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_Alignment:
+	.long	AlignmentException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+/* Program check exception */
+	. = 0x700
+ProgramCheck:
+	EXCEPTION_PROLOG(SRR0, SRR1)
+	addi	r3,r1,STACK_FRAME_OVERHEAD
+	li	r20,MSR_KERNEL
+	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
+	lwz	r6,GOT(transfer_to_handler)
+	mtlr	r6
+	blrl
+.L_ProgramCheck:
+	.long	ProgramCheckException - _start + _START_OFFSET
+	.long	int_return - _start + _START_OFFSET
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
+	STD_EXCEPTION(0x900, Decrementer, DecrementerPITException)
+	STD_EXCEPTION(0xa00, APU, UnknownException)
+#endif	
+	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
+
+#ifdef CONFIG_440
+	STD_EXCEPTION(0x1300, DataTLBError, UnknownException)
+	STD_EXCEPTION(0x1400, InstructionTLBError, UnknownException)
+#else
+	STD_EXCEPTION(0x1000, PIT, DecrementerPITException)
+	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
+	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
+#endif
+	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
+
+	.globl	_end_of_vectors
+_end_of_vectors:
+	. = _START_OFFSET
 #endif
 	.globl	_start
 _start:
@@ -1017,107 +1089,6 @@ start_ram:
 
 
 #ifndef CONFIG_NAND_SPL
-/*****************************************************************************/
-	.globl	_start_of_vectors
-_start_of_vectors:
-
-#if 0
-/*TODO Fixup _start above so we can do this*/
-/* Critical input. */
-	CRIT_EXCEPTION(0x100, CritcalInput, CritcalInputException)
-#endif
-
-/* Machine check */
-	CRIT_EXCEPTION(0x200, MachineCheck, MachineCheckException)
-
-/* Data Storage exception. */
-	STD_EXCEPTION(0x300, DataStorage, UnknownException)
-
-/* Instruction Storage exception. */
-	STD_EXCEPTION(0x400, InstStorage, UnknownException)
-
-/* External Interrupt exception. */
-	STD_EXCEPTION(0x500, ExtInterrupt, external_interrupt)
-
-/* Alignment exception. */
-	. = 0x600
-Alignment:
-	EXCEPTION_PROLOG
-	mfspr	r4,DAR
-	stw	r4,_DAR(r21)
-	mfspr	r5,DSISR
-	stw	r5,_DSISR(r21)
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_Alignment:
-	.long	AlignmentException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-/* Program check exception */
-	. = 0x700
-ProgramCheck:
-	EXCEPTION_PROLOG
-	addi	r3,r1,STACK_FRAME_OVERHEAD
-	li	r20,MSR_KERNEL
-	rlwimi	r20,r23,0,16,16		/* copy EE bit from saved MSR */
-	lwz	r6,GOT(transfer_to_handler)
-	mtlr	r6
-	blrl
-.L_ProgramCheck:
-	.long	ProgramCheckException - _start + EXC_OFF_SYS_RESET
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-	/* No FPU on MPC8xx.  This exception is not supposed to happen.
-	*/
-	STD_EXCEPTION(0x800, FPUnavailable, UnknownException)
-
-	/* I guess we could implement decrementer, and may have
-	 * to someday for timekeeping.
-	 */
-	STD_EXCEPTION(0x900, Decrementer, timer_interrupt)
-	STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
-	STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
-	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
-	STD_EXCEPTION(0xd00, SingleStep, UnknownException)
-
-	STD_EXCEPTION(0xe00, Trap_0e, UnknownException)
-	STD_EXCEPTION(0xf00, Trap_0f, UnknownException)
-
-	/* On the MPC8xx, this is a software emulation interrupt.  It occurs
-	 * for all unimplemented and illegal instructions.
-	 */
-	STD_EXCEPTION(0x1000, PIT, PITException)
-
-	STD_EXCEPTION(0x1100, InstructionTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1200, DataTLBMiss, UnknownException)
-	STD_EXCEPTION(0x1300, InstructionTLBError, UnknownException)
-	STD_EXCEPTION(0x1400, DataTLBError, UnknownException)
-
-	STD_EXCEPTION(0x1500, Reserved5, UnknownException)
-	STD_EXCEPTION(0x1600, Reserved6, UnknownException)
-	STD_EXCEPTION(0x1700, Reserved7, UnknownException)
-	STD_EXCEPTION(0x1800, Reserved8, UnknownException)
-	STD_EXCEPTION(0x1900, Reserved9, UnknownException)
-	STD_EXCEPTION(0x1a00, ReservedA, UnknownException)
-	STD_EXCEPTION(0x1b00, ReservedB, UnknownException)
-
-	STD_EXCEPTION(0x1c00, DataBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1d00, InstructionBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1e00, PeripheralBreakpoint, UnknownException)
-	STD_EXCEPTION(0x1f00, DevPortBreakpoint, UnknownException)
-
-	CRIT_EXCEPTION(0x2000, DebugBreakpoint, DebugException )
-
-	.globl	_end_of_vectors
-_end_of_vectors:
-
-
-	. = 0x2100
-
 /*
  * This code finishes saving the registers to the exception frame
  * and jumps to the appropriate handler for the exception.
@@ -1133,28 +1104,12 @@ transfer_to_handler:
 	SAVE_4GPRS(8, r21)
 	SAVE_8GPRS(12, r21)
 	SAVE_8GPRS(24, r21)
-#if 0
-	andi.	r23,r23,MSR_PR
-	mfspr	r23,SPRG3		/* if from user, fix up tss.regs */
-	beq	2f
-	addi	r24,r1,STACK_FRAME_OVERHEAD
-	stw	r24,PT_REGS(r23)
-2:	addi	r2,r23,-TSS		/* set r2 to current */
-	tovirt(r2,r2,r23)
-#endif
 	mflr	r23
 	andi.	r24,r23,0x3f00		/* get vector offset */
 	stw	r24,TRAP(r21)
 	li	r22,0
 	stw	r22,RESULT(r21)
 	mtspr	SPRG2,r22		/* r1 is now kernel sp */
-#if 0
-	addi	r24,r2,TASK_STRUCT_SIZE /* check for kernel stack overflow */
-	cmplw	0,r1,r2
-	cmplw	1,r1,r24
-	crand	1,1,4
-	bgt	stack_ovf		/* if r2 < r1 < r2+TASK_STRUCT_SIZE */
-#endif
 	lwz	r24,0(r23)		/* virtual address of handler */
 	lwz	r23,4(r23)		/* where to go when done */
 	mtspr	SRR0,r24
@@ -1215,16 +1170,64 @@ 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   csrr0,r2
+        mtspr   csrr1,r0
 	lwz	r0,GPR0(r1)
 	lwz	r2,GPR2(r1)
 	lwz	r1,GPR1(r1)
 	SYNC
 	rfci
 
-/* Cache functions.
-*/
+#ifdef CONFIG_440
+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   mcsrr0,r2
+        mtspr   mcsrr1,r0
+        lwz     r0,GPR0(r1)
+        lwz     r2,GPR2(r1)
+        lwz     r1,GPR1(r1)
+        SYNC
+        rfmci
+#endif /* CONFIG_440 */
+
+
+/*
+ * Cache functions.
+ *
+ * NOTE: currently the 440s run with dcache _disabled_ once relocated to DRAM,
+ * although for some cache-ralated calls stubs have to be provided to satisfy
+ * symbols resolution.
+ *
+ */
+#ifdef CONFIG_440
+       .globl  dcache_disable
+dcache_disable:
+        blr
+
+        .globl  dcache_status
+dcache_status:
+        blr
+#else
 flush_dcache:
 	addis	r9,r0,0x0002		/* set mask for EE and CE msr bits */
 	ori	r9,r9,0x8000
@@ -1303,24 +1306,13 @@ dcache_status:
 	mfdccr	r3
 	srwi	r3, r3, 31	/* >>31 => select bit 0 */
 	blr
+#endif
 
 	.globl get_pvr
 get_pvr:
 	mfspr	r3, PVR
 	blr
 
-#if !defined(CONFIG_440)
-	.globl wr_pit
-wr_pit:
-	mtspr	pit, r3
-	blr
-#endif
-
-	.globl wr_tcr
-wr_tcr:
-	mtspr	tcr, r3
-	blr
-
 /*------------------------------------------------------------------------------- */
 /* Function:	 out16 */
 /* Description:	 Output 16 bits */
@@ -1518,7 +1510,7 @@ 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! */
 
@@ -1588,7 +1580,7 @@ clear_bss:
 	 */
 	.globl	trap_init
 trap_init:
-	lwz	r7, GOT(_start)
+	lwz	r7, GOT(_start_of_vectors)
 	lwz	r8, GOT(_end_of_vectors)
 
 	li	r9, 0x100		/* reset vector always at 0x100 */
@@ -1608,35 +1600,48 @@ trap_init:
 	/*
 	 * relocate `hdlr' and `int_return' entries
 	 */
-	li	r7, .L_MachineCheck - _start + EXC_OFF_SYS_RESET
-	li	r8, Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_MachineCheck - _start + _START_OFFSET
+	li	r8, Alignment - _start + _START_OFFSET
 2:
 	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
+	addi	r7, r7, 0x100		/* next exception vector */
 	cmplw	0, r7, r8
 	blt	2b
 
-	li	r7, .L_Alignment - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_Alignment - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_ProgramCheck - _start + EXC_OFF_SYS_RESET
+	li	r7, .L_ProgramCheck - _start + _START_OFFSET
 	bl	trap_reloc
 
-	li	r7, .L_FPUnavailable - _start + EXC_OFF_SYS_RESET
-	li	r8, SystemCall - _start + EXC_OFF_SYS_RESET
-3:
-	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	3b
+#ifdef CONFIG_440
+	li	r7, .L_FPUnavailable - _start + _START_OFFSET
+        bl      trap_reloc
 
-	li	r7, .L_SingleStep - _start + EXC_OFF_SYS_RESET
-	li	r8, _end_of_vectors - _start + EXC_OFF_SYS_RESET
-4:
-	bl	trap_reloc
-	addi	r7, r7, 0x100		/* next exception vector	*/
-	cmplw	0, r7, r8
-	blt	4b
+	li	r7, .L_Decrementer - _start + _START_OFFSET
+        bl      trap_reloc
+
+	li	r7, .L_APU - _start + _START_OFFSET
+        bl      trap_reloc
+        
+	li      r7, .L_InstructionTLBError - _start + _START_OFFSET
+        bl      trap_reloc
+
+        li      r7, .L_DataTLBError - _start + _START_OFFSET
+        bl      trap_reloc
+#else /* CONFIG_440 */
+	li	r7, .L_PIT - _start + _START_OFFSET
+        bl      trap_reloc
+
+	li	r7, .L_InstructionTLBMiss - _start + _START_OFFSET
+        bl      trap_reloc
+
+	li	r7, .L_DataTLBMiss - _start + _START_OFFSET
+        bl      trap_reloc
+#endif /* CONFIG_440 */
+
+        li      r7, .L_DebugBreakpoint - _start + _START_OFFSET
+        bl      trap_reloc
 
 #if !defined(CONFIG_440)
 	addi	r7,r0,0x1000		/* set ME bit (Machine Exceptions) */
diff --git a/cpu/ppc4xx/traps.c b/cpu/ppc4xx/traps.c
index 6aecca2..54659d3 100644
--- a/cpu/ppc4xx/traps.c
+++ b/cpu/ppc4xx/traps.c
@@ -36,6 +36,8 @@
 #include <command.h>
 #include <asm/processor.h>
 
+DECLARE_GLOBAL_DATA_PTR;
+
 #if (CONFIG_COMMANDS & CFG_CMD_KGDB)
 int (*debugger_exception_handler)(struct pt_regs *) = 0;
 #endif
@@ -45,8 +47,7 @@ extern unsigned long search_exception_table(unsigned long);
 
 /* THIS NEEDS CHANGING to use the board info structure.
  */
-#define END_OF_MEM	0x00400000
-
+#define END_OF_MEM	(gd->bd->bi_memstart + gd->bd->bi_memsize)
 
 static __inline__ void set_tsr(unsigned long val)
 {
@@ -88,29 +89,29 @@ extern void do_bedbug_breakpoint(struct pt_regs *);
 void
 print_backtrace(unsigned long *sp)
 {
-	int cnt = 0;
-	unsigned long i;
-
-	printf("Call backtrace: ");
-	while (sp) {
-		if ((uint)sp > END_OF_MEM)
-			break;
-
-		i = sp[1];
-		if (cnt++ % 7 == 0)
-			printf("\n");
-		printf("%08lX ", i);
-		if (cnt > 32) break;
-		sp = (unsigned long *)*sp;
-	}
-	printf("\n");
+        int cnt = 0;
+        unsigned long i;
+
+        printf("Call backtrace: ");
+        while (sp) {
+                if ((uint)sp > END_OF_MEM)
+                        break;
+
+                i = sp[1];
+                if (cnt++ % 7 == 0)
+                        printf("\n");
+                printf("%08lX ", i);
+                if (cnt > 32) break;
+                sp = (unsigned long *)*sp;
+        }
+        printf("\n");
 }
 
 void show_regs(struct pt_regs * regs)
 {
 	int i;
 
-	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DAR: %08lX\n",
+	printf("NIP: %08lX XER: %08lX LR: %08lX REGS: %p TRAP: %04lx DEAR: %08lX\n",
 	       regs->nip, regs->xer, regs->link, regs, regs->trap, regs->dar);
 	printf("MSR: %08lx EE: %01x PR: %01x FP: %01x ME: %01x IR/DR: %01x%01x\n",
 	       regs->msr, regs->msr&MSR_EE ? 1 : 0, regs->msr&MSR_PR ? 1 : 0,
@@ -139,14 +140,14 @@ _exception(int signr, struct pt_regs *regs)
 {
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
-	panic("Exception in kernel pc %lx signal %d",regs->nip,signr);
+	panic("Exception");
 }
 
 void
 MachineCheckException(struct pt_regs *regs)
 {
-	unsigned long fixup;
-
+	unsigned long fixup, val;
+	
 	/* Probing PCI using config cycles cause this exception
 	 * when a device is not present.  Catch it and return to
 	 * the PCI exception handler.
@@ -161,26 +162,50 @@ MachineCheckException(struct pt_regs *regs)
 		return;
 #endif
 
-	printf("Machine check in kernel mode.\n");
+	printf("Machine Check Exception.\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("regs %p ", regs);
+
+	val = get_esr();
+
+#if !defined(CONFIG_440)
+	if (val& ESR_IMCP) {
+		printf("Instruction");
+		mtspr(ESR, val & ~ESR_IMCP);
+	} else
+		printf("Data");
+	printf(" machine check.\n");
+
+#elif defined(CONFIG_440)
+	if (val& ESR_IMCP){
+		printf("Instruction Synchronous Machine Check exception\n");
+		mtspr(SPRN_ESR, val & ~ESR_IMCP);
 	}
+        else { 
+		val = mfspr(MCSR);
+		if (val & MCSR_IB)
+			printf("Instruction Read PLB Error\n");
+		if (val & MCSR_DRB)
+			printf("Data Read PLB Error\n");
+		if (val & MCSR_DWB)
+			printf("Data Write PLB Error\n");
+		if (val & MCSR_TLBP)
+			printf("TLB Parity Error\n");
+		if (val & MCSR_ICP){
+			/*flush_instruction_cache(); */
+			printf("I-Cache Parity Error\n");
+		}
+		if (val & MCSR_DCSP)
+			printf("D-Cache Search Parity Error\n");
+		if (val & MCSR_DCFP)
+			printf("D-Cache Flush Parity Error\n");
+		if (val & MCSR_IMPE)
+			printf("Machine Check exception is imprecise\n");
+
+		/* Clear MCSR */
+		mtspr(SPRN_MCSR, val);
+	}
+#endif
 	show_regs(regs);
 	print_backtrace((unsigned long *)regs->gpr[1]);
 	panic("machine check");
@@ -224,7 +249,7 @@ ProgramCheckException(struct pt_regs *regs)
 }
 
 void
-PITException(struct pt_regs *regs)
+DecrementerPITException(struct pt_regs *regs)
 {
 	/*
 	 * Reset PIT interrupt
diff --git a/include/asm-ppc/processor.h b/include/asm-ppc/processor.h
index 5efc3ee..29e6101 100644
--- a/include/asm-ppc/processor.h
+++ b/include/asm-ppc/processor.h
@@ -308,7 +308,7 @@
 #define SPRN_SRR0	0x01A	/* Save/Restore Register 0 */
 #define SPRN_SRR1	0x01B	/* Save/Restore Register 1 */
 #define SPRN_SRR2	0x3DE	/* Save/Restore Register 2 */
-#define SPRN_SRR3 	0x3DF	/* Save/Restore Register 3 */
+#define SPRN_SRR3	0x3DF	/* Save/Restore Register 3 */
 #ifdef CONFIG_BOOKE
 #define SPRN_SVR	0x3FF	/* System Version Register */
 #else
@@ -451,6 +451,17 @@
 #define SPRN_PID1       0x279   /* Process ID Register 1 */
 #define SPRN_PID2       0x27a   /* Process ID Register 2 */
 #define SPRN_MCSR	0x23c	/* Machine Check Syndrome register */
+#ifdef CONFIG_440
+#define MCSR_MCS	0x80000000	/* Machine Check Summary */
+#define MCSR_IB		0x40000000	/* Instruction PLB Error */
+#define MCSR_DRB	0x20000000	/* Data Read PLB Error */
+#define MCSR_DWB	0x10000000	/* Data Write PLB Error */
+#define MCSR_TLBP	0x08000000	/* TLB Parity Error */
+#define MCSR_ICP	0x04000000	/* I-Cache Parity Error */
+#define MCSR_DCSP	0x02000000	/* D-Cache Search Parity Error */
+#define MCSR_DCFP	0x01000000	/* D-Cache Flush Parity Error */
+#define MCSR_IMPE	0x00800000	/* Imprecise Machine Check Exception */
+#endif
 #define ESR_ST          0x00800000      /* Store Operation */
 
 #if defined(CONFIG_MPC86xx)
@@ -544,6 +555,8 @@
 #define SPRG7   SPRN_SPRG7
 #define SRR0	SPRN_SRR0	/* Save and Restore Register 0 */
 #define SRR1	SPRN_SRR1	/* Save and Restore Register 1 */
+#define SRR2	SPRN_SRR2	/* Save and Restore Register 2 */
+#define SRR3	SPRN_SRR3	/* Save and Restore Register 3 */
 #define SVR	SPRN_SVR	/* System Version Register */
 #define TBRL	SPRN_TBRL	/* Time Base Read Lower Register */
 #define TBRU	SPRN_TBRU	/* Time Base Read Upper Register */
diff --git a/include/configs/CPCI440.h b/include/configs/CPCI440.h
index 90d3a8d..7b5f728 100644
--- a/include/configs/CPCI440.h
+++ b/include/configs/CPCI440.h
@@ -33,6 +33,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_CPCI440		1	    /* Board is ebony		*/
 #define CONFIG_440GP		1	    /* Specifc GP support	*/
+#define CONFIG_440		1	    /* ... PPC440 family	*/
 #define CONFIG_4xx		1	    /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	    /* Call board_early_init_f	*/
 #undef	CFG_DRAM_TEST			    /* Disable-takes long time! */
diff --git a/include/configs/KAREF.h b/include/configs/KAREF.h
index 2ad6f06..fd9bd31 100644
--- a/include/configs/KAREF.h
+++ b/include/configs/KAREF.h
@@ -38,6 +38,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_KAREF	     1		/* Board is Kamino Ref Variant */
 #define CONFIG_440GX		  1	     /* Specifc GX support	*/
+#define CONFIG_440		  1	     /* ... PPC440 family	*/
 #define CONFIG_4xx		  1	     /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	     /* Call board_pre_init	*/
 #define CONFIG_MISC_INIT_F	  1	     /* Call board misc_init_f	*/
diff --git a/include/configs/METROBOX.h b/include/configs/METROBOX.h
index 465a4ec..148fe9a 100644
--- a/include/configs/METROBOX.h
+++ b/include/configs/METROBOX.h
@@ -104,6 +104,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_METROBOX		  1	     /* Board is Metrobox	*/
 #define CONFIG_440GX		  1	     /* Specifc GX support	*/
+#define CONFIG_440		  1	     /* ... PPC440 family	*/
 #define CONFIG_4xx		  1	     /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	     /* Call board_pre_init	*/
 #define CONFIG_MISC_INIT_F	  1	     /* Call board misc_init_f	*/
diff --git a/include/configs/alpr.h b/include/configs/alpr.h
index 67f62d3..47893e8 100644
--- a/include/configs/alpr.h
+++ b/include/configs/alpr.h
@@ -29,6 +29,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_ALPR		1	    /* Board is ebony		*/
 #define CONFIG_440GX		1	    /* Specifc GX support	*/
+#define CONFIG_440		1	    /* ... PPC440 family	*/
 #define CONFIG_4xx		1	    /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	    /* Call board_pre_init	*/
 #define CONFIG_LAST_STAGE_INIT	1	    /* call last_stage_init()	*/
diff --git a/include/configs/bamboo.h b/include/configs/bamboo.h
index 763d1c7..af337ee 100644
--- a/include/configs/bamboo.h
+++ b/include/configs/bamboo.h
@@ -32,6 +32,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_BAMBOO		1	/* Board is BAMBOO              */
 #define CONFIG_440EP		1	/* Specific PPC440EP support    */
+#define CONFIG_440		1	/* ... PPC440 family	        */
 #define CONFIG_4xx		1	/* ... PPC4xx family	        */
 #define CONFIG_SYS_CLK_FREQ	33333333    /* external freq to pll	*/
 
diff --git a/include/configs/ebony.h b/include/configs/ebony.h
index a42319b..5bd326b 100644
--- a/include/configs/ebony.h
+++ b/include/configs/ebony.h
@@ -32,6 +32,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_EBONY		1	    /* Board is ebony		*/
 #define CONFIG_440GP		1	    /* Specifc GP support	*/
+#define CONFIG_440		1	    /* ... PPC440 family	*/
 #define CONFIG_4xx		1	    /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	    /* Call board_early_init_f	*/
 #undef	CFG_DRAM_TEST			    /* Disable-takes long time! */
diff --git a/include/configs/katmai.h b/include/configs/katmai.h
index cc47a16..e6ebe38 100644
--- a/include/configs/katmai.h
+++ b/include/configs/katmai.h
@@ -29,7 +29,7 @@
 
 #ifndef __CONFIG_H
 #define __CONFIG_H
-
+//#define DEBUG
 /*-----------------------------------------------------------------------
  * High Level Configuration Options
  *----------------------------------------------------------------------*/
diff --git a/include/configs/ocotea.h b/include/configs/ocotea.h
index fe4e638..31f8bb3 100644
--- a/include/configs/ocotea.h
+++ b/include/configs/ocotea.h
@@ -41,6 +41,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_OCOTEA		1	    /* Board is ebony		*/
 #define CONFIG_440GX		1	    /* Specifc GX support	*/
+#define CONFIG_440		1	    /* ... PPC440 family	*/
 #define CONFIG_4xx		1	    /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	    /* Call board_pre_init	*/
 #undef	CFG_DRAM_TEST			    /* Disable-takes long time! */
diff --git a/include/configs/p3p440.h b/include/configs/p3p440.h
index aa0901f..cae5bd5 100644
--- a/include/configs/p3p440.h
+++ b/include/configs/p3p440.h
@@ -35,6 +35,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_P3P440		1	    /* Board is P3P440		*/
 #define CONFIG_440GP		1	    /* Specifc GP support	*/
+#define CONFIG_440		1	    /* ... PPC440 family	*/
 #define CONFIG_4xx		1	    /* ... PPC4xx family	*/
 #define CONFIG_BOARD_EARLY_INIT_F 1	    /* Call board_early_init_f	*/
 #define CONFIG_MISC_INIT_R	1	    /* Call misc_init_r		*/
diff --git a/include/configs/pcs440ep.h b/include/configs/pcs440ep.h
index 23bdfc8..638031f 100644
--- a/include/configs/pcs440ep.h
+++ b/include/configs/pcs440ep.h
@@ -32,6 +32,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_PCS440EP		1	/* Board is PCS440EP            */
 #define CONFIG_440EP		1	/* Specific PPC440EP support    */
+#define CONFIG_440		1	/* ... PPC440 family	        */
 #define CONFIG_4xx		1	/* ... PPC4xx family	        */
 #define CONFIG_SYS_CLK_FREQ	33333333    /* external freq to pll	*/
 
diff --git a/include/configs/sequoia.h b/include/configs/sequoia.h
index 23243a4..42b42fc 100644
--- a/include/configs/sequoia.h
+++ b/include/configs/sequoia.h
@@ -37,6 +37,7 @@
 #else
 #define CONFIG_440GRX		1		/* Specific PPC440GRx	*/
 #endif
+#define CONFIG_440		1		/* ... PPC440 family	*/
 #define CONFIG_4xx		1		/* ... PPC4xx family	*/
 /* Detect Sequoia PLL input clock automatically via CPLD bit		*/
 #define CONFIG_SYS_CLK_FREQ    ((in8(CFG_BCSR_BASE + 3) & 0x80) ? \
diff --git a/include/configs/taishan.h b/include/configs/taishan.h
index 2b28f93..cbbb006 100644
--- a/include/configs/taishan.h
+++ b/include/configs/taishan.h
@@ -30,6 +30,7 @@
  *----------------------------------------------------------------------*/
 #define CONFIG_TAISHAN		1	/* Board is taishan		*/
 #define CONFIG_440GX		1	/* Specifc GX support		*/
+#define CONFIG_440		1	/* ... PPC440 family		*/
 #define CONFIG_4xx		1	/* ... PPC4xx family		*/
 #undef	CFG_DRAM_TEST			/* Disable-takes long time!	*/
 #define CONFIG_SYS_CLK_FREQ	33333333 /* external freq to pll	*/
diff --git a/include/configs/yosemite.h b/include/configs/yosemite.h
index b68ae54..c96b14e 100644
--- a/include/configs/yosemite.h
+++ b/include/configs/yosemite.h
@@ -38,6 +38,7 @@
 #define CONFIG_440GR		1	/* Specific PPC440GR support	*/
 #define CONFIG_HOSTNAME		yellowstone
 #endif
+#define CONFIG_440		1	/* ... PPC440 family		*/
 #define CONFIG_4xx		1	/* ... PPC4xx family		*/
 #define CONFIG_SYS_CLK_FREQ	66666666    /* external freq to pll	*/
 
diff --git a/include/ppc405.h b/include/ppc405.h
index 71ad12e..6be2a50 100644
--- a/include/ppc405.h
+++ b/include/ppc405.h
@@ -27,6 +27,15 @@
 /*--------------------------------------------------------------------- */
 	#define  srr2  0x3de      /* save/restore register 2 */
 	#define  srr3  0x3df      /* save/restore register 3 */
+
+	/*
+	 * 405 does not really have CSRR0/1 but SRR2/3 are used during critical
+	 * exception for the exact same purposes - let's alias them and have a
+	 * common handling in crit_return() and CRIT_EXCEPTION
+	 */
+	#define  csrr0 srr2
+	#define  csrr1 srr3
+
 	#define  dbsr  0x3f0      /* debug status register */
 	#define  dbcr0 0x3f2      /* debug control register 0 */
 	#define  dbcr1 0x3bd      /* debug control register 1 */
diff --git a/include/ppc440.h b/include/ppc440.h
index 07f75de..4bb644e 100644
--- a/include/ppc440.h
+++ b/include/ppc440.h
@@ -82,10 +82,7 @@
 #define	 ivor13 0x19d	/* interrupt vector offset register 13 */
 #define	 ivor14 0x19e	/* interrupt vector offset register 14 */
 #define	 ivor15 0x19f	/* interrupt vector offset register 15 */
-#if defined(CONFIG_440GX) || \
-    defined(CONFIG_440EP) || defined(CONFIG_440GR) || \
-    defined(CONFIG_440EPX) || defined(CONFIG_440GRX) || \
-    defined(CONFIG_440SP) || defined(CONFIG_440SPE)
+#if defined(CONFIG_440)
 #define	 mcsrr0 0x23a	/* machine check save/restore register 0 */
 #define	 mcsrr1 0x23b	/* mahcine check save/restore register 1 */
 #define	 mcsr	0x23c	/* machine check status register */
diff --git a/include/ppc4xx.h b/include/ppc4xx.h
index 67759c7..8cead66 100644
--- a/include/ppc4xx.h
+++ b/include/ppc4xx.h
@@ -22,6 +22,7 @@
 #ifndef	__PPC4XX_H__
 #define __PPC4XX_H__
 
+#define _START_OFFSET 0x2100
 
 #if defined(CONFIG_440)
 #include <ppc440.h>
diff --git a/include/ppc_asm.tmpl b/include/ppc_asm.tmpl
index 3e47e82..f15628a 100644
--- a/include/ppc_asm.tmpl
+++ b/include/ppc_asm.tmpl
@@ -217,7 +217,7 @@
  * We assume sprg3 has the physical address of the current
  * task's thread_struct.
  */
-#define EXCEPTION_PROLOG	\
+#define EXCEPTION_PROLOG(reg1, reg2)	\
 	mtspr	SPRG0,r20;	\
 	mtspr	SPRG1,r21;	\
 	mfcr	r20;		\
@@ -235,8 +235,10 @@
 	stw	r22,_CTR(r21);	\
 	mfspr	r20,XER;	\
 	stw	r20,_XER(r21);	\
-	mfspr	r22,SRR0;	\
-	mfspr	r23,SRR1;	\
+	mfspr	r20,DEAR;	\
+	stw	r20,_DAR(r21);	\
+	mfspr	r22,reg1;	\
+	mfspr	r23,reg2;	\
 	stw	r0,GPR0(r21);	\
 	stw	r1,GPR1(r21);	\
 	stw	r2,GPR2(r21);	\
@@ -249,41 +251,6 @@
  */
 
 /*
- * Critical exception entry code.  This is just like the other exception
- * code except that it uses SRR2 and SRR3 instead of SRR0 and SRR1.
- */
-#define CRITICAL_EXCEPTION_PROLOG       \
-	mtspr   SPRG0,r20;      \
-	mtspr   SPRG1,r21;      \
-	mfcr    r20;            \
-	subi    r21,r1,INT_FRAME_SIZE+STACK_UNDERHEAD;  /* alloc exc. frame */\
-	stw     r20,_CCR(r21);          /* save registers */ \
-	stw     r22,GPR22(r21); \
-	stw     r23,GPR23(r21); \
-	mfspr   r20,SPRG0;      \
-	stw     r20,GPR20(r21); \
-	mfspr   r22,SPRG1;      \
-	stw     r22,GPR21(r21); \
-	mflr    r20;            \
-	stw     r20,_LINK(r21); \
-	mfctr   r22;            \
-	stw     r22,_CTR(r21);  \
-	mfspr   r20,XER;        \
-	stw     r20,_XER(r21);  \
-	mfspr   r22,990;        /* SRR2 */      \
-	mfspr   r23,991;        /* SRR3 */      \
-	stw     r0,GPR0(r21);   \
-	stw     r1,GPR1(r21);   \
-	stw     r2,GPR2(r21);   \
-	stw     r1,0(r21);      \
-	mr      r1,r21;                 /* set new kernel sp */ \
-	SAVE_4GPRS(3, r21);
-/*
- * Note: code which follows this uses cr0.eq (set if from kernel),
- * r21, r22 (SRR2), and r23 (SRR3).
- */
-
-/*
  * Exception vectors.
  *
  * The data words for `hdlr' and `int_return' are initialized with
@@ -293,30 +260,45 @@
 #define STD_EXCEPTION(n, label, hdlr)			\
 	. = n;						\
 label:							\
-	EXCEPTION_PROLOG;				\
+	EXCEPTION_PROLOG(SRR0, SRR1);			\
 	lwz	r3,GOT(transfer_to_handler);		\
 	mtlr	r3;					\
 	addi	r3,r1,STACK_FRAME_OVERHEAD;		\
 	li	r20,MSR_KERNEL;				\
 	rlwimi	r20,r23,0,25,25;			\
-	blrl	; 					\
+	blrl;	 					\
 .L_ ## label :						\
-	.long	hdlr - _start + EXC_OFF_SYS_RESET;	\
-	.long	int_return - _start + EXC_OFF_SYS_RESET
-
-
-#define CRIT_EXCEPTION(n, label, hdlr)                  \
-	. = n;                                          \
-label:                                                  \
-	CRITICAL_EXCEPTION_PROLOG;                      \
-	lwz     r3,GOT(transfer_to_handler);            \
-	mtlr    r3;                                     \
-	addi    r3,r1,STACK_FRAME_OVERHEAD;             \
-	li      r20,(MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE)); \
-	rlwimi  r20,r23,0,25,25;                        \
-	blrl    ;                                       \
-.L_ ## label :                                          \
-	.long   hdlr - _start + EXC_OFF_SYS_RESET;      \
-	.long   crit_return - _start + EXC_OFF_SYS_RESET
+	.long	hdlr - _start + _START_OFFSET;		\
+	.long	int_return - _start + _START_OFFSET
+
+#define CRIT_EXCEPTION(n, label, hdlr)				\
+        . = n;							\
+label:								\
+        EXCEPTION_PROLOG(csrr0, csrr1);				\
+        lwz     r3,GOT(transfer_to_handler);			\
+        mtlr    r3;						\
+        addi    r3,r1,STACK_FRAME_OVERHEAD;			\
+        li      r20,(MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE));	\
+        rlwimi  r20,r23,0,25,25;				\
+        blrl;							\
+.L_ ## label :							\
+        .long   hdlr - _start + _START_OFFSET;			\
+        .long   crit_return - _start + _START_OFFSET
+
+#ifdef CONFIG_440
+#define MCK_EXCEPTION(n, label, hdlr)				\
+        . = n;							\
+label:								\
+        EXCEPTION_PROLOG(MCSRR0, MCSRR1);			\
+        lwz     r3,GOT(transfer_to_handler);			\
+        mtlr    r3;						\
+        addi    r3,r1,STACK_FRAME_OVERHEAD;			\
+        li      r20,(MSR_KERNEL & ~(MSR_ME|MSR_DE|MSR_CE));	\
+        rlwimi  r20,r23,0,25,25;				\
+        blrl;							\
+.L_ ## label :							\
+        .long   hdlr - _start + _START_OFFSET;			\
+        .long   mck_return - _start + _START_OFFSET
+#endif /* CONFIG_440  */
 
 #endif	/* __PPC_ASM_TMPL__ */