More progress on the disassebler lib
authorandrewfish <andrewfish@6f19259b-4bc3-4df7-8a09-765794883524>
Tue, 2 Feb 2010 04:37:34 +0000 (04:37 +0000)
committerandrewfish <andrewfish@6f19259b-4bc3-4df7-8a09-765794883524>
Tue, 2 Feb 2010 04:37:34 +0000 (04:37 +0000)
git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@9909 6f19259b-4bc3-4df7-8a09-765794883524

ArmPkg/Library/ArmDisassemblerLib/ThumbDisassembler.c

index 25cb8ae4708f2916cf71894ce12752cdb831dbec..02958c85c209cb99b8c5602284d7daa6389ad444 100644 (file)
@@ -17,6 +17,8 @@
 #include <Library/BaseLib.h>\r
 #include <Library/PrintLib.h>\r
 \r
+extern CHAR8 *gCondition[];\r
+\r
 extern CHAR8 *gReg[];\r
 \r
 #define LOAD_STORE_FORMAT1            1\r
@@ -24,7 +26,8 @@ extern CHAR8 *gReg[];
 #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 PUSH_FORMAT                   6 \r
+#define POP_FORMAT                  106 \r
 #define IMMED_8                       7\r
 #define CONDITIONAL_BRANCH            8\r
 #define UNCONDITIONAL_BRANCH          9\r
@@ -42,6 +45,10 @@ extern CHAR8 *gReg[];
 #define CPS_FORMAT                   20\r
 #define ENDIAN_FORMAT                21\r
      \r
