Fix bug

[mirror_edk2.git] / ArmPkg / Drivers / CpuDxe / Exception.c

diff --git a/ArmPkg/Drivers/CpuDxe/Exception.c b/ArmPkg/Drivers/CpuDxe/Exception.c
index 8859ca8667477fac4f38ba0a9bdf5446fdc89b63..f795c431a2dd8109edeabcb4a86e82f96f26080a 100644 (file)
--- a/ArmPkg/Drivers/CpuDxe/Exception.c
+++ b/ArmPkg/Drivers/CpuDxe/Exception.c
@@ -13,9 +13,7 @@
 **/\r
 \r
 #include "CpuDxe.h" \r
-#include <Library/CacheMaintenanceLib.h>\r
 \r
-extern BOOLEAN gExceptionContext;\r
 \r
 VOID\r
 ExceptionHandlersStart (\r
@@ -122,16 +120,7 @@ RegisterDebuggerInterruptHandler (
   return EFI_SUCCESS;\r
 }\r
 \r
-CHAR8 *gExceptionTypeString[] = {\r
-  "Reset",\r
-  "Undefined Instruction",\r
-  "SWI",\r
-  "Prefetch Abort",\r
-  "Data Abort",\r
-  "Undefined",\r
-  "IRQ",\r
-  "FIQ"\r
-};\r
+\r
 \r
 VOID\r
 EFIAPI\r
@@ -142,8 +131,7 @@ CommonCExceptionHandler (
 {\r
   BOOLEAN Dispatched = FALSE;\r
  \r
-  gExceptionContext = TRUE; \r
-  \r
+ \r
   if (ExceptionType <= MAX_ARM_EXCEPTION) {\r
     if (gDebuggerExceptionHandlers[ExceptionType]) {\r
       //\r
@@ -162,8 +150,6 @@ CommonCExceptionHandler (
     ASSERT (FALSE);\r
   }\r
 \r
-  gExceptionContext = FALSE; \r
-\r
   if (Dispatched) {\r
     //\r
     // We did work so this was an expected ExceptionType\r
@@ -178,12 +164,7 @@ CommonCExceptionHandler (
     return;\r
   }\r
 \r
-  //\r
-  // Code after here is the default exception handler...\r
-  //\r
-  DEBUG ((EFI_D_ERROR, "%a Exception from %08x\n", gExceptionTypeString[ExceptionType], SystemContext.SystemContextArm->PC));\r
-  ASSERT (FALSE);\r
-\r
+  DefaultExceptionHandler (ExceptionType, SystemContext);\r
 }\r
 \r
 \r
@@ -197,26 +178,24 @@ InitializeExceptions (
   UINTN                Offset;\r
   UINTN                Length;\r
   UINTN                Index;\r
-  BOOLEAN              Enabled;\r
+  BOOLEAN              IrqEnabled;\r
+  BOOLEAN              FiqEnabled;\r
   EFI_PHYSICAL_ADDRESS Base;\r
+  UINT32               *VectorBase;\r
 \r
   //\r
   // Disable interrupts\r
   //\r
-  Cpu->GetInterruptState (Cpu, &Enabled);\r
+  Cpu->GetInterruptState (Cpu, &IrqEnabled);\r
   Cpu->DisableInterrupt (Cpu);\r
 \r
   //\r
-  // Initialize the C entry points for interrupts\r
+  // EFI does not use the FIQ, but a debugger might so we must disable \r
+  // as we take over the exception vectors. \r
   //\r
-  for (Index = 0; Index <= MAX_ARM_EXCEPTION; Index++) {\r
-    Status = RegisterInterruptHandler (Index, NULL);\r
-    ASSERT_EFI_ERROR (Status);\r
-    \r
-    Status = RegisterDebuggerInterruptHandler (Index, NULL);\r
-    ASSERT_EFI_ERROR (Status);\r
-  }\r
-  \r
+  FiqEnabled = ArmGetFiqState ();\r
+  ArmDisableFiq ();\r
+\r
   //\r
   // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.\r
   //\r
@@ -226,6 +205,7 @@ InitializeExceptions (
   // Reserve space for the exception handlers\r
   //\r
   Base = (EFI_PHYSICAL_ADDRESS)PcdGet32 (PcdCpuVectorBaseAddress);\r
+  VectorBase = (UINT32 *)(UINTN)Base;\r
   Status = gBS->AllocatePages (AllocateAddress, EfiBootServicesCode, EFI_SIZE_TO_PAGES (Length), &Base);\r
   // If the request was for memory that's not in the memory map (which is often the case for 0x00000000\r
   // on embedded systems, for example, we don't want to hang up.  So we'll check here for a status of \r
@@ -234,7 +214,25 @@ InitializeExceptions (
     ASSERT_EFI_ERROR (Status);\r
   }\r
 \r
-  CopyMem ((VOID *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress), (VOID *)ExceptionHandlersStart, Length);\r
+  // Save existing vector table, in case debugger is already hooked in\r
+  CopyMem ((VOID *)gDebuggerExceptionHandlers, (VOID *)VectorBase, sizeof (gDebuggerExceptionHandlers));\r
+\r
+  //\r
+  // Initialize the C entry points for interrupts\r
+  //\r
+  for (Index = 0; Index <= MAX_ARM_EXCEPTION; Index++) {\r
+    Status = RegisterInterruptHandler (Index, NULL);\r
+    ASSERT_EFI_ERROR (Status);\r
+    \r
+    if (VectorBase[Index] == 0xEAFFFFFE) {\r
+      // Exception handler contains branch to vector location (jmp $) so no handler\r
+      // NOTE: This code assumes vectors are ARM and not Thumb code\r
+      gDebuggerExceptionHandlers[Index] = NULL;\r
+    }\r
+  }\r
+\r
+  // Copy our assembly code into the page that contains the exception vectors. \r
+  CopyMem ((VOID *)VectorBase, (VOID *)ExceptionHandlersStart, Length);\r
 \r
   //\r
   // Patch in the common Assembly exception handler\r
@@ -245,7 +243,11 @@ InitializeExceptions (
   // Flush Caches since we updated executable stuff\r
   InvalidateInstructionCacheRange ((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);\r
 \r
-  if (Enabled) {\r
+  if (FiqEnabled) {\r
+    ArmEnableFiq ();\r
+  }\r
+\r
+  if (IrqEnabled) {\r
     // \r
     // Restore interrupt state\r
     //\r