Merge branch 'master' of git://git.denx.de/u-boot-arm

2 files changed, 157 insertions, 5 deletions

diff --git a/cpu/blackfin/interrupt.S b/cpu/blackfin/interrupt.S
index 8c7a262..69bba3f 100644
--- a/cpu/blackfin/interrupt.S
+++ b/cpu/blackfin/interrupt.S
@@ -8,6 +8,9 @@
 #include <config.h>
 #include <asm/blackfin.h>
 #include <asm/entry.h>
+#include <asm/ptrace.h>
+#include <asm/deferred.h>
+#include <asm/mach-common/bits/core.h>
 
 .text
 
@@ -19,10 +22,75 @@ ENTRY(_trap)
 	SAVE_ALL_SYS
 
 	r0 = sp;	/* stack frame pt_regs pointer argument ==> r0 */
+	r1 = 3;		/* EVT3 space */
 	sp += -12;
 	call _trap_c;
 	sp += 12;
 
+#ifdef CONFIG_EXCEPTION_DEFER
+	CC = R0 == 0;
+	IF CC JUMP .Lexit_trap;
+
+	/* To avoid double faults, lower our priority to IRQ5 */
+	p4.l = lo(COREMMR_BASE);
+	p4.h = hi(COREMMR_BASE);
+
+	r7.h = _exception_to_level5;
+	r7.l = _exception_to_level5;
+	[p4 + (EVT5 - COREMMR_BASE)] = r7;
+
+	/*
+	 * Save these registers, as they are only valid in exception context
+	 *  (where we are now - as soon as we defer to IRQ5, they can change)
+	 */
+	p5.l = _deferred_regs;
+	p5.h = _deferred_regs;
+	r6 = [p4 + (DCPLB_FAULT_ADDR - COREMMR_BASE)];
+	[p5 + (deferred_regs_DCPLB_FAULT_ADDR * 4)] = r6;
+
+	r6 = [p4 + (ICPLB_FAULT_ADDR - COREMMR_BASE)];
+	[p5 + (deferred_regs_ICPLB_FAULT_ADDR * 4)] = r6;
+
+	/* Save the state of single stepping */
+	r6 = SYSCFG;
+	[p5 + (deferred_regs_SYSCFG * 4)] = r6;
+	/* Clear it while we handle the exception in IRQ5 mode
+	 * RESTORE_ALL_SYS will load it, so all we need to do is store it
+	 * in the right place
+	 */
+	BITCLR(r6, SYSCFG_SSSTEP_P);
+	[SP + PT_SYSCFG] = r6;
+
+	/* Since we are going to clobber RETX, we need to save it */
+	r6 = retx;
+	[p5 + (deferred_regs_retx * 4)] = r6;
+
+	/* Save the current IMASK, since we change in order to jump to level 5 */
+	cli r6;
+	[p5 + (deferred_regs_IMASK * 4)] = r6;
+
+	/* Disable all interrupts, but make sure level 5 is enabled so
+	 * we can switch to that level.
+	 */
+	r6 = 0x3f;
+	sti r6;
+
+	/* Clobber RETX so we don't end up back at a faulting instruction */
+	[sp + PT_RETX] = r7;
+
+	/* In case interrupts are disabled IPEND[4] (global interrupt disable bit)
+	 * clear it (re-enabling interrupts again) by the special sequence of pushing
+	 * RETI onto the stack.  This way we can lower ourselves to IVG5 even if the
+	 * exception was taken after the interrupt handler was called but before it
+	 * got a chance to enable global interrupts itself.
+	 */
+	[--sp] = reti;
+	sp += 4;
+
+	RAISE 5;
+.Lexit_trap:
+#endif
+
 #if ANOMALY_05000257
 	R7  = LC0;
 	LC0 = R7;
@@ -35,6 +103,43 @@ ENTRY(_trap)
 	rtx;
 ENDPROC(_trap)
 
+#ifdef CONFIG_EXCEPTION_DEFER
+/* Deferred (IRQ5) exceptions */
+ENTRY(_exception_to_level5)
+	SAVE_ALL_SYS
+
+	/* Now we have to fix things up */
+	p4.l = lo(EVT5);
+	p4.h = hi(EVT5);
+	r0.l = _evt_default;
+	r0.h = _evt_default;
+	[p4] = r0;
+	csync;
+
+	p4.l = _deferred_regs;
+	p4.h = _deferred_regs;
+	r0 = [p4 + (deferred_regs_retx * 4)];
+	[sp + PT_PC] = r0;
+
+	r0 = [p4 + (deferred_regs_SYSCFG * 4)];
+	[sp + PT_SYSCFG] = r0;
+
+	r0 = sp;	/* stack frame pt_regs pointer argument ==> r0 */
+	r1 = 5;	/* EVT5 space */
+	sp += -12;
+	call _trap_c;
+	sp += 12;
+
+	/* Restore IMASK */
+	r0 = [p4 + (deferred_regs_IMASK * 4)];
+	sti r0;
+
+	RESTORE_ALL_SYS
+
+	rti;
+ENDPROC(_exception_to_level5)
+#endif
+
 /* default entry point for interrupts */
 ENTRY(_evt_default)
 	SAVE_ALL_SYS