+#define B_T3                        200\r
+#define B_T4                        201\r
+#define BL_T2                       202\r
+\r
 \r
 typedef struct {\r
   CHAR8   *Start;\r
@@ -60,8 +67,8 @@ THUMB_INSTRUCTIONS gOpThumb[] = {
   { "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
+  { "ADD" , 0xa800, 0xf800, DATA_FORMAT6_SP }, \r
+  { "ADD" , 0xb000, 0xff80, DATA_FORMAT7 },\r
 \r
   { "AND" , 0x4000, 0xffc0, DATA_FORMAT5 },\r
 \r
@@ -69,22 +76,20 @@ THUMB_INSTRUCTIONS gOpThumb[] = {
   { "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
+  { "B"   , 0xe000, 0xf800, UNCONDITIONAL_BRANCH_SHORT },\r
   { "BLX" , 0x4780, 0xff80, BRANCH_EXCHANGE },\r
-  { "BX"  , 0x4700, 0xff80, BRANCH_EXCHANGE },\r
+  { "BX"  , 0x4700, 0xff87, 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" , 0x2800, 0xf800, 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
+  { "MOV" , 0x4600, 0xff00, DATA_FORMAT8 },\r
   { "EOR" , 0x4040, 0xffc0, DATA_FORMAT5 },\r
 \r
   { "LDMIA" , 0xc800, 0xf800, LOAD_STORE_MULTIPLE_FORMAT1 },\r
@@ -99,12 +104,13 @@ THUMB_INSTRUCTIONS gOpThumb[] = {
   { "LDRSB" , 0x5600, 0xfe00, LOAD_STORE_FORMAT2 },\r
   { "LDRSH" , 0x5e00, 0xfe00, LOAD_STORE_FORMAT2 },\r
  \r
+  { "MOVS", 0x0000, 0xffc0, DATA_FORMAT5 },   // LSL with imm5 == 0 is a MOVS, so this must go before LSL\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
+  { "MOVS", 0x2000, 0xf800, DATA_FORMAT3 },\r
   { "MOV" , 0x1c00, 0xffc0, DATA_FORMAT3 },\r
   { "MOV" , 0x4600, 0xff00, DATA_FORMAT8 },\r
 \r
@@ -112,16 +118,16 @@ THUMB_INSTRUCTIONS gOpThumb[] = {
   { "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
+  { "POP" , 0xbc00, 0xfe00, POP_FORMAT },\r
+  { "PUSH", 0xb400, 0xfe00, PUSH_FORMAT },\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
+  { "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
@@ -146,9 +152,13 @@ THUMB_INSTRUCTIONS gOpThumb[] = {
   { "UXTH", 0xb280, 0xffc0, DATA_FORMAT5 }\r
 };\r
 \r
-#if 0  \r
 THUMB_INSTRUCTIONS gOpThumb2[] = {\r
-  ,\r
+  { "B",    0xf0008000, 0xf800d000, B_T3  },\r
+  { "B",    0xf0009000, 0xf800d000, B_T4  },\r
+  { "BL",   0xf000d000, 0xf800d000, B_T4  },\r
+  { "BLX",  0xf000c000, 0xf800d000, BL_T2 }\r
+  \r
+#if 0  \r
   \r
   // 32-bit Thumb instructions  op1 01\r
   \r
@@ -195,14 +205,14 @@ THUMB_INSTRUCTIONS gOpThumb2[] = {
   //  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
 \r
 CHAR8 mThumbMregListStr[4*15 + 1];\r
 \r
 CHAR8 *\r
 ThumbMRegList (\r
-  UINT32  OpCode\r
+  UINT32  RegBitMask\r
   )\r
 {\r
   UINTN     Index, Start, End;\r
@@ -213,10 +223,10 @@ ThumbMRegList (
   *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
+  for (Index = 0, First = TRUE; Index <= 15; Index++) {\r
+    if ((RegBitMask & (1 << Index)) != 0) {\r
       Start = End = Index;\r
-      for (Index++; ((OpCode & (1 << Index)) != 0) && (Index <= 9); Index++) {\r
+      for (Index++; ((RegBitMask & (1 << Index)) != 0) && (Index <= 9); Index++) {\r
         End = Index;\r
       }\r
       \r
@@ -227,12 +237,11 @@ ThumbMRegList (
       }\r
       \r
       if (Start == End) {\r
-        AsciiStrCat  (Str, gReg[(Start == 9)?15:Start]);\r
-        AsciiStrCat  (Str, ", ");\r
+        AsciiStrCat  (Str, gReg[Start]);\r
       } else {\r
         AsciiStrCat  (Str, gReg[Start]);\r
         AsciiStrCat  (Str, "-");\r
-        AsciiStrCat  (Str, gReg[(End == 9)?15:End]);\r
+        AsciiStrCat  (Str, gReg[End]);\r
       }\r
     }\r
   }\r
@@ -246,11 +255,21 @@ ThumbMRegList (
 }\r
 \r
 UINT32\r
-SignExtend (\r
-  IN  UINT32  Data\r
+SignExtend32 (\r
+  IN  UINT32  Data,\r
+  IN  UINT32  TopBit\r
   )\r
 {\r
-  return 0;\r
+  if (((Data & TopBit) == 0) || (TopBit == BIT31)) {\r
+    return Data;\r
+  }\r
\r
+  do {\r
+    TopBit <<= 1;\r
+    Data |= TopBit; \r
+  } while ((TopBit & BIT31) != BIT31);\r
+\r
+  return Data;\r
 }\r
 \r
 /**\r
@@ -276,19 +295,20 @@ DisassembleThumbInstruction (
 {\r
   UINT16  *OpCodePtr;\r
   UINT16  OpCode;\r
-  UINT16  OpCode32;\r
+  UINT32  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
+  UINT32  PC, Target;\r
+  CHAR8   *Cond;\r
+  BOOLEAN S, J1, J2;\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
+  OpCode32 = (((UINT32)OpCode) << 16) | *(OpCodePtr + 1);\r
 \r
   // These register names match branch form, but not others\r
   Rd = OpCode & 0x7;\r
@@ -297,7 +317,7 @@ DisassembleThumbInstruction (
   H1 = (OpCode & BIT7) != 0;\r
   H2 = (OpCode & BIT6) != 0;\r
   imod = (OpCode & BIT4) != 0;\r
-  PC = (UINT32)(UINTN)*OpCodePtr;\r
+  PC = (UINT32)(UINTN)OpCodePtr;\r
 \r
   // Increment by the minimum instruction size, Thumb2 could be bigger\r
   *OpCodePtrPtr += 1;\r
@@ -305,136 +325,172 @@ DisassembleThumbInstruction (
   for (Index = 0; Index < sizeof (gOpThumb)/sizeof (THUMB_INSTRUCTIONS); Index++) {\r
     if ((OpCode & gOpThumb[Index].Mask) == gOpThumb[Index].OpCode) {\r
       if (Extended) {\r
-        Offset = AsciiSPrint (Buf, Size, "0x%04x       %a", OpCode, gOpThumb[Index].Start);   \r
+        Offset = AsciiSPrint (Buf, Size, "0x%04x       %-6a", OpCode, gOpThumb[Index].Start);   \r
       } else {\r
-        Offset = AsciiSPrint (Buf, Size, "%a", gOpThumb[Index].Start);   \r
+        Offset = AsciiSPrint (Buf, Size, "%-6a", gOpThumb[Index].Start);   \r
       }\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d #0x%x]", Rd, Rn, (OpCode >> 4) & 0x7c);   \r
+        return;\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [r%d, r%d]", Rd, Rn, Rm);   \r
+        return;\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
+        Target = (OpCode & 0xff) << 2;\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [pc, #0x%x] ;0x%08x", (OpCode >> 8) & 7, Target, PC + 4 + Target);   \r
+        return;\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
+        // Rt, [SP, #imm8]\r
+        Target = (OpCode & 0xff) << 2;\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, [sp, #0x%x]", (OpCode >> 8) & 7, Target, PC + 3 + Target);   \r
+        return;\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
+        // <Rn>!, {r0-r7}\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d!, %a", (OpCode >> 8) & 7, ThumbMRegList (OpCode & 0xff));   \r
+        return;\r
\r
+      case POP_FORMAT:\r
+        // POP {r0-r7,pc}\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " %a", ThumbMRegList ((OpCode & 0xff) | ((OpCode & BIT8) == BIT8 ? BIT15 : 0)));   \r
+        return;\r
+\r
+      case PUSH_FORMAT:\r
+        // PUSH {r0-r7,lr}\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " %a", ThumbMRegList ((OpCode & 0xff) | ((OpCode & BIT8) == BIT8 ? BIT14 : 0)));   \r
+        return;\r
+\r
       \r
       case IMMED_8:\r
         // A6.7 <immed_8>\r
         AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%x", OpCode & 0xff);   \r
-        break;\r
+        return;\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
+        // Patch in the condition code. A little hack but based on "%-6a"\r
+        Cond = gCondition[(OpCode >> 8) & 0xf];\r
+        Buf[Offset-5] = *Cond++;\r
+        Buf[Offset-4] = *Cond;\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x",  PC + 4 + SignExtend32 ((OpCode & 0xff) << 1, BIT8));   \r
+        return;\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%04x", PC + 4 + SignExtend32 ((OpCode & 0x3ff) << 1, BIT11));   \r
+        return;\r
\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " %a", gReg[Rn | (H2 ? 8:0)]);   \r
+        return;\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
+        return;\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
+        return;\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
+        // A6.4.3  <Rd>|<Rn>, #imm8\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, #0x%x", (OpCode >> 8) & 7, OpCode & 0xff);   \r
+        return;\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
+        return;\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
+        return;\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, sp, 0x%x", (OpCode >> 8) & 7, (OpCode & 0xff) << 2);   \r
+        return;\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " r%d, pc, 0x%x", (OpCode >> 8) & 7, (OpCode & 0xff) << 2);   \r
+        return;\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
+        AsciiSPrint (&Buf[Offset], Size - Offset, " sp, sp, 0x%x", (OpCode & 0x7f)*4);   \r
+        return;\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
+        return;\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
+        return;\r
 \r
       case ENDIAN_FORMAT:\r
         // A7.1.24\r
         AsciiSPrint (&Buf[Offset], Size - Offset, " %a", (OpCode & BIT3) == 0 ? "LE":"BE");   \r
-        break;\r
+        return;\r
       }\r
     }\r
   }\r
-#if 0  \r
+\r
+  \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
       if (Extended) {\r
-        Offset = AsciiSPrint (Buf, Size, "0x%04x   %a", OpCode32, gOpThumb2[Index].Start);   \r
+        Offset = AsciiSPrint (Buf, Size, "0x%04x       %-6a", OpCode32, gOpThumb2[Index].Start);   \r
       } else {\r
-        Offset = AsciiSPrint (Buf, Size, "%a", gOpThumb2[Index].Start);   \r
+        Offset = AsciiSPrint (Buf, Size, "       %-6a", gOpThumb2[Index].Start);   \r
       }\r
       switch (gOpThumb2[Index].AddressMode) {\r
+      case B_T3:\r
+        Cond = gCondition[(OpCode32 >> 22) & 0xf];\r
+        Buf[Offset-5] = *Cond++;\r
+        Buf[Offset-4] = *Cond;\r
+        // S:J2:J1:imm6:imm11:0\r
+        Target = ((OpCode32 << 1) & 0xffe) + ((OpCode32 >> 4) & 0x3f000);\r
+        Target |= (OpCode & BIT11) ? BIT18 : 0;  // J2\r
+        Target |= (OpCode & BIT13) ? BIT17 : 0;  // J1\r
+        Target |= (OpCode & BIT26) ? BIT19 : 0;  // S\r
+        Target = SignExtend32 (Target, BIT19);\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%08x", Target);   \r
+        return;\r
+      case B_T4:\r
+        // S:I1:I2:imm10:imm11:0\r
+        Target = ((OpCode32 << 1) & 0xffe) + ((OpCode32 >> 4) & 0x3ff000);\r
+        S  = (OpCode & BIT26);\r
+        J1 = (OpCode & BIT13);\r
+        J2 = (OpCode & BIT11);\r
+        Target |= !(J2 ^ S) ? BIT21 : 0;  // I2\r
+        Target |= !(J1 ^ S) ? BIT22 : 0;  // I1\r
+        Target |= (OpCode & BIT26) ? BIT23 : 0;  // S\r
+        Target = SignExtend32 (Target, BIT23);\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%08x", Target);   \r
+        return;\r
+\r
+      case BL_T2:\r
+        // S:I1:I2:imm10:imm11:0\r
+        Target = ((OpCode32 << 2) & 0x1ffc) + ((OpCode32 >> 3) & 0x7fe000);\r
+        S  = (OpCode & BIT26);\r
+        J1 = (OpCode & BIT13);\r
+        J2 = (OpCode & BIT11);\r
+        Target |= !(J2 ^ S) ? BIT22 : 0;  // I2\r
+        Target |= !(J1 ^ S) ? BIT23 : 0;  // I1\r
+        Target |= (OpCode & BIT26) ? BIT24 : 0;  // S\r
+        Target = SignExtend32 (Target, BIT24);\r
+        AsciiSPrint (&Buf[Offset], Size - Offset, " 0x%08x", Target);   \r
+        return;\r
       }\r
     }\r
   }\r
-#endif\r
-  // Unknown instruction is 16-bits\r
-  *OpCodePtrPtr -= 1;\r
-  if (!Extended) {\r
-    AsciiSPrint (Buf, Size, "0x%04x", OpCode);\r
-  }\r
+\r
+  AsciiSPrint (Buf, Size, "0x%08x", OpCode32);\r
 }\r
 \r
 \r