2 files changed, 51 insertions, 1 deletions

diff --git a/firmware/crt0.S b/firmware/crt0.S
index 1e07e0c029..54432c9731 100644
--- a/firmware/crt0.S
+++ b/firmware/crt0.S
@@ -249,6 +249,9 @@ boot_table:
     /* Set up stack for IRQ mode */ 
     msr    cpsr_c, #0xd2
     ldr    sp, =irq_stack
+    /* Let abort mode use IRQ stack */
+    msr    cpsr_c, #0xd7
+    ldr    sp, =irq_stack
     /* Switch to supervisor mode */
     msr    cpsr_c, #0xd3
     ldr    sp, =stackend
@@ -278,18 +281,37 @@ ecode:
 ecodeend:
 
     .global irq
+    .global UIE
 
+/* All illegal exceptions call into UIE with exception address as first
+   parameter. This is calculated differently depending on which exception
+   we're in. Second parameter is exception number, used for a string lookup
+   in UIE.
+ */
 undef_instr_handler:
+    mov    r0, lr
+    mov    r1, #0
+    b      UIE
+
+/* We run supervisor mode most of the time, and should never see a software
+   exception being thrown. Perhaps make it illegal and call UIE?
+ */
 software_int_handler:
 reserved_handler:
     movs   pc, lr
 
 prefetch_abort_handler:
+    sub    r0, lr, #4
+    mov    r1, #1
+    b      UIE
+
 fiq_handler:
     subs   pc, lr, #4
 
 data_abort_handler:
-    subs   pc, lr, #8
+    sub    r0, lr, #8 
+    mov    r1, #2
+    b      UIE
 
 irq_handler:
     stmfd  sp!, {r0-r3, r12, lr}
diff --git a/firmware/system.c b/firmware/system.c
index da15ee1122..81e6957c2b 100644
--- a/firmware/system.c
+++ b/firmware/system.c
@@ -1119,6 +1119,34 @@ void irq(void)
 }
 #endif
 
+static const char* const uiename[] = {
+    "Undefined instruction", "Prefetch abort", "Data abort"
+};
+
+/* Unexpected Interrupt or Exception handler. Currently only deals with
+   exceptions, but will deal with interrupts later.
+ */
+void UIE(unsigned int pc, unsigned int num)
+{
+    char str[32];
+    
+    lcd_clear_display();
+#ifdef HAVE_LCD_BITMAP
+    lcd_setfont(FONT_SYSFIXED);
+#endif
+    lcd_puts(0, 0, uiename[num]);
+    snprintf(str, sizeof(str), "at %08x", pc);
+    lcd_puts(0, 1, str);
+    lcd_update();
+    
+    while (1)
+    {
+        /* TODO: perhaps add button handling in here when we get a polling
+           driver some day.
+         */
+    }
+}
+
 /* TODO: The following two function have been lifted straight from IPL, and
    hence have a lot of numeric addresses used straight. I'd like to use
    #defines for these, but don't know what most of them are for or even what