diff --git a/cpu/blackfin/traps.c b/cpu/blackfin/traps.c
index becc36e..caaea94 100644
--- a/cpu/blackfin/traps.c
+++ b/cpu/blackfin/traps.c
@@ -18,6 +18,7 @@
  */
 
 #include <common.h>
+#include <kgdb.h>
 #include <linux/types.h>
 #include <asm/traps.h>
 #include <asm/cplb.h>
@@ -25,6 +26,7 @@
 #include <asm/mach-common/bits/core.h>
 #include <asm/mach-common/bits/mpu.h>
 #include <asm/mach-common/bits/trace.h>
+#include <asm/deferred.h>
 #include "cpu.h"
 
 #define trace_buffer_save(x) \
@@ -69,8 +71,16 @@ const struct memory_map const bfin_memory_map[] = {
 	}
 };
 
-void trap_c(struct pt_regs *regs)
+#ifdef CONFIG_EXCEPTION_DEFER
+unsigned int deferred_regs[deferred_regs_last];
+#endif
+
+/*
+ * Handle all exceptions while running in EVT3 or EVT5
+ */
+int trap_c(struct pt_regs *regs, uint32_t level)
 {
+	uint32_t ret = 0;
 	uint32_t trapnr = (regs->seqstat & EXCAUSE);
 	bool data = false;
 
@@ -87,7 +97,7 @@ void trap_c(struct pt_regs *regs)
 			 */
 			if (last_cplb_fault_retx != regs->retx) {
 				last_cplb_fault_retx = regs->retx;
-				return;
+				return ret;
 			}
 		}
 
@@ -102,6 +112,12 @@ void trap_c(struct pt_regs *regs)
 		size_t i;
 		unsigned long tflags;
 
+#ifdef CONFIG_EXCEPTION_DEFER
+		/* This should never happen */
+		if (level == 5)
+			bfin_panic(regs);
+#endif
+
 		/*
 		 * Keep the trace buffer so that a miss here points people
 		 * to the right place (their code).  Crashes here rarely
@@ -167,11 +183,40 @@ void trap_c(struct pt_regs *regs)
 		trace_buffer_restore(tflags);
 		break;
 	}
-
+#ifdef CONFIG_CMD_KGDB
+	/* Single step
+	 * if we are in IRQ5, just ignore, otherwise defer, and handle it in kgdb
+	 */
+	case VEC_STEP:
+		if (level == 3) {
+			/* If we just returned from an interrupt, the single step
+			 * event is for the RTI instruction.
+			 */
+			if (regs->retx == regs->pc)
+				break;
+			/* we just return if we are single stepping through IRQ5 */
+			if (regs->ipend & 0x20)
+				break;
+			/* Otherwise, turn single stepping off & fall through,
+			 * which defers to IRQ5
+			 */
+			regs->syscfg &= ~1;
+		}
+		/* fall through */
+#endif
 	default:
-		/* All traps come here */
+#ifdef CONFIG_CMD_KGDB
+		if (level == 3) {
+			/* We need to handle this at EVT5, so try again */
+			ret = 1;
+			break;
+		}
+		if (debugger_exception_handler && (*debugger_exception_handler)(regs))
+			return 0;
+#endif
 		bfin_panic(regs);
 	}
+	return ret;
 }
 
 #ifdef CONFIG_DEBUG_DUMP
@@ -251,8 +296,10 @@ void dump(struct pt_regs *fp)
 	if (!ENABLE_DUMP)
 		return;
 
-	/* fp->ipend is garbage, so load it ourself */
+#ifndef CONFIG_CMD_KGDB
+	/* fp->ipend is normally garbage, so load it ourself */
 	fp->ipend = bfin_read_IPEND();
+#endif
 
 	hwerrcause = (fp->seqstat & HWERRCAUSE) >> HWERRCAUSE_P;
 	excause = (fp->seqstat & EXCAUSE) >> EXCAUSE_P;