ARM: save and print exception contextHEADmaster

Debugging aid. Prints nice exception context info like:

R0   = 0x00000000 R8  = 0x402fe8b0
R1   = 0x402ffd80 R9  = 0x40309b15
R2   = 0x00000800 R10 = 0x00000000
R3   = 0x402ffd40 R11 = 0x00000000
R4   = 0x402ffd40 R12 = 0x402fdd38
R5   = 0x402ffd80 SP  = 0x40309694
R6   = 0x00000003 LR  = 0x402fa348
R7   = 0x00000800 PC  = 0x402f8614
VEC = 0x00000003

Data structures, definitions and code taken from RTEMS.

3 files changed, 201 insertions, 23 deletions

diff --git a/main/cpu/arm/arm.h b/main/cpu/arm/arm.h
index e56a382..5eb85a4 100644
--- a/main/cpu/arm/arm.h
+++ b/main/cpu/arm/arm.h
@@ -76,3 +76,102 @@
 #define VEC_IRQ              5
 #define VEC_RESERVED         6
 #define VEC_FIQ              7
+
+/* Taken from RTEMS score/cpu.h */
+#define ARM_EXCEPTION_FRAME_SIZE 80
+#define ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET 52
+#define ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET 72
+
+#define ARM_PSR_N (1 << 31)
+#define ARM_PSR_Z (1 << 30)
+#define ARM_PSR_C (1 << 29)
+#define ARM_PSR_V (1 << 28)
+#define ARM_PSR_Q (1 << 27)
+#define ARM_PSR_J (1 << 24)
+#define ARM_PSR_GE_SHIFT 16
+#define ARM_PSR_GE_MASK (0xf << ARM_PSR_GE_SHIFT)
+#define ARM_PSR_E (1 << 9)
+#define ARM_PSR_A (1 << 8)
+#define ARM_PSR_I (1 << 7)
+#define ARM_PSR_F (1 << 6)
+#define ARM_PSR_T (1 << 5)
+#define ARM_PSR_M_SHIFT 0
+#define ARM_PSR_M_MASK (0x1f << ARM_PSR_M_SHIFT)
+#define ARM_PSR_M_USR 0x10
+#define ARM_PSR_M_FIQ 0x11
+#define ARM_PSR_M_IRQ 0x12
+#define ARM_PSR_M_SVC 0x13
+#define ARM_PSR_M_ABT 0x17
+#define ARM_PSR_M_UND 0x1b
+#define ARM_PSR_M_SYS 0x1f
+
+#ifndef _ASSEMBLY_
+
+#include "stddefs.h"
+
+/* Exception context.
+ * These data structures gratefully taken from the RTEMS
+ * source code cpukit/score/cpu/arm/rtems/score/cpu.h
+ */
+
+typedef struct {
+  uint32_t register_fpexc;
+  uint32_t register_fpscr;
+  uint64_t register_d0;
+  uint64_t register_d1;
+  uint64_t register_d2;
+  uint64_t register_d3;
+  uint64_t register_d4;
+  uint64_t register_d5;
+  uint64_t register_d6;
+  uint64_t register_d7;
+  uint64_t register_d8;
+  uint64_t register_d9;
+  uint64_t register_d10;
+  uint64_t register_d11;
+  uint64_t register_d12;
+  uint64_t register_d13;
+  uint64_t register_d14;
+  uint64_t register_d15;
+  uint64_t register_d16;
+  uint64_t register_d17;
+  uint64_t register_d18;
+  uint64_t register_d19;
+  uint64_t register_d20;
+  uint64_t register_d21;
+  uint64_t register_d22;
+  uint64_t register_d23;
+  uint64_t register_d24;
+  uint64_t register_d25;
+  uint64_t register_d26;
+  uint64_t register_d27;
+  uint64_t register_d28;
+  uint64_t register_d29;
+  uint64_t register_d30;
+  uint64_t register_d31;
+} ARM_VFP_context;
+
+typedef struct {
+  uint32_t register_r0;
+  uint32_t register_r1;
+  uint32_t register_r2;
+  uint32_t register_r3;
+  uint32_t register_r4;
+  uint32_t register_r5;
+  uint32_t register_r6;
+  uint32_t register_r7;
+  uint32_t register_r8;
+  uint32_t register_r9;
+  uint32_t register_r10;
+  uint32_t register_r11;
+  uint32_t register_r12;
+  uint32_t register_sp;
+  void *register_lr;
+  void *register_pc;
+  uint32_t register_cpsr;
+  int vector;
+  const ARM_VFP_context *vfp_context;
+  uint32_t reserved_for_stack_alignment;
+} CPU_Exception_frame;
+
+#endif
diff --git a/main/cpu/arm/except_arm.c b/main/cpu/arm/except_arm.c
index 51230cc..fbb21e1 100644
--- a/main/cpu/arm/except_arm.c
+++ b/main/cpu/arm/except_arm.c
@@ -35,16 +35,56 @@
 ulong   ExceptionAddr;
 int ExceptionType;
 
