+/** @file\r
+ Default exception handler\r
+\r
+ Copyright (c) 2008-2010, Apple Inc. All rights reserved.\r
+ \r
+ All rights reserved. This program and the accompanying materials\r
+ are licensed and made available under the terms and conditions of the BSD License\r
+ which accompanies this distribution. The full text of the license may be found at\r
+ http://opensource.org/licenses/bsd-license.php\r
+\r
+ THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
+ WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
+\r
+**/\r
+\r
+#include <Base.h>\r
+#include <Library/BaseLib.h>\r
+#include <Library/PrintLib.h>\r
+\r
+extern CHAR8 *gReg[];\r
+\r
+#define LOAD_STORE_FORMAT1 1\r
+#define LOAD_STORE_FORMAT2 2\r
+#define LOAD_STORE_FORMAT3 3\r
+#define LOAD_STORE_FORMAT4 4\r
+#define LOAD_STORE_MULTIPLE_FORMAT1 5 \r
+#define LOAD_STORE_MULTIPLE_FORMAT2 6 \r
+#define IMMED_8 7\r
+#define CONDITIONAL_BRANCH 8\r
+#define UNCONDITIONAL_BRANCH 9\r
+#define UNCONDITIONAL_BRANCH_SHORT 109\r
+#define BRANCH_EXCHANGE 10\r
+#define DATA_FORMAT1 11\r
+#define DATA_FORMAT2 12\r
+#define DATA_FORMAT3 13\r
+#define DATA_FORMAT4 14\r
+#define DATA_FORMAT5 15\r
+#define DATA_FORMAT6_SP 16\r
+#define DATA_FORMAT6_PC 116\r
+#define DATA_FORMAT7 17\r
+#define DATA_FORMAT8 19\r
+#define CPS_FORMAT 20\r
+#define ENDIAN_FORMAT 21\r
+ \r
+\r
+typedef struct {\r
+ CHAR8 *Start;\r
+ UINT32 OpCode;\r
+ UINT32 Mask;\r
+ UINT32 AddressMode;\r
+} THUMB_INSTRUCTIONS;\r
+\r
+THUMB_INSTRUCTIONS gOpThumb[] = {\r
+// Thumb 16-bit instrucitons\r
+// Op Mask Format\r
+ { "ADC" , 0x4140, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "ADD" , 0x1c00, 0xfe00, DATA_FORMAT2 },\r
+ { "ADD" , 0x3000, 0xf800, DATA_FORMAT3 },\r
+ { "ADD" , 0x1800, 0xfe00, DATA_FORMAT1 },\r
+ { "ADD" , 0x4400, 0xff00, DATA_FORMAT8 }, // A8.6.9\r
+ { "ADD" , 0xa000, 0xf100, DATA_FORMAT6_PC },\r
+ { "ADD" , 0xa100, 0xf100, DATA_FORMAT6_SP }, \r
+ { "ADD" , 0xb000, 0xff10, DATA_FORMAT7 },\r
+\r
+ { "AND" , 0x4000, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "ASR" , 0x1000, 0xf800, DATA_FORMAT4 },\r
+ { "ASR" , 0x4100, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "B" , 0xd000, 0xf000, CONDITIONAL_BRANCH },\r
+ { "B" , 0xe000, 0xf100, UNCONDITIONAL_BRANCH_SHORT },\r
+ { "BL" , 0xf100, 0xf100, UNCONDITIONAL_BRANCH },\r
+ { "BLX" , 0xe100, 0xf100, UNCONDITIONAL_BRANCH },\r
+ { "BLX" , 0x4780, 0xff80, BRANCH_EXCHANGE },\r
+ { "BX" , 0x4700, 0xff80, BRANCH_EXCHANGE },\r
+\r
+ { "BIC" , 0x4380, 0xffc0, DATA_FORMAT5 },\r
+ { "BKPT", 0xdf00, 0xff00, IMMED_8 },\r
+ { "CMN" , 0x42c0, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "CMP" , 0x2800, 0xf100, DATA_FORMAT3 },\r
+ { "CMP" , 0x4280, 0xffc0, DATA_FORMAT5 },\r
+ { "CMP" , 0x4500, 0xff00, DATA_FORMAT8 },\r
+\r
+ { "CPS" , 0xb660, 0xffe8, CPS_FORMAT },\r
+ { "CPY" , 0x4600, 0xff00, DATA_FORMAT8 },\r
+ { "EOR" , 0x4040, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "LDMIA" , 0xc800, 0xf800, LOAD_STORE_MULTIPLE_FORMAT1 },\r
+ { "LDR" , 0x6800, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "LDR" , 0x5800, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "LDR" , 0x4800, 0xf800, LOAD_STORE_FORMAT3 },\r
+ { "LDR" , 0x9800, 0xf800, LOAD_STORE_FORMAT4 },\r
+ { "LDRB" , 0x7800, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "LDRB" , 0x5c00, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "LDRH" , 0x8800, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "LDRH" , 0x7a00, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "LDRSB" , 0x5600, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "LDRSH" , 0x5e00, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ \r
+ { "LSL" , 0x0000, 0xf800, DATA_FORMAT4 },\r
+ { "LSL" , 0x4080, 0xffc0, DATA_FORMAT5 },\r
+ { "LSR" , 0x0001, 0xf800, DATA_FORMAT4 },\r
+ { "LSR" , 0x40c0, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "MOV" , 0x2000, 0xf800, DATA_FORMAT3 },\r
+ { "MOV" , 0x1c00, 0xffc0, DATA_FORMAT3 },\r
+ { "MOV" , 0x4600, 0xff00, DATA_FORMAT8 },\r
+\r
+ { "MUL" , 0x4340, 0xffc0, DATA_FORMAT5 },\r
+ { "MVN" , 0x41c0, 0xffc0, DATA_FORMAT5 },\r
+ { "NEG" , 0x4240, 0xffc0, DATA_FORMAT5 },\r
+ { "ORR" , 0x4180, 0xffc0, DATA_FORMAT5 },\r
+ { "POP" , 0xbc00, 0xfe00, LOAD_STORE_MULTIPLE_FORMAT2 },\r
+ { "POP" , 0xe400, 0xfe00, LOAD_STORE_MULTIPLE_FORMAT2 },\r
+ \r
+ { "REV" , 0xba00, 0xffc0, DATA_FORMAT5 },\r
+ { "REV16" , 0xba40, 0xffc0, DATA_FORMAT5 },\r
+ { "REVSH" , 0xbac0, 0xffc0, DATA_FORMAT5 },\r
+\r
+ { "ROR" , 0x41c0, 0xffc0, DATA_FORMAT5 },\r
+ { "SBC" , 0x4180, 0xffc0, DATA_FORMAT5 },\r
+ { "SETEND" , 0xb650, 0xfff0, ENDIAN_FORMAT },\r
+\r
+ { "STMIA" , 0xc000, 0xf800, LOAD_STORE_MULTIPLE_FORMAT1 },\r
+ { "STR" , 0x6000, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "STR" , 0x5000, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "STR" , 0x4000, 0xf800, LOAD_STORE_FORMAT3 },\r
+ { "STR" , 0x9000, 0xf800, LOAD_STORE_FORMAT4 },\r
+ { "STRB" , 0x7000, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "STRB" , 0x5800, 0xfe00, LOAD_STORE_FORMAT2 },\r
+ { "STRH" , 0x8000, 0xf800, LOAD_STORE_FORMAT1 },\r
+ { "STRH" , 0x5200, 0xfe00, LOAD_STORE_FORMAT2 },\r
+\r
+ { "SUB" , 0x1e00, 0xfe00, DATA_FORMAT2 },\r
+ { "SUB" , 0x3800, 0xf800, DATA_FORMAT3 },\r
+ { "SUB" , 0x1a00, 0xfe00, DATA_FORMAT1 },\r
+ { "SUB" , 0xb080, 0xff80, DATA_FORMAT7 },\r
+\r
+ { "SWI" , 0xdf00, 0xff00, IMMED_8 },\r
+ { "SXTB", 0xb240, 0xffc0, DATA_FORMAT5 },\r
+ { "SXTH", 0xb200, 0xffc0, DATA_FORMAT5 },\r
+ { "TST" , 0x4200, 0xffc0, DATA_FORMAT5 },\r
+ { "UXTB", 0xb2c0, 0xffc0, DATA_FORMAT5 },\r
+ { "UXTH", 0xb280, 0xffc0, DATA_FORMAT5 }\r
+};\r
+\r
+#if 0 \r
+THUMB_INSTRUCTIONS gOpThumb2[] = {\r
+ ,\r
+ \r
+ // 32-bit Thumb instructions op1 01\r
+ \r
+ // 1110 100x x0xx xxxx xxxx xxxx xxxx xxxx Load/store multiple\r
+ { "SRSDB", 0xe80dc000, 0xffdffff0, SRS_FORMAT }, // SRSDB<c> SP{!},#<mode>\r
+ { "SRS" , 0xe98dc000, 0xffdffff0, SRS_IA_FORMAT }, // SRS{IA}<c> SP{!},#<mode>\r
+ { "RFEDB", 0xe810c000, 0xffd0ffff, RFE_FORMAT }, // RFEDB<c> <Rn>{!}\r
+ { "RFE" , 0xe990c000, 0xffd0ffff, RFE_IA_FORMAT }, // RFE{IA}<c> <Rn>{!}\r
+ \r
+ { "STM" , 0xe8800000, 0xffd00000, STM_FORMAT }, // STM<c>.W <Rn>{!},<registers>\r
+ { "LDM" , 0xe8900000, 0xffd00000, STM_FORMAT }, // LDR<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]\r
+ { "POP" , 0xe8bd0000, 0xffff2000, REGLIST_FORMAT }, // POP<c>.W <registers> >1 register\r
+ { "POP" , 0xf85d0b04, 0xffff0fff, RT_FORMAT }, // POP<c>.W <registers> 1 register\r
+\r
+ { "STMDB", 0xe9000000, 0xffd00000, STM_FORMAT }, // STMDB\r
+ { "PUSH" , 0xe8bd0000, 0xffffa000, REGLIST_FORMAT }, // PUSH<c>.W <registers> >1 register\r
+ { "PUSH" , 0xf84d0b04, 0xffff0fff, RT_FORMAT }, // PUSH<c>.W <registers> 1 register\r
+ { "LDMDB", 0xe9102000, 0xffd02000, STM_FORMAT }, // LDMDB<c> <Rn>{!},<registers>\r
+\r
+ // 1110 100x x1xx xxxx xxxx xxxx xxxx xxxx Load/store dual,\r
+ { "STREX" , 0xe0400000, 0xfff000f0, 3REG_IMM8_FORMAT }, // STREX<c> <Rd>,<Rt>,[<Rn>{,#<imm>}]\r
+ { "STREXB", 0xe8c00f40, 0xfff00ff0, 3REG_FORMAT }, // STREXB<c> <Rd>,<Rt>,[<Rn>]\r
+ { "STREXD", 0xe8c00070, 0xfff000f0, 4REG_FORMAT }, // STREXD<c> <Rd>,<Rt>,<Rt2>,[<Rn>]\r
+ { "STREXH", 0xe8c00f70, 0xfff00ff0, 3REG_FORMAT }, // STREXH<c> <Rd>,<Rt>,[<Rn>]\r
+ { "STRH", 0xf8c00000, 0xfff00000, 2REG_IMM8_FORMAT }, // STRH<c>.W <Rt>,[<Rn>{,#<imm12>}]\r
+ { "STRH", 0xf8200000, 0xfff00000, }, // STRH<c>.W <Rt>,[<Rn>,<Rm>{,LSL #<imm2>}]\r
+\r
+\r
+\r
+ // 1110 101x xxxx xxxx xxxx xxxx xxxx xxxx Data-processing\r
+ // 1110 11xx xxxx xxxx xxxx xxxx xxxx xxxx Coprocessor\r
+ \r
+ // 1111 0x0x xxxx xxxx 0xxx xxxx xxxx xxxx Data-processing modified immediate\r
+ // 1111 0x1x xxxx xxxx 0xxx xxxx xxxx xxxx Data-processing plain immediate\r
+ // 1111 0xxx xxxx xxxx 1xxx xxxx xxxx xxxx Branches\r
+ \r
+ // 1111 1000 xxx0 xxxx xxxx xxxx xxxx xxxx Store single data item\r
+ // 1111 1001 xxx0 xxxx xxxx xxxx xxxx xxxx SIMD or load/store\r
+ // 1111 100x x001 xxxx xxxx xxxx xxxx xxxx Load byte, memory hints \r
+ // 1111 100x x011 xxxx xxxx xxxx xxxx xxxx Load halfword, memory hints\r
+ // 1111 100x x101 xxxx xxxx xxxx xxxx xxxx Load word \r
+\r
+ // 1111 1 010 xxxx xxxx xxxx xxxx xxxx xxxx Data-processing register\r
+ // 1111 1 011 0xxx xxxx xxxx xxxx xxxx xxxx Multiply\r
+ // 1111 1 011 1xxx xxxx xxxx xxxx xxxx xxxx Long Multiply\r
+ // 1111 1 1xx xxxx xxxx xxxx xxxx xxxx xxxx Coprocessor \r
+};\r
+#endif\r
+\r
+CHAR8 mThumbMregListStr[4*15 + 1];\r
+\r
+CHAR8 *\r
+ThumbMRegList (\r
+ UINT32 OpCode\r
+ )\r
+{\r
+ UINTN Index, Start, End;\r
+ CHAR8 *Str;\r
+ BOOLEAN First;\r
+ \r
+ Str = mThumbMregListStr;\r
+ *Str = '\0';\r
+ AsciiStrCat (Str, "{");\r
+ // R0 - R7, PC\r
+ for (Index = 0, First = TRUE; Index <= 9; Index++) {\r
+ if ((OpCode & (1 << Index)) != 0) {\r
+ Start = End = Index;\r
+ for (Index++; ((OpCode & (1 << Index)) != 0) && (Index <= 9); Index++) {\r
+ End = Index;\r
+ }\r
+ \r
+ if (!First) {\r
+ AsciiStrCat (Str, ",");\r
+ } else {\r
+ First = FALSE;\r
+ }\r
+ \r
+ if (Start == End) {\r
+ AsciiStrCat (Str, gReg[(Start == 9)?15:Start]);\r
+ AsciiStrCat (Str, ", ");\r
+ } else {\r
+ AsciiStrCat (Str, gReg[Start]);\r
+ AsciiStrCat (Str, "-");\r
+ AsciiStrCat (Str, gReg[(End == 9)?15:End]);\r
+ }\r
+ }\r
+ }\r
+ if (First) {\r
+ AsciiStrCat (Str, "ERROR");\r
+ }\r
+ AsciiStrCat (Str, "}");\r
+ \r
+ // BugBug: Make caller pass in buffer it is cleaner\r
+ return mThumbMregListStr;\r
+}\r
+\r
+UINT32\r
+SignExtend (\r
+ IN UINT32 Data\r
+ )\r
+{\r
+ return 0;\r
+}\r
+\r
+/**\r
+ Place a dissasembly of of **OpCodePtr into buffer, and update OpCodePtr to \r
+ point to next instructin. \r
+ \r
+ We cheat and only decode instructions that access \r
+ memory. If the instruction is not found we dump the instruction in hex.\r
+ \r
+ @param OpCodePtrPtr Pointer to pointer of ARM Thumb instruction to disassemble. \r
+ @param Buf Buffer to sprintf disassembly into.\r
+ @param Size Size of Buf in bytes. \r
+ \r
+**/\r
+VOID\r
+DisassembleThumbInstruction (\r
+ IN UINT16 **OpCodePtrPtr,\r
+ OUT CHAR8 *Buf,\r
+ OUT UINTN Size\r
+ )\r
+{\r
+ UINT16 *OpCodePtr;\r
+ UINT16 OpCode;\r
+ UINT16 OpCode32;\r
+ UINT32 Index;\r
+ UINT32 Offset;\r
+ UINT16 Rd, Rn, Rm;\r
+ INT32 target_addr;\r
+ BOOLEAN H1, H2, imod;\r
+ UINT32 PC;\r
+\r
+ OpCodePtr = *OpCodePtrPtr;\r
+ OpCode = **OpCodePtrPtr;\r
+ \r
+ // Thumb2 is a stream of 16-bit instructions not a 32-bit instruction.\r
+ OpCode32 = (OpCode << 16) | *(OpCodePtr + 1);\r
+\r
+ // These register names match branch form, but not others\r
+ Rd = OpCode & 0x7;\r
+ Rn = (OpCode >> 3) & 0x7;\r
+ Rm = (OpCode >> 6) & 0x7;\r
+ H1 = (OpCode & BIT7) != 0;\r
+ H2 = (OpCode & BIT6) != 0;\r
+ imod = (OpCode & BIT4) != 0;\r
+ PC = (UINT32)(UINTN)*OpCodePtr;\r
+\r
+ // Increment by the minimum instruction size, Thumb2 could be bigger\r
+ *OpCodePtrPtr += 1;\r
+ \r
+ for (Index = 0; Index < sizeof (gOpThumb)/sizeof (THUMB_INSTRUCTIONS); Index++) {\r
+ if ((OpCode & gOpThumb[Index].Mask) == gOpThumb[Index].OpCode) {\r
+ Offset = AsciiSPrint (Buf, Size, "%a", gOpThumb[Index].Start); \r
+ switch (gOpThumb[Index].AddressMode) {\r
+ case LOAD_STORE_FORMAT1:\r
+ // A6.5.1 <Rd>, [<Rn>, #<5_bit_offset>]\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d #0x%x]", Rd, (OpCode >> 7) & 7, (OpCode >> 6) & 0x1f); \r
+ break;\r
+ case LOAD_STORE_FORMAT2:\r
+ // A6.5.1 <Rd>, [<Rn>, <Rm>]\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d, r%d]", Rd, (OpCode >> 3) & 7, Rm); \r
+ break;\r
+ case LOAD_STORE_FORMAT3:\r
+ // A6.5.1 <Rd>, [PC, #<8_bit_offset>]\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [pc, #0x%x]", (OpCode >> 8) & 7, OpCode & 0xff); \r
+ break;\r
+ case LOAD_STORE_FORMAT4:\r
+ // FIX ME!!!!!\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [sp, #0x%x]", (OpCode >> 8) & 7, OpCode & 0xff); \r
+ break;\r
+ \r
+ case LOAD_STORE_MULTIPLE_FORMAT1:\r
+ // <Rn>!, <registers> \r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d!, %a", (OpCode >> 8) & 7, ThumbMRegList (!BIT8 & OpCode)); \r
+ break;\r
+ case LOAD_STORE_MULTIPLE_FORMAT2:\r
+ // <Rn>!, <registers> \r
+ // BIT8 is PC \r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d!, %a", (OpCode >> 8) & 7, ThumbMRegList (OpCode)); \r
+ break;\r
+ \r
+ case IMMED_8:\r
+ // A6.7 <immed_8>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%x", OpCode & 0xff); \r
+ break;\r
+\r
+ case CONDITIONAL_BRANCH:\r
+ // A6.3.1 B<cond> <target_address>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, "%a 0x%04x", PC + 4 + SignExtend ((OpCode & 0xff) << 1)); \r
+ break;\r
+ case UNCONDITIONAL_BRANCH_SHORT:\r
+ // A6.3.2 B <target_address>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x", PC + 4 + SignExtend ((OpCode & 0x3ff) << 1)); \r
+ break;\r
+ case UNCONDITIONAL_BRANCH:\r
+ // A6.3.2 BL|BLX <target_address> ; Produces two 16-bit instructions \r
+ target_addr = *(OpCodePtr - 1);\r
+ if ((target_addr & 0xf800) == 0xf000) {\r
+ target_addr = ((target_addr & 0x3ff) << 12) | (OpCode & 0x3ff);\r
+ } else {\r
+ target_addr = OpCode & 0x3ff;\r
+ }\r
+ // PC + 2 +/- target_addr\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x", PC + 2 + SignExtend (target_addr)); \r
+ break;\r
+ case BRANCH_EXCHANGE:\r
+ // A6.3.3 BX|BLX <Rm>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d", gReg[Rn | (H2 ? 8:0)]); \r
+ break;\r
+\r
+ case DATA_FORMAT1:\r
+ // A6.4.3 <Rd>, <Rn>, <Rm>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, r%d", Rd, Rn, Rm); \r
+ break;\r
+ case DATA_FORMAT2:\r
+ // A6.4.3 <Rd>, <Rn>, #3_bit_immed\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", Rd, Rn, Rm); \r
+ break;\r
+ case DATA_FORMAT3:\r
+ // A6.4.3 <Rd>|<Rn>, #8_bit_immed\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", (OpCode >> 8) & 0x7, OpCode & 0xff); \r
+ break;\r
+ case DATA_FORMAT4:\r
+ // A6.4.3 <Rd>|<Rm>, #immed_5\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d, 0x%x", Rn, Rd, (OpCode >> 6) & 0x1f); \r
+ break;\r
+ case DATA_FORMAT5:\r
+ // A6.4.3 <Rd>|<Rm>, <Rm>|<Rs>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, r%d", Rd, Rn); \r
+ break;\r
+ case DATA_FORMAT6_SP:\r
+ // A6.4.3 <Rd>, <reg>, #<8_Bit_immed>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, sp, 0x%x", (OpCode >> 8) & 7, OpCode & 0xff); \r
+ break;\r
+ case DATA_FORMAT6_PC:\r
+ // A6.4.3 <Rd>, <reg>, #<8_Bit_immed>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, pc, 0x%x", (OpCode >> 8) & 7, OpCode & 0xff); \r
+ break;\r
+ case DATA_FORMAT7:\r
+ // A6.4.3 SP, SP, #<7_Bit_immed>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " sp, sp 0x%x", (OpCode & 0x7f)*4); \r
+ break;\r
+ case DATA_FORMAT8:\r
+ // A6.4.3 <Rd>|<Rn>, <Rm>\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " %a, %a", gReg[Rd | (H1 ? 8:0)], gReg[Rn | (H2 ? 8:0)]); \r
+ break;\r
+ \r
+ case CPS_FORMAT:\r
+ // A7.1.24\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, "%a %a%a%a", imod ? "ID":"IE", ((OpCode & BIT2) == 0) ? "":"a", ((OpCode & BIT1) == 0) ? "":"i", ((OpCode & BIT0) == 0) ? "":"f"); \r
+ break;\r
+\r
+ case ENDIAN_FORMAT:\r
+ // A7.1.24\r
+ AsciiSPrint (&Buf[Offset], Size - Offset, " %a", (OpCode & BIT3) == 0 ? "LE":"BE"); \r
+ break;\r
+ }\r
+ }\r
+ }\r
+#if 0 \r
+ // Thumb2 are 32-bit instructions\r
+ *OpCodePtrPtr += 1;\r
+ for (Index = 0; Index < sizeof (gOpThumb2)/sizeof (THUMB_INSTRUCTIONS); Index++) {\r
+ if ((OpCode32 & gOpThumb2[Index].Mask) == gOpThumb2[Index].OpCode) {\r
+ }\r
+ }\r
+#endif\r
+ // Unknown instruction is 16-bits\r
+ *OpCodePtrPtr -= 1;\r
+ AsciiSPrint (Buf, Size, "0x%04x", OpCode);\r
+}\r
+\r
+\r
+\r
+VOID\r
+DisassembleArmInstruction (\r
+ IN UINT32 **OpCodePtr,\r
+ OUT CHAR8 *Buf,\r
+ OUT UINTN Size\r
+ );\r
+\r
+\r
+/**\r
+ Place a dissasembly of of **OpCodePtr into buffer, and update OpCodePtr to \r
+ point to next instructin. \r
+ \r
+ We cheat and only decode instructions that access \r
+ memory. If the instruction is not found we dump the instruction in hex.\r
+ \r
+ @param OpCodePtrPtr Pointer to pointer of ARM Thumb instruction to disassemble. \r
+ @param Thumb TRUE for Thumb(2), FALSE for ARM instruction stream\r
+ @param Buf Buffer to sprintf disassembly into.\r
+ @param Size Size of Buf in bytes. \r
+ \r
+**/\r
+VOID\r
+DisassembleInstruction (\r
+ IN UINT8 **OpCodePtr,\r
+ IN BOOLEAN Thumb,\r
+ OUT CHAR8 *Buf,\r
+ OUT UINTN Size\r
+ )\r
+{\r
+ if (Thumb) {\r
+ DisassembleThumbInstruction ((UINT16 **)OpCodePtr, Buf, Size);\r
+ } else {\r
+ DisassembleArmInstruction ((UINT32 **)OpCodePtr, Buf, Size);\r
+ }\r
+}\r
+ \r