1 files changed, 153 insertions, 0 deletions
diff --git a/ArmPkg/Drivers/CpuDxe/AArch64/Exception.c b/ArmPkg/Drivers/CpuDxe/AArch64/Exception.c
new file mode 100644
index 0000000000..1aac6a5a91
--- /dev/null
+++ b/ArmPkg/Drivers/CpuDxe/AArch64/Exception.c
@@ -0,0 +1,153 @@
+/** @file
+
+  Copyright (c) 2008 - 2009, Apple Inc. All rights reserved.<BR>
+  Portions Copyright (c) 2011 - 2013, ARM Ltd. All rights reserved.<BR>
+
+  This program and the accompanying materials
+  are licensed and made available under the terms and conditions of the BSD License
+  which accompanies this distribution.  The full text of the license may be found at
+  http://opensource.org/licenses/bsd-license.php
+
+  THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
+  WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
+
+**/
+
+#include "CpuDxe.h"
+
+#include <Chipset/AArch64.h>
+
+VOID
+ExceptionHandlersStart (
+  VOID
+  );
+
+VOID
+ExceptionHandlersEnd (
+  VOID
+  );
+
+VOID
+CommonExceptionEntry (
+  VOID
+  );
+
+VOID
+AsmCommonExceptionEntry (
+  VOID
+  );
+
+
+EFI_EXCEPTION_CALLBACK  gExceptionHandlers[MAX_ARM_EXCEPTION + 1];
+EFI_EXCEPTION_CALLBACK  gDebuggerExceptionHandlers[MAX_ARM_EXCEPTION + 1];
+
+
+
+/**
+  This function registers and enables the handler specified by InterruptHandler for a processor
+  interrupt or exception type specified by InterruptType. If InterruptHandler is NULL, then the
+  handler for the processor interrupt or exception type specified by InterruptType is uninstalled.
+  The installed handler is called once for each processor interrupt or exception.
+
+  @param  InterruptType    A pointer to the processor's current interrupt state. Set to TRUE if interrupts
+                           are enabled and FALSE if interrupts are disabled.
+  @param  InterruptHandler A pointer to a function of type EFI_CPU_INTERRUPT_HANDLER that is called
+                           when a processor interrupt occurs. If this parameter is NULL, then the handler
+                           will be uninstalled.
+
+  @retval EFI_SUCCESS           The handler for the processor interrupt was successfully installed or uninstalled.
+  @retval EFI_ALREADY_STARTED   InterruptHandler is not NULL, and a handler for InterruptType was
+                                previously installed.
+  @retval EFI_INVALID_PARAMETER InterruptHandler is NULL, and a handler for InterruptType was not
+                                previously installed.
+  @retval EFI_UNSUPPORTED       The interrupt specified by InterruptType is not supported.
+
+**/
+EFI_STATUS
+RegisterInterruptHandler (
+  IN EFI_EXCEPTION_TYPE             InterruptType,
+  IN EFI_CPU_INTERRUPT_HANDLER      InterruptHandler
+  )
+{
+  if (InterruptType > MAX_ARM_EXCEPTION) {
+    return EFI_UNSUPPORTED;
+  }
+
+  if ((InterruptHandler != NULL) && (gExceptionHandlers[InterruptType] != NULL)) {
+    return EFI_ALREADY_STARTED;
+  }
+
+  gExceptionHandlers[InterruptType] = InterruptHandler;
+
+  return EFI_SUCCESS;
+}
+
+
+
+VOID
+EFIAPI
+CommonCExceptionHandler (
+  IN     EFI_EXCEPTION_TYPE           ExceptionType,
+  IN OUT EFI_SYSTEM_CONTEXT           SystemContext
+  )
+{
+  if (ExceptionType <= MAX_AARCH64_EXCEPTION) {
+    if (gExceptionHandlers[ExceptionType]) {
+      gExceptionHandlers[ExceptionType] (ExceptionType, SystemContext);
+      return;
+    }
+  } else {
+    DEBUG ((EFI_D_ERROR, "Unknown exception type %d from %016lx\n", ExceptionType, SystemContext.SystemContextAArch64->ELR));
+    ASSERT (FALSE);
+  }
+
+  DefaultExceptionHandler (ExceptionType, SystemContext);
+}
+
+
+
+EFI_STATUS
+InitializeExceptions (
+  IN EFI_CPU_ARCH_PROTOCOL    *Cpu
+  )
+{
+  EFI_STATUS           Status;
+  BOOLEAN              IrqEnabled;
+  BOOLEAN              FiqEnabled;
+
+  Status = EFI_SUCCESS;
+  ZeroMem (gExceptionHandlers,sizeof(*gExceptionHandlers));
+
+  //
+  // Disable interrupts
+  //
+  Cpu->GetInterruptState (Cpu, &IrqEnabled);
+  Cpu->DisableInterrupt (Cpu);
+
+  //
+  // EFI does not use the FIQ, but a debugger might so we must disable
+  // as we take over the exception vectors.
+  //
+  FiqEnabled = ArmGetFiqState ();
+  ArmDisableFiq ();
+
+  // AArch64 alignment? The Vector table must be 2k-byte aligned (bottom 11 bits zero)?
+  //DEBUG ((EFI_D_ERROR, "vbar set addr: 0x%016lx\n",(UINTN)ExceptionHandlersStart));
+  //ASSERT(((UINTN)ExceptionHandlersStart & ((1 << 11)-1)) == 0);
+
+  // We do not copy the Exception Table at PcdGet32(PcdCpuVectorBaseAddress). We just set Vector Base Address to point into CpuDxe code.
+  ArmWriteVBar ((UINTN)ExceptionHandlersStart);
+
+  if (FiqEnabled) {
+    ArmEnableFiq ();
+  }
+
+  if (IrqEnabled) {
+    //
+    // Restore interrupt state
+    //
+    Status = Cpu->EnableInterrupt (Cpu);
+  }
+
+  return Status;
+}