From 6f72e28d04348ad4539679949609675b114896e1 Mon Sep 17 00:00:00 2001
From: andrewfish <andrewfish@6f19259b-4bc3-4df7-8a09-765794883524>
Date: Wed, 27 Jan 2010 02:47:47 +0000
Subject: Added a library for the default exception handler and started to add
 a dissasembler. Cleaned up some issues with BlockIo in BeagleBoard and add
 some extra info on Ebl devices command to show if a blk device has a file
 system on it.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@9830 6f19259b-4bc3-4df7-8a09-765794883524
---
 .../DefaultExceptionHandlerLib/ArmDisassembler.c   | 442 +++++++++++++++++++++
 .../DefaultExceptionHandler.c                      | 286 +++++++++++++
 .../DefaultExceptionHandlerLib.inf                 |  42 ++
 .../DefaultExceptionHandlerLib/ThumbDisassembler.c | 399 +++++++++++++++++++
 4 files changed, 1169 insertions(+)
 create mode 100644 ArmPkg/Library/DefaultExceptionHandlerLib/ArmDisassembler.c
 create mode 100644 ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
 create mode 100644 ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandlerLib.inf
 create mode 100644 ArmPkg/Library/DefaultExceptionHandlerLib/ThumbDisassembler.c

(limited to 'ArmPkg/Library/DefaultExceptionHandlerLib')