+#define PRIx32 "lx"
+#define PRIxPTR "lx"
+
+/* Taken from RTEMS cpukit/score/cpu/arm/arm-exception-frame-print.c */
+static void _CPU_Exception_frame_print( const CPU_Exception_frame *frame )
+{
+  printf(
+    "\n"
+    "R0   = 0x%08" PRIx32 " R8  = 0x%08" PRIx32 "\n"
+    "R1   = 0x%08" PRIx32 " R9  = 0x%08" PRIx32 "\n"
+    "R2   = 0x%08" PRIx32 " R10 = 0x%08" PRIx32 "\n"
+    "R3   = 0x%08" PRIx32 " R11 = 0x%08" PRIx32 "\n"
+    "R4   = 0x%08" PRIx32 " R12 = 0x%08" PRIx32 "\n"
+    "R5   = 0x%08" PRIx32 " SP  = 0x%08" PRIx32 "\n"
+    "R6   = 0x%08" PRIx32 " LR  = 0x%08" PRIxPTR "\n"
+    "R7   = 0x%08" PRIx32 " PC  = 0x%08" PRIxPTR "\n"
+    "VEC = 0x%08" PRIxPTR "\n",
+    frame->register_r0,
+    frame->register_r8,
+    frame->register_r1,
+    frame->register_r9,
+    frame->register_r2,
+    frame->register_r10,
+    frame->register_r3,
+    frame->register_r11,
+    frame->register_r4,
+    frame->register_r12,
+    frame->register_r5,
+    frame->register_sp,
+    frame->register_r6,
+    (uint32_t) frame->register_lr,
+    frame->register_r7,
+    (uint32_t) frame->register_pc,
+    (uint32_t) frame->vector
+  );
+}
+
 /***********************************************************************
  *
  * umon_exception()
  * Default exception handler used by the low level code in vectors_arm.S.
  */
 void
-umon_exception(ulong addr, ulong type)
+umon_exception(CPU_Exception_frame *frame)
 {
-    ExceptionAddr = addr;
-    ExceptionType = type;
+    _CPU_Exception_frame_print(frame);
+
+    ExceptionAddr = (uint32_t) frame->register_pc;
+    ExceptionType = frame->vector;
+
     monrestart(EXCEPTION);
 }
 
diff --git a/main/cpu/arm/vectors_arm.S b/main/cpu/arm/vectors_arm.S
index 54d9ef2..128c4ac 100644
--- a/main/cpu/arm/vectors_arm.S
+++ b/main/cpu/arm/vectors_arm.S
@@ -27,39 +27,78 @@
  *
  */
 
+#define _ASSEMBLY_ 1
+
 #include "arm.h"
 
+#define MORE_CONTEXT_SIZE \
+  (ARM_EXCEPTION_FRAME_SIZE - ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET)
+
+#define EXCEPTIONSAVE(n)         \
+	sub     sp, #MORE_CONTEXT_SIZE	 ; \
+        stmdb   sp!, {r0-r12}	 ; \
+        mov     r4, #(n)	 ; \
+	 ; \
+        b       save_more_context	 ; \
+
 .global undefined_instruction
+.global software_interrupt
+.global abort_prefetch
+.global abort_data
+.global not_assigned
+.global interrupt_request
+.global fast_interrupt_request
+
 undefined_instruction:
-mov r1, #EXCTYPE_UNDEF
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_UNDEF)
 
-.global software_interrupt
 software_interrupt:
-mov r1, #EXCTYPE_SWI
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_SWI)
 
-.global abort_prefetch
 abort_prefetch:
-mov r1, #EXCTYPE_ABORTP
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_ABORTP)
 
-.global abort_data
 abort_data:
-mov r1, #EXCTYPE_ABORTD
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_ABORTD)
 
-.global not_assigned
 not_assigned:
-mov r1, #EXCTYPE_NOTASSGN
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_NOTASSGN)
 
-.global interrupt_request
 interrupt_request:
-mov r1, #EXCTYPE_IRQ
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_IRQ)
 
-.global fast_interrupt_request
 fast_interrupt_request:
-mov r1, #EXCTYPE_FIRQ
-b umon_exception
+EXCEPTIONSAVE(EXCTYPE_FIRQ)
+
+/* This code gratefully taken from RTEMS */
+
+save_more_context:
+        /* Save more context */
+        mov     r2, lr
+        mrs     r3, spsr
+        mrs     r7, cpsr
+        orr     r5, r3, #ARM_PSR_I
+        bic     r5, #ARM_PSR_T
+        msr     cpsr, r5
+        mov     r0, sp
+        mov     r1, lr
+        msr     cpsr, r7
+        mov     r5, #0
+        add     r6, sp, #ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET
+        stm     r6, {r0-r5}
+
+        /* Argument for high level handler */
+        mov     r0, sp
+
+        /* Clear VFP context pointer */
+        add     r3, sp, #ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET
+        mov     r1, #0
+        str     r1, [r3]
+
+        /* Call high level handler */
+	b umon_exception
+
+        /* Just in case */
+twiddle:
+        b       twiddle
+