[PATCH v2 1/3] cpukit/aarch64: Add ESR register decoding

[Kinsey Moore]

From: Alex White <alex.white at oarcorp.com>

---
 .../aarch64/aarch64-exception-frame-print.c   | 83 ++++++++++++++++++-
 1 file changed, 79 insertions(+), 4 deletions(-)

diff --git a/cpukit/score/cpu/aarch64/aarch64-exception-frame-print.c b/cpukit/score/cpu/aarch64/aarch64-exception-frame-print.c
index 59b5d06032..71d94b2f85 100644
--- a/cpukit/score/cpu/aarch64/aarch64-exception-frame-print.c
+++ b/cpukit/score/cpu/aarch64/aarch64-exception-frame-print.c
@@ -43,10 +43,70 @@
 #endif
 
 #include <inttypes.h>
+#include <stdio.h>
 
 #include <rtems/score/cpu.h>
 #include <rtems/bspIo.h>
 
+const char* _exception_class_to_string( uint16_t exception_class );
+
+void _binary_sprintf(
+  char *s,
+  size_t maxlen,
+  uint32_t num_bits,
+  uint32_t value
+);
+
+const char* _exception_class_to_string( uint16_t exception_class )
+{
+  switch (exception_class)
+  {
+  case 0b000001: return "Trapped WFI or WFE instruction";
+  case 0b000011: return "Trapped MCR or MRC access with (coproc==0b1111)";
+  case 0b000100: return "Trapped MCRR or MRRC access with (coproc==0b1111)";
+  case 0b000101: return "Trapped MCR or MRC access with (coproc==0b1110)";
+  case 0b000110: return "Trapped LDC or STC access";
+  case 0b001100: return "Trapped MRRC access with (coproc==0b1110)";
+  case 0b001110: return "Illegal Execution state";
+  case 0b011000: return "Trapped MSR, MRS, or System instruction";
+  case 0b100000: return "Instruction Abort from a lower Exception level";
+  case 0b100001: return "Instruction Abort taken without a change in Exception "
+                        "level";
+  case 0b100010: return "PC alignment fault";
+  case 0b100100: return "Data Abort from a lower Exception level";
+  case 0b100101: return "Data Abort taken without a change in Exception level";
+  case 0b100110: return "SP alignment fault";
+  case 0b110000: return "Breakpoint exception from a lower Exception level";
+  case 0b110001: return "Breakpoint exception taken without a change in "
+                        "Exception level";
+  case 0b110010: return "Software Step exception from a lower Exception level";
+  case 0b110011: return "Software Step exception taken without a change in "
+                        "Exception level";
+  case 0b110100: return "Watchpoint exception from a lower Exception level";
+  case 0b110101: return "Watchpoint exception taken without a change in "
+                        "Exception level";
+  case 0b111000: return "BKPT instruction execution in AArch32 state";
+  case 0b111100: return "BRK instruction execution in AArch64 state";
+  default: return "Unknown";
+  }
+}
+
+void _binary_sprintf(
+  char *s,
+  size_t maxlen,
+  uint32_t num_bits,
+  uint32_t value
+)
+{
+  uint32_t mask = 1<<(num_bits-1);
+  int cx = 0;
+
+  while ( mask != 0 ) {
+    cx += snprintf(s + cx, maxlen - cx, "%d", (value&mask ? 1 : 0));
+    mask >>= 1;
+  }
+}
+
 void _CPU_Exception_frame_print( const CPU_Exception_frame *frame )
 {
   printk(
@@ -68,8 +128,7 @@ void _CPU_Exception_frame_print( const CPU_Exception_frame *frame )
     "X14  = 0x%016" PRIx64  " SP   = 0x%016" PRIxPTR "\n"
     "X15  = 0x%016" PRIx64  " PC   = 0x%016" PRIxPTR "\n"
     "X16  = 0x%016" PRIx64  " DAIF = 0x%016" PRIx64 "\n"
-    "VEC  = 0x%016" PRIxPTR " CPSR = 0x%016" PRIx64 "\n"
-    "ESR  = 0x%016" PRIx64  " FAR  = 0x%016" PRIx64 "\n",
+    "VEC  = 0x%016" PRIxPTR " CPSR = 0x%016" PRIx64 "\n",
     frame->register_x0, frame->register_x17,
     frame->register_x1, frame->register_x18,
     frame->register_x2, frame->register_x19,
@@ -87,10 +146,26 @@ void _CPU_Exception_frame_print( const CPU_Exception_frame *frame )
     frame->register_x14, (intptr_t)frame->register_sp,
     frame->register_x15, (intptr_t)frame->register_pc,
     frame->register_x16, frame->register_daif,
-    (intptr_t) frame->vector, frame->register_cpsr,
-    frame->register_syndrome, frame->register_fault_address
+    (intptr_t) frame->vector, frame->register_cpsr
+  );
+
+  uint32_t ec = (frame->register_syndrome >> 26) & 0x3f;
+  uint32_t il = (frame->register_syndrome >> 25) & 0x1;
+  uint32_t iss = frame->register_syndrome & 0x1ffffff;
+  char ec_str[7];
+  char iss_str[26];
+
+  _binary_sprintf(ec_str, 7, 6, ec);
+  _binary_sprintf(iss_str, 26, 25, iss);
+
+  printk(
+    "ESR  = EC: 0b%s" " IL: 0b%d" " ISS: 0b%s" "\n"
+    "       %s\n",
+    ec_str, il, iss_str, _exception_class_to_string(ec)
   );
 
+  printk( "FAR  = 0x%016" PRIx64 "\n", frame->register_fault_address );
+
   const uint128_t *qx = &frame->register_q0;
   int i;
 
-- 
2.20.1