diff --git a/ArmPkg/Library/DefaultExceptionHandlerLib/ArmDisassembler.c b/ArmPkg/Library/DefaultExceptionHandlerLib/ArmDisassembler.c
new file mode 100644
index 0000000000..034236b946
--- /dev/null
+++ b/ArmPkg/Library/DefaultExceptionHandlerLib/ArmDisassembler.c
@@ -0,0 +1,442 @@
+/** @file
+  Default exception handler
+
+  Copyright (c) 2008-2010, Apple Inc. All rights reserved.
+  
+  All rights reserved. This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Library/BaseLib.h>
+#include <Library/UefiLib.h>
+#include <Library/PrintLib.h>
+
+
+CHAR8 *gCondition[] = {
+  "EQ",
+  "NE",
+  "CS",
+  "CC",
+  "MI",
+  "PL",
+  "VS",
+  "VC",
+  "HI",
+  "LS",
+  "GE",
+  "LT",
+  "GT",
+  "LE",
+  "",
+  "2"
+};
+
+#define COND(_a)  gCondition[(_a) >> 28]
+
+CHAR8 *gReg[] = {
+  "r0",
+  "r1",
+  "r2",
+  "r3",
+  "r4",
+  "r5",
+  "r6",
+  "r7",
+  "r8",
+  "r9",
+  "r10",
+  "r11",
+  "r12",
+  "sp",
+  "lr",
+  "pc"
+};
+
+CHAR8 *gLdmAdr[] = {
+  "DA",
+  "IA",
+  "DB",
+  "IB"
+};
+
+CHAR8 *gLdmStack[] = {
+  "FA",
+  "FD",
+  "EA",
+  "ED"
+};
+
+#define LDM_EXT(_reg, _off) ((_reg == 13) ? gLdmStack[(_off)] : gLdmAdr[(_off)])
+
+
+#define SIGN(_U)  ((_U) ? "" : "-")
+#define WRITE(_W) ((_W) ? "!" : "")
+#define BYTE(_B)  ((_B) ? "B":"")
+#define USER(_B)  ((_B) ? "^" : "")
+
+CHAR8 mMregListStr[4*15 + 1];
+
+CHAR8 *
+MRegList (
+  UINT32  OpCode
+  )
+{
+  UINTN     Index, Start, End;
+  CHAR8     *Str;
+  BOOLEAN   First;
+ 
+  Str = mMregListStr;
+  *Str = '\0';
+  AsciiStrCat  (Str, "{");
+  for (Index = 0, First = TRUE; Index <= 15; Index++) {
+    if ((OpCode & (1 << Index)) != 0) {
+      Start = End = Index;
+      for (Index++; ((OpCode & (1 << Index)) != 0) && Index <= 15; Index++) {
+        End = Index;
+      }
+      
+      if (!First) {
+        AsciiStrCat  (Str, ",");
+      } else {
+        First = FALSE;
+      }
+      
+      if (Start == End) {
+        AsciiStrCat  (Str, gReg[Start]);
+        AsciiStrCat  (Str, ", ");
+      } else {
+        AsciiStrCat  (Str, gReg[Start]);
+        AsciiStrCat  (Str, "-");
+        AsciiStrCat  (Str, gReg[End]);
+      }
+    }
+  }
+  if (First) {
+    AsciiStrCat  (Str, "ERROR");
+  }
+  AsciiStrCat  (Str, "}");
+  
+  // BugBug: Make caller pass in buffer it is cleaner
+  return mMregListStr;
+}
+
+CHAR8 *
+FieldMask (
+  IN  UINT32  Mask
+  )
+{
+  return "";
+}
+
+UINT32
+RotateRight (
+  IN UINT32 Op,
+  IN UINT32 Shift
+  )
+{
+  return (Op >> Shift) | (Op << (32 - Shift));
+}
+
+
+/**
+  DEBUG print the faulting instruction. We cheat and only decode instructions that access 
+  memory. If the instruction is not found we dump the instruction in hex.
+   
+  @param  Insturction   ARM instruction to disassemble.  
+  
+**/
+VOID
+DisassembleArmInstruction (
+  IN  UINT32    *OpCodePtr,
+  OUT CHAR8     *Buf,
+  OUT UINTN     Size
+  )
+{
+  UINT32    OpCode = *OpCodePtr;
+  CHAR8     *Type, *Root;
+  BOOLEAN   I, P, U, B, W, L, S, H;
+  UINT32    Rn, Rd, Rm;
+  UINT32    imode, offset_8, offset_12;
+  UINT32    Index;
+  UINT32    shift_imm, shift;
+
+  I = (OpCode & BIT25) == BIT25;
+  P = (OpCode & BIT24) == BIT24;
+  U = (OpCode & BIT23) == BIT23;
+  B = (OpCode & BIT22) == BIT22;  // Also called S
+  W = (OpCode & BIT21) == BIT21;  
+  L = (OpCode & BIT20) == BIT20;
+  S = (OpCode & BIT6) == BIT6;
+  H = (OpCode & BIT5) == BIT5;
+  Rn = (OpCode >> 16) & 0xf;
+  Rd = (OpCode >> 12) & 0xf;
+  Rm = (OpCode & 0xf);
+
+  // LDREX, STREX
+  if ((OpCode  & 0x0fe000f0) == 0x01800090) {
+    if (L) {
+      // A4.1.27  LDREX{<cond>} <Rd>, [<Rn>] 
+      AsciiSPrint (Buf, Size, "LDREX%a %a, [%a]", COND (OpCode), gReg[Rd], gReg[Rn]);   
+    } else {
+     // A4.1.103  STREX{<cond>} <Rd>, <Rm>, [<Rn>]
+      AsciiSPrint (Buf, Size, "STREX%a %a, %a, [%a]", COND (OpCode), gReg[Rd], gReg[Rn], gReg[Rn]);   
+    } 
+    return;
+  }
+  
+  // LDM/STM
+  if ((OpCode  & 0x0e000000) == 0x08000000) {
+    if (L) {
+      // A4.1.20 LDM{<cond>}<addressing_mode> <Rn>{!}, <registers>
+      // A4.1.21 LDM{<cond>}<addressing_mode> <Rn>, <registers_without_pc>^
+      // A4.1.22 LDM{<cond>}<addressing_mode> <Rn>{!}, <registers_and_pc>^
+      AsciiSPrint (Buf, Size, "LDM%a%a, %a%a, %a", COND (OpCode), LDM_EXT (Rn ,(OpCode >> 23) & 3), gReg[Rn], WRITE (W), MRegList (OpCode), USER (B));   
+    } else {
+      // A4.1.97 STM{<cond>}<addressing_mode> <Rn>{!}, <registers>
+      // A4.1.98 STM{<cond>}<addressing_mode> <Rn>, <registers>^
+      AsciiSPrint (Buf, Size, "STM%a%a, %a%a, %a", COND (OpCode), LDM_EXT (Rn ,(OpCode >> 23) & 3), gReg[Rn], WRITE (W), MRegList (OpCode), USER (B));   
+    }   
+    return;
+  }
+
+  // LDR/STR Address Mode 2
+  if ( ((OpCode  & 0x0c000000) == 0x04000000) || ((OpCode & 0xfd70f000 ) == 0xf550f000) ) {
+    offset_12 = OpCode & 0xfff;
+    if ((OpCode & 0xfd70f000 ) == 0xf550f000) {
+      Index = AsciiSPrint (Buf, Size, "PLD");
+    } else {
+      Index = AsciiSPrint (Buf, Size, "%a%a%a%a %a, ", L ? "LDR" : "STR", COND (OpCode), BYTE (B), (!P & W) ? "T":"", gReg[Rd]); 
+    }
+    if (P) {
+      if (!I) {
+        // A5.2.2 [<Rn>, #+/-<offset_12>]
+        // A5.2.5 [<Rn>, #+/-<offset_12>]
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x]%a", gReg[Rn], SIGN (U), offset_12, WRITE (W));
+      } else if ((OpCode & 0x03000ff0) == 0x03000000) {
+        // A5.2.3 [<Rn>, +/-<Rm>]
+        // A5.2.6 [<Rn>, +/-<Rm>]!
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a]%a", gReg[Rn], SIGN (U), WRITE (W));
+      } else {
+        // A5.2.4 [<Rn>, +/-<Rm>, LSL #<shift_imm>]
+        // A5.2.7 [<Rn>, +/-<Rm>, LSL #<shift_imm>]!
+        shift_imm = (OpCode >> 7) & 0x1f;
+        shift = (OpCode >> 5) & 0x3;
+        if (shift == 0x0) {
+          Type = "LSL";
+        } else if (shift == 0x1) {
+          Type = "LSR";
+          if (shift_imm == 0) {
+            shift_imm = 32;
+          }
+        } else if (shift == 0x12) {
+          Type = "ASR";
+        } else if (shift_imm == 0) {
+          AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, RRX]%a", gReg[Rn], SIGN (U), gReg[Rm], WRITE (W));
+          return;
+        } else {
+          Type = "ROR";
+        }
+          
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a%a, %a, #%d]%a", gReg[Rn], SIGN (U), gReg[Rm], Type, shift_imm, WRITE (W));
+      }
+    } else {  // !P
+      if (!I) {
+        // A5.2.8  [<Rn>], #+/-<offset_12>
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a0x%x", gReg[Rn], SIGN (U), offset_12);
+      } else if ((OpCode & 0x03000ff0) == 0x03000000) {
+        // A5.2.9  [<Rn>], +/-<Rm>
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (U), gReg[Rm]);
+      } else {
+        // A5.2.10 [<Rn>], +/-<Rm>, LSL #<shift_imm>
+        shift_imm = (OpCode >> 7) & 0x1f;
+        shift = (OpCode >> 5) & 0x3;
+
+        if (shift == 0x0) {
+          Type = "LSL";
+        } else if (shift == 0x1) {
+          Type = "LSR";
+          if (shift_imm == 0) {
+            shift_imm = 32;
+          }
+        } else if (shift == 0x12) {
+          Type = "ASR";
+        } else if (shift_imm == 0) {
+          AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, RRX", gReg[Rn], SIGN (U), gReg[Rm]);
+          // FIx me
+          return;
+        } else {
+          Type = "ROR";
+        }
+          
+        AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a%a, %a, #%d", gReg[Rn], SIGN (U), gReg[Rm], Type, shift_imm);
+      }
+    }
+    return;  
+  }
+  
+  if ((OpCode  & 0x0e000000) == 0x00000000) {
+    // LDR/STR address mode 3
+    // LDR|STR{<cond>}H|SH|SB|D <Rd>, <addressing_mode>
+    if (L) {
+      if (!S) {
+        Root = "LDR%aH %a, ";
+      } else if (!H) {
+        Root = "LDR%aSB %a, ";
+      } else {
+        Root = "LDR%aSH %a, ";
+      }
+    } else {
+      if (!S) {
+        Root = "STR%aH %a ";
+      } else if (!H) {
+        Root = "LDR%aD %a ";
+      } else {
+        Root = "STR%aD %a ";
+      }
+    }
+  
+    Index = AsciiSPrint (Buf, Size, Root, COND (OpCode), gReg[Rd]); 
+
+    S = (OpCode & BIT6) == BIT6;
+    H = (OpCode & BIT5) == BIT5;
+    offset_8 = ((OpCode >> 4) | (OpCode * 0xf)) & 0xff;
+    if (P & !W) {
+      // Immediate offset/index
+      if (B) {
+        // A5.3.2  [<Rn>, #+/-<offset_8>]
+        // A5.3.4  [<Rn>, #+/-<offset_8>]!
+        AsciiSPrint  (&Buf[Index], Size - Index, "[%a, #%a%d]%a", gReg[Rn], SIGN (U), offset_8, WRITE (W));
+      } else {
+        // A5.3.3  [<Rn>, +/-<Rm>]
+        // A5.3.5  [<Rn>, +/-<Rm>]!
+        AsciiSPrint  (&Buf[Index], Size - Index, "[%a, #%a%]a", gReg[Rn], SIGN (U), gReg[Rm], WRITE (W));
+      }
+    } else {
+      // Register offset/index
+      if (B) {
+        // A5.3.6 [<Rn>], #+/-<offset_8>
+        AsciiSPrint  (&Buf[Index], Size - Index, "[%a], #%a%d", gReg[Rn], SIGN (U), offset_8);
+      } else {
+        // A5.3.7 [<Rn>], +/-<Rm>
+        AsciiSPrint  (&Buf[Index], Size - Index, "[%a], #%a%a", gReg[Rn], SIGN (U), gReg[Rm]);
+      }
+    }
+    return;
+  }
+
+  if ((OpCode  & 0x0fb000f0) == 0x01000050) {
+    // A4.1.108  SWP   SWP{<cond>}B <Rd>, <Rm>, [<Rn>]
+    // A4.1.109  SWPB  SWP{<cond>}B <Rd>, <Rm>, [<Rn>]
+    AsciiSPrint (Buf, Size, "SWP%a%a %a, %a, [%a]", COND (OpCode), BYTE (B), gReg[Rd], gReg[Rm], gReg[Rn]);
+    return;
+  }
+  
+  if ((OpCode  & 0xfe5f0f00) == 0xf84d0500) {
+    // A4.1.90 SRS SRS<addressing_mode> #<mode>{!}
+    AsciiSPrint (Buf, Size, "SRS%a #0x%x%a", gLdmStack[(OpCode >> 23) & 3], OpCode & 0x1f, WRITE (W));
+    return;
+  }
+
+  if ((OpCode  & 0xfe500f00) == 0xf8100500) {
+    // A4.1.59 RFE<addressing_mode> <Rn>{!}
+    AsciiSPrint (Buf, Size, "RFE%a %a", gLdmStack[(OpCode >> 23) & 3], gReg[Rn], WRITE (W));
+    return;
+  }
+  
+  if ((OpCode  & 0xfff000f0) == 0xe1200070) {
+    // A4.1.7 BKPT <immed_16>
+    AsciiSPrint (Buf, Size, "BKPT %x", ((OpCode >> 8) | (OpCode & 0xf)) & 0xffff);
+    return;
+  }  
+  
+  if ((OpCode  & 0xfff10020) == 0xf1000000) {
+    // A4.1.16 CPS<effect> <iflags> {, #<mode>}
+    if (((OpCode >> 6) & 0x7) == 0) {
+      AsciiSPrint (Buf, Size, "CPS #0x%x", (OpCode & 0x2f));
+    } else {
+      imode = (OpCode >> 18) & 0x3;
+      Index = AsciiSPrint (Buf, Size, "CPS%a %a%a%a", (imode == 3) ? "ID":"IE", (OpCode & BIT8) ? "A":"", (OpCode & BIT7) ? "I":"", (OpCode & BIT6) ? "F":"");
+      if ((OpCode & BIT17) != 0) {
+        AsciiSPrint (&Buf[Index], Size - Index, ", #0x%x", OpCode & 0x1f);
+      }
+    }
+    return;
+  }  
+    
+  if ((OpCode  & 0x0f000000) == 0x0f000000) {
+    // A4.1.107 SWI{<cond>} <immed_24>
+    AsciiSPrint (Buf, Size, "SWI%a %x", COND (OpCode), OpCode & 0x00ffffff);
+    return;
+  }  
+
+  if ((OpCode  & 0x0fb00000) == 0x01000000) {
+    // A4.1.38 MRS{<cond>} <Rd>, CPSR  MRS{<cond>} <Rd>, SPSR
+    AsciiSPrint (Buf, Size, "MRS%a %a, %a", COND (OpCode), gReg[Rd], B ? "SPSR" : "CPSR");
+    return;
+  }  
+
+
+  if ((OpCode  & 0x0db00000) == 0x03200000) {
+    // A4.1.38 MSR{<cond>} CPSR_<fields>, #<immediate> MSR{<cond>} CPSR_<fields>, <Rm>
+    if (I) {
+      // MSR{<cond>} CPSR_<fields>, #<immediate>
+      AsciiSPrint (Buf, Size, "MRS%a %a_%a, #0x%x", COND (OpCode),  B ? "SPSR" : "CPSR", FieldMask ((OpCode >> 16) & 0xf), RotateRight (OpCode & 0xf, ((OpCode >> 8) & 0xf) *2));
+    } else {
+      // MSR{<cond>} CPSR_<fields>, <Rm>
+      AsciiSPrint (Buf, Size, "MRS%a %a_%a, %a", COND (OpCode), B ? "SPSR" : "CPSR", gReg[Rd]);
+    }
+    return;
+  }  
+
+  if ((OpCode  & 0xff000010) == 0xfe000000) {
+    // A4.1.13 CDP{<cond>} <coproc>, <opcode_1>, <CRd>, <CRn>, <CRm>, <opcode_2>
+    AsciiSPrint (Buf, Size, "CDP%a 0x%x, 0x%x, CR%d, CR%d, CR%d, 0x%x", COND (OpCode), (OpCode >> 8) & 0xf, (OpCode >> 20) & 0xf, Rn, Rd, Rm, (OpCode >> 5) &0x7);
+    return;
+  }
+  
+  if ((OpCode  & 0x0e000000) == 0x0c000000) {
+    // A4.1.19 LDC and A4.1.96 SDC
+    if ((OpCode & 0xf0000000) == 0xf0000000) {
+      Index = AsciiSPrint (Buf, Size, "%a2 0x%x, CR%d, ", L ? "LDC":"SDC", (OpCode >> 8) & 0xf, Rd);
+    } else {
+      Index = AsciiSPrint (Buf, Size, "%a%a 0x%x, CR%d, ",  L ? "LDC":"SDC", COND (OpCode), (OpCode >> 8) & 0xf, Rd);
+    }
+    
+    if (!P) {
+      if (!W) { 
+        // A5.5.5.5 [<Rn>], <option>
+      AsciiSPrint (&Buf[Index], Size - Index, "[%a], {0x%x}", gReg[Rn], OpCode & 0xff); 
+      } else {
+        // A.5.5.4  [<Rn>], #+/-<offset_8>*4
+      AsciiSPrint (&Buf[Index], Size - Index, "[%a], #%a0x%x*4", gReg[Rn], SIGN (U), OpCode & 0xff); 
+      }
+    } else {
+      // A5.5.5.2 [<Rn>, #+/-<offset_8>*4 ]!
+      AsciiSPrint (&Buf[Index], Size - Index, "[%a, #%a0x%x*4]%a", gReg[Rn], SIGN (U), OpCode & 0xff, WRITE (W)); 
+    }
+    
+  }
+  
+  if ((OpCode  & 0x0f000010) == 0x0e000010) {
+    // A4.1.32 MRC2, MCR2 
+    AsciiSPrint (Buf, Size, "%a%a 0x%x, 0x%x, %a, CR%d, CR%d, 0x%x", L ? "MRC":"MCR", COND (OpCode), (OpCode >> 8) & 0xf, (OpCode >> 20) & 0xf, gReg[Rd], Rn, Rm, (OpCode >> 5) &0x7);
+    return;    
+  }
+
+  if ((OpCode  & 0x0ff00000) == 0x0c400000) {
+    // A4.1.33 MRRC2, MCRR2 
+    AsciiSPrint (Buf, Size, "%a%a 0x%x, 0x%x, %a, %a, CR%d", L ? "MRRC":"MCRR", COND (OpCode), (OpCode >> 4) & 0xf, (OpCode >> 20) & 0xf, gReg[Rd], gReg[Rn], Rm);
+    return;    
+  }
+
+  AsciiSPrint (Buf, Size, "Faulting OpCode 0x%08x", OpCode);
+  return;
+}
+
diff --git a/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
new file mode 100644
index 0000000000..56504948a3
--- /dev/null
+++ b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
@@ -0,0 +1,286 @@
+/** @file
+  Default exception handler
+
+  Copyright (c) 2008-2010, Apple Inc. All rights reserved.
+  
+  All rights reserved. This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Uefi.h>
+#include <Library/UefiLib.h>
+#include <Library/BaseLib.h>
+#include <Library/DebugLib.h>
+#include <Library/PeCoffGetEntryPointLib.h>
+
+#include <Guid/DebugImageInfoTable.h>
+#include <Protocol/DebugSupport.h>
+#include <Protocol/LoadedImage.h>
+
+
+VOID
+DisassembleArmInstruction (
+  IN  UINT32    *OpCodePtr,
+  OUT CHAR8     *Buf,
+  OUT UINTN     Size
+  );
+
+VOID
+DisassembleThumbInstruction (
+  IN  UINT16    *OpCodePtr,
+  OUT CHAR8     *Buf,
+  OUT UINTN     Size
+  );
+
+
+EFI_DEBUG_IMAGE_INFO_TABLE_HEADER *gDebugImageTableHeader = NULL;
+
+
+typedef struct {
+  UINT32  BIT;
+  CHAR8   Char;
+} CPSR_CHAR;
+
+
+
+ 
+/**
+  Use the EFI Debug Image Table to lookup the FaultAddress and find which PE/COFF image 
+  it came from. As long as the PE/COFF image contains a debug directory entry a 
+  string can be returned. For ELF and Mach-O images the string points to the Mach-O or ELF
+  image. Microsoft tools contain a pointer to the PDB file that contains the debug information.
+
+  @param  FaultAddress         Address to find PE/COFF image for. 
+  @param  ImageBase            Return load address of found image
+  @param  PeCoffSizeOfHeaders  Return the size of the PE/COFF header for the image that was found
+
+  @retval NULL                 FaultAddress not in a loaded PE/COFF image.
+  @retval                      Path and file name of PE/COFF image.
+  
+**/
+CHAR8 *
+GetImageName (
+  IN  UINT32  FaultAddress,
+  OUT UINT32  *ImageBase,
+  OUT UINT32  *PeCoffSizeOfHeaders
+  )
+{
+  EFI_DEBUG_IMAGE_INFO  *DebugTable;
+  UINTN                 Entry;
+  CHAR8                 *Address;
+
+  
+  DebugTable = gDebugImageTableHeader->EfiDebugImageInfoTable;
+  if (DebugTable == NULL) {
+    return NULL;
+  }
+
+  Address = (CHAR8 *)(UINTN)FaultAddress;
+  for (Entry = 0; Entry < gDebugImageTableHeader->TableSize; Entry++, DebugTable++) {
+    if (DebugTable->NormalImage != NULL) {
+      if ((DebugTable->NormalImage->ImageInfoType == EFI_DEBUG_IMAGE_INFO_TYPE_NORMAL) && 
+          (DebugTable->NormalImage->LoadedImageProtocolInstance != NULL)) {
+        if ((Address >= (CHAR8 *)DebugTable->NormalImage->LoadedImageProtocolInstance->ImageBase) &&
+            (Address <= ((CHAR8 *)DebugTable->NormalImage->LoadedImageProtocolInstance->ImageBase + DebugTable->NormalImage->LoadedImageProtocolInstance->ImageSize))) {
+          *ImageBase = (UINT32)DebugTable->NormalImage->LoadedImageProtocolInstance->ImageBase;
+          *PeCoffSizeOfHeaders = PeCoffGetSizeOfHeaders ((VOID *)(UINTN)*ImageBase);
+          return PeCoffLoaderGetPdbPointer (DebugTable->NormalImage->LoadedImageProtocolInstance->ImageBase);
+        }           
+      }
+    }  
+  }
+
+  return NULL;
+}
+
+
+/**
+  Convert the Current Program Status Register (CPSR) to a string. The string is 
+  a defacto standard in the ARM world. 
+  
+  It is possible to add extra bits by adding them to CpsrChar array.
+
+  @param  Cpsr         ARM CPSR register value
+  @param  ReturnStr    32 byte string that contains string version of CPSR
+
+**/
+VOID
+CpsrString (
+  IN  UINT32  Cpsr,
+  OUT CHAR8   *ReturnStr
+  )
+{
+  UINTN Index;
+  CHAR8 *Str = ReturnStr;
+  CHAR8 *ModeStr;
+  CPSR_CHAR CpsrChar[] = {
+    { 31, 'n' },
+    { 30, 'z' },
+    { 29, 'c' },
+    { 28, 'v' },
+
+    { 9,  'e' },
+    { 8,  'a' },
+    { 7,  'i' },
+    { 6,  'f' },
+    { 5,  't' },
+    { 0,  '?' }
+  };
+  
+  for (Index = 0; CpsrChar[Index].BIT != 0; Index++, Str++) {
+    *Str = CpsrChar[Index].Char;
+    if ((Cpsr & (1 << CpsrChar[Index].BIT)) != 0) {
+      // Concert to upper case if bit is set
+      *Str &= ~0x20;
+    }
+  }
+  
+  *Str++ = '_';
+  *Str = '\0';
+  
+  switch (Cpsr & 0x1f) {
+  case 0x10:
+    ModeStr = "usr";
+    break;
+  case 0x011:
+    ModeStr = "fiq";
+    break;
+  case 0x12:
+    ModeStr = "irq";
+    break;
+  case 0x13:
+    ModeStr = "svc";
+    break;
+  case 0x16:
+    ModeStr = "mon";
+    break;
+  case 0x17:
+    ModeStr = "abt";
+    break;
+  case 0x1b:
+    ModeStr = "und";
+    break;
+  case 0x1f:
+    ModeStr = "sys";
+    break;
+  
+  default:
+    ModeStr = "???";
+    break;
+  }
+  
+  AsciiStrCat (Str, ModeStr);
+  return;
+}  
+  
+CHAR8 *gExceptionTypeString[] = {
+  "Reset",
+  "Undefined OpCode",
+  "SWI",
+  "Prefetch Abort",
+  "Data Abort",
+  "Undefined",
+  "IRQ",
+  "FIQ"
+};
+
+
+/**
+  This is the default action to take on an unexpected exception
+  
+  Since this is exception context don't do anything crazy like try to allcoate memory.
+
+  @param  ExceptionType    Type of the exception
+  @param  SystemContext    Register state at the time of the Exception
+
+
+**/
+VOID
+DefaultExceptionHandler (
+  IN     EFI_EXCEPTION_TYPE           ExceptionType,
+  IN OUT EFI_SYSTEM_CONTEXT           SystemContext
+  )
+{
+
+  DEBUG ((EFI_D_ERROR, "\n%a Exception PC at 0x%08x  CPSR 0x%08x ", gExceptionTypeString[ExceptionType], SystemContext.SystemContextArm->PC, SystemContext.SystemContextArm->CPSR));
+  DEBUG_CODE_BEGIN ();
+    CHAR8   *Pdb;
+    UINT32  ImageBase;
+    UINT32  PeCoffSizeOfHeader;
+    UINT32  Offset;
+    CHAR8   CpsrStr[32];  // char per bit. Lower 5-bits are mode that is a 3 char string
+    CHAR8   Buffer[80];
+    
+    CpsrString (SystemContext.SystemContextArm->CPSR, CpsrStr);
+    DEBUG ((EFI_D_ERROR, "%a\n", CpsrStr));
+  
+    Pdb = GetImageName (SystemContext.SystemContextArm->PC, &ImageBase, &PeCoffSizeOfHeader);
+    Offset = SystemContext.SystemContextArm->PC - ImageBase;
+    if (Pdb != NULL) {
+      DEBUG ((EFI_D_ERROR, "%a\n", Pdb));
+
+      //
+      // A PE/COFF image loads its headers into memory so the headers are 
+      // included in the linked addressess. ELF and Mach-O images do not
+      // include the headers so the first byte of the image is usually
+      // text (code). If you look at link maps from ELF or Mach-O images
+      // you need to subtact out the size of the PE/COFF header to get
+      // get the offset that matches the link map. 
+      //
+      DEBUG ((EFI_D_ERROR, "loaded at 0x%08x (PE/COFF offset) 0x%x (ELF or Mach-O offset) 0x%x", ImageBase, Offset, Offset - PeCoffSizeOfHeader));
+      
+      // If we come from an image it is safe to show the instruction. We know it should not fault
+      if ((SystemContext.SystemContextArm->CPSR & 0x20) == 0) {
+        // ARM
+        DisassembleArmInstruction ((UINT32 *)(UINTN)SystemContext.SystemContextArm->PC, Buffer, sizeof (Buffer));
+        DEBUG ((EFI_D_ERROR, "\n%a", Buffer));
+      } else {
+        // Thumb
+        DEBUG ((EFI_D_ERROR, "\nFaulting Instruction 0x%04x", (UINT16 *)(UINTN)SystemContext.SystemContextArm->PC));
+      }
+    }
+  DEBUG_CODE_END ();
+  DEBUG ((EFI_D_ERROR, "\n  R0 0x%08x   R1 0x%08x   R2 0x%08x   R3 0x%08x\n", SystemContext.SystemContextArm->R0, SystemContext.SystemContextArm->R1, SystemContext.SystemContextArm->R2, SystemContext.SystemContextArm->R3));
+  DEBUG ((EFI_D_ERROR, "  R4 0x%08x   R5 0x%08x   R6 0x%08x   R7 0x%08x\n", SystemContext.SystemContextArm->R4, SystemContext.SystemContextArm->R5, SystemContext.SystemContextArm->R6, SystemContext.SystemContextArm->R7));
+  DEBUG ((EFI_D_ERROR, "  R8 0x%08x   R9 0x%08x  R10 0x%08x  R11 0x%08x\n", SystemContext.SystemContextArm->R8, SystemContext.SystemContextArm->R9, SystemContext.SystemContextArm->R10, SystemContext.SystemContextArm->R11));
+  DEBUG ((EFI_D_ERROR, " R12 0x%08x   SP 0x%08x   LR 0x%08x   PC 0x%08x\n", SystemContext.SystemContextArm->R12, SystemContext.SystemContextArm->SP, SystemContext.SystemContextArm->LR, SystemContext.SystemContextArm->PC));
+  DEBUG ((EFI_D_ERROR, "DFSR 0x%08x DFAR 0x%08x IFSR 0x%08x IFAR 0x%08x\n\n", SystemContext.SystemContextArm->DFSR, SystemContext.SystemContextArm->DFAR, SystemContext.SystemContextArm->IFSR, SystemContext.SystemContextArm->IFAR));
+
+  ASSERT (FALSE);
+}
+
+
+
+
+/**
+  The constructor function caches EFI Debug table information for use in the exception handler.
+  
+
+  @param  ImageHandle   The firmware allocated handle for the EFI image.
+  @param  SystemTable   A pointer to the EFI System Table.
+  
+  @retval EFI_SUCCESS   The constructor always returns EFI_SUCCESS.
+
+**/
+EFI_STATUS
+EFIAPI
+DefaultExceptionHandlerConstructor (
+  IN EFI_HANDLE                ImageHandle,
+  IN EFI_SYSTEM_TABLE          *SystemTable
+  )
+{
+  EFI_STATUS  Status;
+  
+  
+  Status = EfiGetSystemConfigurationTable (&gEfiDebugImageInfoTableGuid, (VOID **)&gDebugImageTableHeader);
+  if (EFI_ERROR (Status)) {
+    gDebugImageTableHeader = NULL;
+  }
+  return Status;
+}
diff --git a/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandlerLib.inf b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandlerLib.inf
new file mode 100644
index 0000000000..a989895545
--- /dev/null
+++ b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandlerLib.inf
@@ -0,0 +1,42 @@
+#/** @file
+# Semihosting  serail port lib
+#
+# Copyright (c) 2008, Apple Inc.
+#
+#  All rights reserved. This program and the accompanying materials
+#  are licensed and made available under the terms and conditions of the BSD License
+#  which accompanies this distribution. The full text of the license may be found at
+#  http://opensource.org/licenses/bsd-license.php
+#  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+#
+#
+#**/
+
+[Defines]
+  INF_VERSION                    = 0x00010005
+  BASE_NAME                      = SemiHostingSerialPortLib
+  FILE_GUID                      = EACDB354-DF1A-4AF9-A171-499737ED818F
+  MODULE_TYPE                    = UEFI_DRIVER
+  VERSION_STRING                 = 1.0
+  LIBRARY_CLASS                  = DefaultExceptionHandlerLib
+  CONSTRUCTOR                    = DefaultExceptionHandlerConstructor
+
+
+[Sources.common]
+  DefaultExceptionHandler.c
+  ArmDisassembler.c
+  ThumbDisassembler.c
+
+[Packages]
+  MdePkg/MdePkg.dec
+  ArmPkg/ArmPkg.dec
+
+[LibraryClasses]
+  UefiLib
+  BaseLib
+  PrintLib
+  DebugLib
+  PeCoffGetEntryPointLib
+
+
diff --git a/ArmPkg/Library/DefaultExceptionHandlerLib/ThumbDisassembler.c b/ArmPkg/Library/DefaultExceptionHandlerLib/ThumbDisassembler.c
new file mode 100644
index 0000000000..b021e7881c
--- /dev/null
+++ b/ArmPkg/Library/DefaultExceptionHandlerLib/ThumbDisassembler.c
@@ -0,0 +1,399 @@
+/** @file
+  Default exception handler
+
+  Copyright (c) 2008-2010, Apple Inc. All rights reserved.
+  
+  All rights reserved. This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include <Base.h>
+#include <Library/BaseLib.h>
+#include <Library/PrintLib.h>
+
+extern CHAR8 *gReg[];
+
+#define LOAD_STORE_FORMAT1            1
+#define LOAD_STORE_FORMAT2            2
+#define LOAD_STORE_FORMAT3            3
+#define LOAD_STORE_FORMAT4            4
+#define LOAD_STORE_MULTIPLE_FORMAT1   5 
+#define LOAD_STORE_MULTIPLE_FORMAT2   6 
+#define IMMED_8                       7
+#define CONDITIONAL_BRANCH            8
+#define UNCONDITIONAL_BRANCH          9
+#define UNCONDITIONAL_BRANCH_SHORT  109
+#define BRANCH_EXCHANGE              10
+#define DATA_FORMAT1                 11
+#define DATA_FORMAT2                 12
+#define DATA_FORMAT3                 13
+#define DATA_FORMAT4                 14
+#define DATA_FORMAT5                 15
+#define DATA_FORMAT6_SP              16
+#define DATA_FORMAT6_PC             116
+#define DATA_FORMAT7                 17
+#define DATA_FORMAT8                 19
+#define CPS_FORMAT                   20
+#define ENDIAN_FORMAT                21
+     
+
+typedef struct {
+  CHAR8   *Start;
+  UINT32  OpCode;
+  UINT32  Mask;
+  UINT32  AddressMode;
+} THUMB_INSTRUCTIONS;
+
+THUMB_INSTRUCTIONS gOp[] = {
+// Thumb 16-bit instrucitons
+//         Op      Mask    Format
+  { "ADC" , 0x4140, 0xffc0, DATA_FORMAT5 },
+
+  { "ADD" , 0x1c00, 0xfe00, DATA_FORMAT2 },
+  { "ADD" , 0x3000, 0xf800, DATA_FORMAT3 },
+  { "ADD" , 0x1800, 0xfe00, DATA_FORMAT1 },
+  { "ADD" , 0x4400, 0xff00, DATA_FORMAT8 },   // A8.6.9
+  { "ADD" , 0xa000, 0xf100, DATA_FORMAT6_PC },
+  { "ADD" , 0xa100, 0xf100, DATA_FORMAT6_SP }, 
+  { "ADD" , 0xb000, 0xff10, DATA_FORMAT7 },
+
+  { "AND" , 0x4000, 0xffc0, DATA_FORMAT5 },
+
+  { "ASR" , 0x1000, 0xf800, DATA_FORMAT4 },
+  { "ASR" , 0x4100, 0xffc0, DATA_FORMAT5 },
+
+  { "B"   , 0xd000, 0xf000, CONDITIONAL_BRANCH },
+  { "B"   , 0xe000, 0xf100, UNCONDITIONAL_BRANCH_SHORT },
+  { "BL"  , 0xf100, 0xf100, UNCONDITIONAL_BRANCH },
+  { "BLX" , 0xe100, 0xf100, UNCONDITIONAL_BRANCH },
+  { "BLX" , 0x4780, 0xff80, BRANCH_EXCHANGE },
+  { "BX"  , 0x4700, 0xff80, BRANCH_EXCHANGE },
+
+  { "BIC" , 0x4380, 0xffc0, DATA_FORMAT5 },
+  { "BKPT", 0xdf00, 0xff00, IMMED_8 },
+  { "CMN" , 0x42c0, 0xffc0, DATA_FORMAT5 },
+
+  { "CMP" , 0x2800, 0xf100, DATA_FORMAT3 },
+  { "CMP" , 0x4280, 0xffc0, DATA_FORMAT5 },
+  { "CMP" , 0x4500, 0xff00, DATA_FORMAT8 },
+
+  { "CPS" , 0xb660, 0xffe8, CPS_FORMAT },
+  { "CPY" , 0x4600, 0xff00, DATA_FORMAT8 },
+  { "EOR" , 0x4040, 0xffc0, DATA_FORMAT5 },
+
+  { "LDMIA" , 0xc800, 0xf800, LOAD_STORE_MULTIPLE_FORMAT1 },
+  { "LDR"   , 0x6800, 0xf800, LOAD_STORE_FORMAT1 },
+  { "LDR"   , 0x5800, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "LDR"   , 0x4800, 0xf800, LOAD_STORE_FORMAT3 },
+  { "LDR"   , 0x9800, 0xf800, LOAD_STORE_FORMAT4 },
+  { "LDRB"  , 0x7800, 0xf800, LOAD_STORE_FORMAT1 },
+  { "LDRB"  , 0x5c00, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "LDRH"  , 0x8800, 0xf800, LOAD_STORE_FORMAT1 },
+  { "LDRH"  , 0x7a00, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "LDRSB" , 0x5600, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "LDRSH" , 0x5e00, 0xfe00, LOAD_STORE_FORMAT2 },
+ 
+  { "LSL" , 0x0000, 0xf800, DATA_FORMAT4 },
+  { "LSL" , 0x4080, 0xffc0, DATA_FORMAT5 },
+  { "LSR" , 0x0001, 0xf800, DATA_FORMAT4 },
+  { "LSR" , 0x40c0, 0xffc0, DATA_FORMAT5 },
+
+  { "MOV" , 0x2000, 0xf800, DATA_FORMAT3 },
+  { "MOV" , 0x1c00, 0xffc0, DATA_FORMAT3 },
+  { "MOV" , 0x4600, 0xff00, DATA_FORMAT8 },
+
+  { "MUL" , 0x4340, 0xffc0, DATA_FORMAT5 },
+  { "MVN" , 0x41c0, 0xffc0, DATA_FORMAT5 },
+  { "NEG" , 0x4240, 0xffc0, DATA_FORMAT5 },
+  { "ORR" , 0x4180, 0xffc0, DATA_FORMAT5 },
+  { "POP" , 0xbc00, 0xfe00, LOAD_STORE_MULTIPLE_FORMAT2 },
+  { "POP" , 0xe400, 0xfe00, LOAD_STORE_MULTIPLE_FORMAT2 },
+  
+  { "REV"   , 0xba00, 0xffc0, DATA_FORMAT5 },
+  { "REV16" , 0xba40, 0xffc0, DATA_FORMAT5 },
+  { "REVSH" , 0xbac0, 0xffc0, DATA_FORMAT5 },
+
+  { "ROR"  , 0x41c0, 0xffc0, DATA_FORMAT5 },
+  { "SBC"  , 0x4180, 0xffc0, DATA_FORMAT5 },
+  { "SETEND"  , 0xb650, 0xfff0, ENDIAN_FORMAT },
+
+  { "STMIA" , 0xc000, 0xf800, LOAD_STORE_MULTIPLE_FORMAT1 },
+  { "STR"   , 0x6000, 0xf800, LOAD_STORE_FORMAT1 },
+  { "STR"   , 0x5000, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "STR"   , 0x4000, 0xf800, LOAD_STORE_FORMAT3 },
+  { "STR"   , 0x9000, 0xf800, LOAD_STORE_FORMAT4 },
+  { "STRB"  , 0x7000, 0xf800, LOAD_STORE_FORMAT1 },
+  { "STRB"  , 0x5800, 0xfe00, LOAD_STORE_FORMAT2 },
+  { "STRH"  , 0x8000, 0xf800, LOAD_STORE_FORMAT1 },
+  { "STRH"  , 0x5200, 0xfe00, LOAD_STORE_FORMAT2 },
+
+  { "SUB" , 0x1e00, 0xfe00, DATA_FORMAT2 },
+  { "SUB" , 0x3800, 0xf800, DATA_FORMAT3 },
+  { "SUB" , 0x1a00, 0xfe00, DATA_FORMAT1 },
+  { "SUB" , 0xb080, 0xff80, DATA_FORMAT7 },
+
+  { "SWI" , 0xdf00, 0xff00, IMMED_8 },
+  { "SXTB", 0xb240, 0xffc0, DATA_FORMAT5 },
+  { "SXTH", 0xb200, 0xffc0, DATA_FORMAT5 },
+  { "TST" , 0x4200, 0xffc0, DATA_FORMAT5 },
+  { "UXTB", 0xb2c0, 0xffc0, DATA_FORMAT5 },
+  { "UXTH", 0xb280, 0xffc0, DATA_FORMAT5 }
+  
+#if 0  
+  ,
+  
+  // 32-bit Thumb instructions  op1 01
+  
+  //  1110 100x x0xx xxxx xxxx xxxx xxxx xxxx Load/store multiple
+  { "SRSDB", 0xe80dc000, 0xffdffff0, SRS_FORMAT },       // SRSDB<c> SP{!},#<mode>
+  { "SRS"  , 0xe98dc000, 0xffdffff0, SRS_IA_FORMAT },    // SRS{IA}<c> SP{!},#<mode>
+  { "RFEDB", 0xe810c000, 0xffd0ffff, RFE_FORMAT },       // RFEDB<c> <Rn>{!}
+  { "RFE"  , 0xe990c000, 0xffd0ffff, RFE_IA_FORMAT },    // RFE{IA}<c> <Rn>{!}
+  
+  { "STM"  , 0xe8800000, 0xffd00000,  STM_FORMAT },      // STM<c>.W <Rn>{!},<registers>
+  { "LDM"  , 0xe8900000, 0xffd00000,  STM_FORMAT },      // LDR<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+  { "POP"  , 0xe8bd0000, 0xffff2000,  REGLIST_FORMAT },  // POP<c>.W <registers> >1 register
+  { "POP"  , 0xf85d0b04, 0xffff0fff,  RT_FORMAT },       // POP<c>.W <registers>  1 register
+
+  { "STMDB", 0xe9000000, 0xffd00000,  STM_FORMAT },      // STMDB
+  { "PUSH" , 0xe8bd0000, 0xffffa000,  REGLIST_FORMAT },  // PUSH<c>.W <registers>  >1 register
+  { "PUSH" , 0xf84d0b04, 0xffff0fff,  RT_FORMAT },       // PUSH<c>.W <registers>   1 register
+  { "LDMDB", 0xe9102000, 0xffd02000,  STM_FORMAT },      // LDMDB<c> <Rn>{!},<registers>
+
+  //  1110 100x x1xx xxxx xxxx xxxx xxxx xxxx Load/store dual,
+  { "STREX" , 0xe0400000, 0xfff000f0, 3REG_IMM8_FORMAT },  // STREX<c> <Rd>,<Rt>,[<Rn>{,#<imm>}]
+  { "STREXB", 0xe8c00f40, 0xfff00ff0, 3REG_FORMAT },       // STREXB<c> <Rd>,<Rt>,[<Rn>]
+  { "STREXD", 0xe8c00070, 0xfff000f0, 4REG_FORMAT },       // STREXD<c> <Rd>,<Rt>,<Rt2>,[<Rn>]
+  { "STREXH", 0xe8c00f70, 0xfff00ff0, 3REG_FORMAT },       // STREXH<c> <Rd>,<Rt>,[<Rn>]
+  { "STRH",   0xf8c00000, 0xfff00000, 2REG_IMM8_FORMAT },  // STRH<c>.W <Rt>,[<Rn>{,#<imm12>}]
+  { "STRH",   0xf8200000, 0xfff00000,  },                  // STRH<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]
+
+
+
+  //  1110 101x xxxx xxxx xxxx xxxx xxxx xxxx Data-processing
+  //  1110 11xx xxxx xxxx xxxx xxxx xxxx xxxx Coprocessor
+  
+  //  1111 0x0x xxxx xxxx 0xxx xxxx xxxx xxxx Data-processing modified immediate
+  //  1111 0x1x xxxx xxxx 0xxx xxxx xxxx xxxx Data-processing plain immediate
+  //  1111 0xxx xxxx xxxx 1xxx xxxx xxxx xxxx Branches
+  
+  //  1111 1000 xxx0 xxxx xxxx xxxx xxxx xxxx Store single data item
+  //  1111 1001 xxx0 xxxx xxxx xxxx xxxx xxxx SIMD or load/store
+  //  1111 100x x001 xxxx xxxx xxxx xxxx xxxx Load byte, memory hints 
+  //  1111 100x x011 xxxx xxxx xxxx xxxx xxxx Load halfword, memory hints
+  //  1111 100x x101 xxxx xxxx xxxx xxxx xxxx Load word 
+
+  //  1111 1 010 xxxx xxxx xxxx xxxx xxxx xxxx Data-processing register
+  //  1111 1 011 0xxx xxxx xxxx xxxx xxxx xxxx Multiply
+  //  1111 1 011 1xxx xxxx xxxx xxxx xxxx xxxx Long Multiply
+  //  1111 1 1xx xxxx xxxx xxxx xxxx xxxx xxxx Coprocessor 
+#endif
+};
+
+
+CHAR8 mThumbMregListStr[4*15 + 1];
+
+CHAR8 *
+ThumbMRegList (
+  UINT32  OpCode
+  )
+{
+  UINTN     Index, Start, End;
+  CHAR8     *Str;
+  BOOLEAN   First;
+ 
+  Str = mThumbMregListStr;
+  *Str = '\0';
+  AsciiStrCat  (Str, "{");
+  // R0 - R7, PC
+  for (Index = 0, First = TRUE; Index <= 9; Index++) {
+    if ((OpCode & (1 << Index)) != 0) {
+      Start = End = Index;
+      for (Index++; ((OpCode & (1 << Index)) != 0) && (Index <= 9); Index++) {
+        End = Index;
+      }
+      
+      if (!First) {
+        AsciiStrCat  (Str, ",");
+      } else {
+        First = FALSE;
+      }
+      
+      if (Start == End) {
+        AsciiStrCat  (Str, gReg[(Start == 9)?15:Start]);
+        AsciiStrCat  (Str, ", ");
+      } else {
+        AsciiStrCat  (Str, gReg[Start]);
+        AsciiStrCat  (Str, "-");
+        AsciiStrCat  (Str, gReg[(End == 9)?15:End]);
+      }
+    }
+  }
+  if (First) {
+    AsciiStrCat  (Str, "ERROR");
+  }
+  AsciiStrCat  (Str, "}");
+  
+  // BugBug: Make caller pass in buffer it is cleaner
+  return mThumbMregListStr;
+}
+
+UINT32
+SignExtend (
+  IN  UINT32  Data
+  )
+{
+  return 0;
+}
+
+/**
+  DEBUG print the faulting instruction. We cheat and only decode instructions that access 
+  memory. If the instruction is not found we dump the instruction in hex.
+   
+  @param  Insturction   ARM instruction to disassemble.  
+  
+**/
+VOID
+DisassembleThumbInstruction (
+  IN  UINT16    *OpCodePtr,
+  OUT CHAR8     *Buf,
+  OUT UINTN     Size
+  )
+{
+  UINT16  OpCode = *OpCodePtr;
+  UINT32  Index;
+  UINT32  Offset;
+  UINT16  Rd, Rn, Rm;
+  INT32   target_addr;
+  BOOLEAN H1, H2, imod;
+  UINT32  PC;
+
+  // These register names match branch form, but not others
+  Rd = OpCode & 0x7;
+  Rn = (OpCode >> 3) & 0x7;
+  Rm = (OpCode >> 6) & 0x7;
+  H1 = (OpCode & BIT7) != 0;
+  H2 = (OpCode & BIT6) != 0;
+  imod = (OpCode & BIT4) != 0;
+  PC = (UINT32)(UINTN)*OpCodePtr;
+
+  for (Index = 0; Index < sizeof (gOp)/sizeof (THUMB_INSTRUCTIONS); Index++) {
+    if ((OpCode & gOp[Index].Mask) == gOp[Index].OpCode) {
+      Offset = AsciiSPrint (Buf, Size, "%a", gOp[Index].Start);   
+      switch (gOp[Index].AddressMode) {
+      case LOAD_STORE_FORMAT1:
+        // A6.5.1  <Rd>, [<Rn>, #<5_bit_offset>]
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d #0x%x]", Rd, (OpCode >> 7) & 7, (OpCode >> 6) & 0x1f);   
+        break;
+      case LOAD_STORE_FORMAT2:
+        // A6.5.1  <Rd>, [<Rn>, <Rm>]
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d, r%d]", Rd, (OpCode >> 3) & 7, Rm);   
+        break;
+      case LOAD_STORE_FORMAT3:
+        // A6.5.1 <Rd>, [PC, #<8_bit_offset>]
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [pc, #0x%x]", (OpCode >> 8) & 7, OpCode & 0xff);   
+        break;
+      case LOAD_STORE_FORMAT4:
+        // FIX ME!!!!!
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [sp, #0x%x]", (OpCode >> 8) & 7, OpCode & 0xff);   
+        break;
+      
+      case LOAD_STORE_MULTIPLE_FORMAT1:
+        // <Rn>!, <registers>  
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d!, %a", (OpCode >> 8) & 7, ThumbMRegList (!BIT8 & OpCode));   
+        break;
+      case LOAD_STORE_MULTIPLE_FORMAT2:
+        // <Rn>!, <registers>  
+        // BIT8 is PC 
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d!, %a", (OpCode >> 8) & 7, ThumbMRegList (OpCode));   
+        break;
+      
+      case IMMED_8:
+        // A6.7 <immed_8>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%x", OpCode & 0xff);   
+        break;
+
+      case CONDITIONAL_BRANCH:
+        // A6.3.1 B<cond> <target_address>
+        AsciiSPrint (&Buf[Offset], Size - Offset, "%a 0x%04x", PC + 4 + SignExtend ((OpCode & 0xff) << 1));   
+        break;
+      case UNCONDITIONAL_BRANCH_SHORT:
+        // A6.3.2 B  <target_address>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x", PC + 4 + SignExtend ((OpCode & 0x3ff) << 1));   
+        break;
+      case UNCONDITIONAL_BRANCH:
+        // A6.3.2 BL|BLX <target_address>  ; Produces two 16-bit instructions 
+        target_addr = *(OpCodePtr - 1);
+        if ((target_addr & 0xf800) == 0xf000) {
+          target_addr = ((target_addr & 0x3ff) << 12) | (OpCode & 0x3ff);
+        } else {
+          target_addr = OpCode & 0x3ff;
+        }
+        // PC + 2 +/- target_addr
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x", PC + 2 + SignExtend (target_addr));   
+        break;
+      case BRANCH_EXCHANGE:
+        // A6.3.3 BX|BLX <Rm>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d", gReg[Rn | (H2 ? 8:0)]);   
+        break;
+
+      case DATA_FORMAT1:
+        // A6.4.3  <Rd>, <Rn>, <Rm>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, r%d", Rd, Rn, Rm);   
+        break;
+      case DATA_FORMAT2:
+        // A6.4.3  <Rd>, <Rn>, #3_bit_immed
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", Rd, Rn, Rm);   
+        break;
+      case DATA_FORMAT3:
+        // A6.4.3  <Rd>|<Rn>, #8_bit_immed
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", (OpCode >> 8) & 0x7, OpCode & 0xff);   
+        break;
+      case DATA_FORMAT4:
+        // A6.4.3  <Rd>|<Rm>, #immed_5
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", Rn, Rd, (OpCode >> 6) & 0x1f);   
+        break;
+      case DATA_FORMAT5:
+        // A6.4.3  <Rd>|<Rm>, <Rm>|<Rs>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d", Rd, Rn);   
+        break;
+      case DATA_FORMAT6_SP:
+        // A6.4.3  <Rd>, <reg>, #<8_Bit_immed>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, sp, 0x%x", (OpCode >> 8) & 7, OpCode & 0xff);   
+        break;
+      case DATA_FORMAT6_PC:
+        // A6.4.3  <Rd>, <reg>, #<8_Bit_immed>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, pc, 0x%x", (OpCode >> 8) & 7, OpCode & 0xff);   
+        break;
+      case DATA_FORMAT7:
+        // A6.4.3  SP, SP, #<7_Bit_immed>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " sp, sp 0x%x", (OpCode & 0x7f)*4);   
+        break;
+      case DATA_FORMAT8:
+        // A6.4.3  <Rd>|<Rn>, <Rm>
+        AsciiSPrint (&Buf[Offset], Size - Offset, " %a, %a", gReg[Rd | (H1 ? 8:0)], gReg[Rn | (H2 ? 8:0)]);   
+        break;
+      
+      case CPS_FORMAT:
+        // A7.1.24
+        AsciiSPrint (&Buf[Offset], Size - Offset, "%a %a%a%a", imod ? "ID":"IE", ((OpCode & BIT2) == 0) ? "":"a",  ((OpCode & BIT1) == 0) ? "":"i", ((OpCode & BIT0) == 0) ? "":"f");   
+        break;
+
+      case ENDIAN_FORMAT:
+        // A7.1.24
+        AsciiSPrint (&Buf[Offset], Size - Offset, " %a", (OpCode & BIT3) == 0 ? "LE":"BE");   
+        break;
+      }
+    }
+  }
+      
+
+}
+
+ 
-- 
cgit v1.2.3