Fix bug in UncachedMemoryAllocationLib, Assembler, make DefaultExceptionHandler lib...

[mirror_edk2.git] / ArmPkg / Library / DefaultExceptionHandlerLib / DefaultExceptionHandler.c

diff --git a/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
index a0f327d654dfdb07d68060633256cf33400b583a..927a66b915ab76ac3ad421992261ffc91166599c 100644 (file)
--- a/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
+++ b/ArmPkg/Library/DefaultExceptionHandlerLib/DefaultExceptionHandler.c
@@ -224,7 +224,9 @@ DefaultExceptionHandler (
   )\r
 {\r
   UINT32    DfsrStatus;\r
+  UINT32    IfsrStatus;\r
   BOOLEAN   DfsrWrite;\r
+  UINT32    PcAdjust = 0;\r
 \r
   DEBUG ((EFI_D_ERROR, "\n%a Exception PC at 0x%08x  CPSR 0x%08x ", gExceptionTypeString[ExceptionType], SystemContext.SystemContextArm->PC, SystemContext.SystemContextArm->CPSR));\r
   DEBUG_CODE_BEGIN ();\r
@@ -235,6 +237,7 @@ DefaultExceptionHandler (
     CHAR8   CpsrStr[32];  // char per bit. Lower 5-bits are mode that is a 3 char string\r
     CHAR8   Buffer[80];\r
     UINT8   *DisAsm;\r
+    UINT32  ItBlock;\r
     \r
     CpsrString (SystemContext.SystemContextArm->CPSR, CpsrStr);\r
     DEBUG ((EFI_D_ERROR, "%a\n", CpsrStr));\r
@@ -256,8 +259,22 @@ DefaultExceptionHandler (
       \r
       // If we come from an image it is safe to show the instruction. We know it should not fault\r
       DisAsm = (UINT8 *)(UINTN)SystemContext.SystemContextArm->PC;\r
-      DisassembleInstruction (&DisAsm, (SystemContext.SystemContextArm->CPSR & BIT5) == BIT5, TRUE, Buffer, sizeof (Buffer));\r
+      ItBlock = 0;\r
+      DisassembleInstruction (&DisAsm, (SystemContext.SystemContextArm->CPSR & BIT5) == BIT5, TRUE, &ItBlock, Buffer, sizeof (Buffer));\r
       DEBUG ((EFI_D_ERROR, "\n%a", Buffer));\r
+      \r
+      switch (ExceptionType) {\r
+      case EXCEPT_ARM_UNDEFINED_INSTRUCTION:\r
+      case EXCEPT_ARM_SOFTWARE_INTERRUPT:\r
+      case EXCEPT_ARM_PREFETCH_ABORT:\r
+      case EXCEPT_ARM_DATA_ABORT:\r
+        // advance PC past the faulting instruction\r
+        PcAdjust = (UINTN)DisAsm - SystemContext.SystemContextArm->PC;\r
+        break;\r
+      \r
+      default:\r
+        break;\r
+      }\r
 \r
     }\r
   DEBUG_CODE_END ();\r
@@ -273,12 +290,17 @@ DefaultExceptionHandler (
   if (DfsrStatus != 0x00) {\r
     DEBUG ((EFI_D_ERROR, " %a: %a 0x%08x\n", FaultStatusToString (DfsrStatus), DfsrWrite ? "write to" : "read from", SystemContext.SystemContextArm->DFAR));\r
   }\r
-  if ((SystemContext.SystemContextArm->IFSR & 0xf) != 0x00) {\r
-    DEBUG ((EFI_D_ERROR, "Instruction %a at 0x%08x, \n", FaultStatusToString (SystemContext.SystemContextArm->IFSR & 0xf), SystemContext.SystemContextArm->IFAR));\r
+\r
+  IfsrStatus = (SystemContext.SystemContextArm->IFSR & 0xf) | ((SystemContext.SystemContextArm->IFSR >> 6) & 0x10);\r
+  if (IfsrStatus != 0) {\r
+    DEBUG ((EFI_D_ERROR, " Instruction %a at 0x%08x\n", FaultStatusToString (SystemContext.SystemContextArm->IFSR & 0xf), SystemContext.SystemContextArm->IFAR));\r
   }\r
 \r
   DEBUG ((EFI_D_ERROR, "\n"));\r
   ASSERT (FALSE);\r
+  \r
+  // If some one is stepping past the exception handler adjust the PC to point to the next instruction \r
+  SystemContext.SystemContextArm->PC += PcAdjust;\r
 }\r
 \r
 \r