Patch from open source community for CryptoPkg to allow it to build for ARM using...

[mirror_edk2.git] / ArmPkg / Drivers / CpuDxe / ExceptionSupport.ARMv6.asm

diff --git a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
old mode 100755 (executable)
new mode 100644 (file)
index 6162e1b..240e1e3
--- a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
+++ b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
@@ -1,8 +1,10 @@
 //------------------------------------------------------------------------------ 
 //
-// Copyright (c) 2008-2010 Apple Inc. All rights reserved.
+// Use ARMv6 instruction to operate on a single stack
 //
-// All rights reserved. This program and the accompanying materials
+// Copyright (c) 2008 - 2010, Apple Inc. 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
@@ -12,7 +14,7 @@
 //
 //------------------------------------------------------------------------------
 
-
+#include <Library/PcdLib.h>
 
 /*
 
@@ -59,6 +61,8 @@ This is the stack constructed by the exception handler (low address to high addr
   PRESERVE8
   AREA  DxeExceptionHandlers, CODE, READONLY
   
+  ALIGN   32
+  
 //
 // This code gets copied to the ARM vector table
 // ExceptionHandlersStart - ExceptionHandlersEnd gets copied
@@ -101,6 +105,7 @@ ResetEntry
   bx        R1
 
 UndefinedInstructionEntry
+  sub       LR, LR, #4                ; Only -2 for Thumb, adjust in CommonExceptionEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
   cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
@@ -112,6 +117,7 @@ UndefinedInstructionEntry
   bx        R1
 
 SoftwareInterruptEntry
+  sub       LR, LR, #4                ; Only -2 for Thumb, adjust in CommonExceptionEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
                                       ; We are already in SVC mode
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
@@ -186,7 +192,7 @@ FiqEntry
 // This gets patched by the C code that patches in the vector table
 //
 CommonExceptionEntry
-  dcd       0x12345678
+  dcd       AsmCommonExceptionEntry
 
 ExceptionHandlersEnd
 
@@ -211,15 +217,27 @@ AsmCommonExceptionEntry
   str       R1, [SP, #0x40]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR
 
   add       R2, SP, #0x38           ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR
-  and       R1, R1, #0x1f           ; Check CPSR to see if User or System Mode
-  cmp       R1, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1df))
-  cmpne     R1, #0x10               ;   
+  and       R3, R1, #0x1f           ; Check CPSR to see if User or System Mode
+  cmp       R3, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1df))
+  cmpne     R3, #0x10               ;   
   stmeqed   R2, {lr}^               ;   save unbanked lr
                                     ; else 
   stmneed   R2, {lr}                ;   save SVC lr
 
-  ldr       R1, [SP, #0x58]         ; PC is the LR pushed by srsfd 
-  str       R1, [SP, #0x3c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC
+
+  ldr       R5, [SP, #0x58]         ; PC is the LR pushed by srsfd 
+                                    ; Check to see if we have to adjust for Thumb entry
+  sub       r4, r0, #1              ; if (ExceptionType == 1 || ExceptionType ==2)) {
+  cmp       r4, #1                  ;   // UND & SVC have differnt LR adjust for Thumb 
+  bhi       NoAdjustNeeded
+  
+  tst       r1, #0x20               ;   if ((CPSR & T)) == T) {  // Thumb Mode on entry 
+  addne     R5, R5, #2              ;     PC += 2;
+  str       R5,[SP,#0x58]           ; Update LR value pused by srsfd 
+  
+NoAdjustNeeded
+
+  str       R5, [SP, #0x3c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC
   
   sub       R1, SP, #0x60           ; We pused 0x60 bytes on the stack 
   str       R1, [SP, #0x34]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP
@@ -227,15 +245,30 @@ AsmCommonExceptionEntry
                                     ; R0 is ExceptionType 
   mov       R1,SP                   ; R1 is SystemContext 
 
+#if (FixedPcdGet32(PcdVFPEnabled))
+  vpush    {d0-d15}                  ; save vstm registers in case they are used in optimizations\r
+#endif
+
 /* 
-VOID\r
-EFIAPI\r
-CommonCExceptionHandler (\r
-  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0\r
-  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1\r
-  )\r
+VOID
+EFIAPI
+CommonCExceptionHandler (
+  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0
+  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1
+  )
+
 */
   blx       CommonCExceptionHandler ; Call exception handler
+\r
+#if (FixedPcdGet32(PcdVFPEnabled))
+  vpop      {d0-d15}\r
+#endif
+  
+  ldr       R1, [SP, #0x4c]         ; Restore EFI_SYSTEM_CONTEXT_ARM.IFSR
+  mcr       p15, 0, R1, c5, c0, 1   ; Write IFSR
+
+  ldr       R1, [SP, #0x44]         ; sRestore EFI_SYSTEM_CONTEXT_ARM.DFSR
+  mcr       p15, 0, R1, c5, c0, 0   ; Write DFSR
   
   ldr       R1,[SP,#0x3c]           ; EFI_SYSTEM_CONTEXT_ARM.PC
   str       R1,[SP,#0x58]           ; Store it back to srsfd stack slot so it can be restored