From 41d478023f9fcc9a27bfc006cbc16b94d85ff98c Mon Sep 17 00:00:00 2001
From: andrewfish <andrewfish@6f19259b-4bc3-4df7-8a09-765794883524>
Date: Fri, 5 Mar 2010 00:57:07 +0000
Subject: [PATCH] Make these drivers more compatible with a projected Debug
 Agent Library implementation.

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@10196 6f19259b-4bc3-4df7-8a09-765794883524
---
 ArmPkg/Drivers/CpuDxe/Exception.c            | 32 +++++++++++++-------
 Omap35xxPkg/InterruptDxe/HardwareInterrupt.c |  2 +-
 2 files changed, 22 insertions(+), 12 deletions(-)

diff --git a/ArmPkg/Drivers/CpuDxe/Exception.c b/ArmPkg/Drivers/CpuDxe/Exception.c
index e313cf3fe9..d99d3d1d01 100644
--- a/ArmPkg/Drivers/CpuDxe/Exception.c
+++ b/ArmPkg/Drivers/CpuDxe/Exception.c
@@ -180,6 +180,7 @@ InitializeExceptions (
   UINTN                Index;
   BOOLEAN              Enabled;
   EFI_PHYSICAL_ADDRESS Base;
+  UINT32               *VectorBase;
 
   //
   // Disable interrupts
@@ -187,16 +188,6 @@ InitializeExceptions (
   Cpu->GetInterruptState (Cpu, &Enabled);
   Cpu->DisableInterrupt (Cpu);
 
-  //
-  // Initialize the C entry points for interrupts
-  //
-  for (Index = 0; Index <= MAX_ARM_EXCEPTION; Index++) {
-    Status = RegisterInterruptHandler (Index, NULL);
-    ASSERT_EFI_ERROR (Status);
-    
-    Status = RegisterDebuggerInterruptHandler (Index, NULL);
-    ASSERT_EFI_ERROR (Status);
-  }
   
   //
   // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.
@@ -207,6 +198,7 @@ InitializeExceptions (
   // Reserve space for the exception handlers
   //
   Base = (EFI_PHYSICAL_ADDRESS)PcdGet32 (PcdCpuVectorBaseAddress);
+  VectorBase = (UINT32 *)(UINTN)Base;
   Status = gBS->AllocatePages (AllocateAddress, EfiBootServicesCode, EFI_SIZE_TO_PAGES (Length), &Base);
   // If the request was for memory that's not in the memory map (which is often the case for 0x00000000
   // on embedded systems, for example, we don't want to hang up.  So we'll check here for a status of 
@@ -215,7 +207,25 @@ InitializeExceptions (
     ASSERT_EFI_ERROR (Status);
   }
 
-  CopyMem ((VOID *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress), (VOID *)ExceptionHandlersStart, Length);
+  // Save existing vector table, in case debugger is already hooked in
+  CopyMem ((VOID *)gDebuggerExceptionHandlers, (VOID *)VectorBase, sizeof (gDebuggerExceptionHandlers));
+
+  //
+  // Initialize the C entry points for interrupts
+  //
+  for (Index = 0; Index <= MAX_ARM_EXCEPTION; Index++) {
+    Status = RegisterInterruptHandler (Index, NULL);
+    ASSERT_EFI_ERROR (Status);
+    
+    if (VectorBase[Index] == 0xEAFFFFFE) {
+      // Exception handler contains branch to vector location (jmp $) so no handler
+      // NOTE: This code assumes vectors are ARM and not Thumb code
+      gDebuggerExceptionHandlers[Index] = NULL;
+    }
+  }
+
+  // Copy our assembly code into the page that contains the exception vectors. 
+  CopyMem ((VOID *)VectorBase, (VOID *)ExceptionHandlersStart, Length);
 
   //
   // Patch in the common Assembly exception handler
diff --git a/Omap35xxPkg/InterruptDxe/HardwareInterrupt.c b/Omap35xxPkg/InterruptDxe/HardwareInterrupt.c
index eae92067d6..55e1373615 100644
--- a/Omap35xxPkg/InterruptDxe/HardwareInterrupt.c
+++ b/Omap35xxPkg/InterruptDxe/HardwareInterrupt.c
@@ -345,7 +345,7 @@ InterruptDxeInitialize (
   MmioWrite32 (INTCPS_MIR(0), 0xFFFFFFFF);
   MmioWrite32 (INTCPS_MIR(1), 0xFFFFFFFF);
   MmioWrite32 (INTCPS_MIR(2), 0xFFFFFFFF);
-  MmioWrite32 (INTCPS_CONTROL, INTCPS_CONTROL_NEWIRQAGR);
+  MmioOr32 (INTCPS_CONTROL, INTCPS_CONTROL_NEWIRQAGR);
  
   Status = gBS->InstallMultipleProtocolInterfaces(&gHardwareInterruptHandle,
                                                   &gHardwareInterruptProtocolGuid,   &gHardwareInterruptProtocol,