Adding support for a single stack, GCC check in will follow
authorAJFISH <AJFISH@6f19259b-4bc3-4df7-8a09-765794883524>
Fri, 8 Jan 2010 21:12:20 +0000 (21:12 +0000)
committerAJFISH <AJFISH@6f19259b-4bc3-4df7-8a09-765794883524>
Fri, 8 Jan 2010 21:12:20 +0000 (21:12 +0000)
git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@9697 6f19259b-4bc3-4df7-8a09-765794883524

ArmPkg/Drivers/CpuDxe/CpuDxe.c
ArmPkg/Drivers/CpuDxe/CpuDxe.inf
ArmPkg/Drivers/CpuDxe/Exception.c
ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
ArmPkg/Drivers/CpuDxe/ExceptionSupport.S

index c57dac2..2b43d2a 100644 (file)
@@ -14,6 +14,9 @@
 \r
 #include "CpuDxe.h"\r
 \r
+BOOLEAN gExceptionContext = FALSE;\r
+BOOLEAN mInterruptState   = FALSE;\r
+\r
 EFI_STATUS\r
 EFIAPI\r
 CpuFlushCpuDataCache (\r
@@ -25,13 +28,13 @@ CpuFlushCpuDataCache (
 {\r
   switch (FlushType) {\r
     case EfiCpuFlushTypeWriteBack:\r
-      WriteBackDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);\r
+      WriteBackDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);\r
       break;\r
     case EfiCpuFlushTypeInvalidate:\r
-      InvalidateDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);\r
+      InvalidateDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);\r
       break;\r
     case EfiCpuFlushTypeWriteBackInvalidate:\r
-      WriteBackInvalidateDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);\r
+      WriteBackInvalidateDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);\r
       break;\r
     default:\r
       return EFI_INVALID_PARAMETER;\r
@@ -46,9 +49,11 @@ CpuEnableInterrupt (
   IN EFI_CPU_ARCH_PROTOCOL          *This\r
   )\r
 {\r
-  if (ArmProcessorMode() != ARM_PROCESSOR_MODE_IRQ) {\r
-    ArmEnableInterrupts(); \r
+  if (!gExceptionContext) {\r
+    ArmEnableInterrupts ();\r
   }\r
+\r
+  mInterruptState  = TRUE;\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -59,9 +64,11 @@ CpuDisableInterrupt (
   IN EFI_CPU_ARCH_PROTOCOL          *This\r
   )\r
 {\r
-  if (ArmProcessorMode() != ARM_PROCESSOR_MODE_IRQ) {\r
-    ArmDisableInterrupts();\r
+  if (!gExceptionContext) {\r
+    ArmDisableInterrupts ();\r
   }\r
+\r
+  mInterruptState = FALSE;\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -76,7 +83,7 @@ CpuGetInterruptState (
     return EFI_INVALID_PARAMETER;\r
   }\r
 \r
-  *State = ArmGetInterruptState();\r
+  *State = mInterruptState;\r
   return EFI_SUCCESS;\r
 }\r
 \r
@@ -98,7 +105,7 @@ CpuRegisterInterruptHandler (
   IN EFI_CPU_INTERRUPT_HANDLER      InterruptHandler\r
   )\r
 {\r
-  return RegisterInterruptHandler(InterruptType, InterruptHandler);\r
+  return RegisterInterruptHandler (InterruptType, InterruptHandler);\r
 }\r
 \r
 EFI_STATUS\r
@@ -147,8 +154,8 @@ CpuDxeInitialize (
   IN EFI_HANDLE         ImageHandle,\r
   IN EFI_SYSTEM_TABLE   *SystemTable\r
   )\r
-{\r
-  InitializeExceptions(&mCpu);  \r
-  return gBS->InstallMultipleProtocolInterfaces(&mCpuHandle, &gEfiCpuArchProtocolGuid, &mCpu, NULL);\r
+{ \r
+  InitializeExceptions (&mCpu);  \r
+  return gBS->InstallMultipleProtocolInterfaces (&mCpuHandle, &gEfiCpuArchProtocolGuid, &mCpu, NULL);\r
 }\r
 \r
index 314965c..f22b255 100644 (file)
@@ -1,4 +1,3 @@
-#%HEADER%\r
 #/** @file\r
 #  \r
 #  DXE CPU driver\r
   CpuDxe.h\r
   DebugSupport.c\r
   Exception.c\r
-  ExceptionSupport.asm | RVCT\r
-  ExceptionSupport.S   | GCC\r
+\r
+#\r
+# Prior to ARMv6 we have multiple stacks, one per mode\r
+#\r
+#  ExceptionSupport.asm | RVCT\r
+  ExceptionSupport.S         | GCC\r
+\r
+#\r
+# ARMv6 or later uses a single stack via srs/stm instructions\r
+#\r
+  ExceptionSupport.ARMv6.asm | RVCT\r
+#  ExceptionSupport.ARMv6.S   | GCC\r
 \r
 [Packages]\r
   ArmPkg/ArmPkg.dec\r
index fa256e6..8859ca8 100644 (file)
@@ -15,6 +15,8 @@
 #include "CpuDxe.h" \r
 #include <Library/CacheMaintenanceLib.h>\r
 \r
+extern BOOLEAN gExceptionContext;\r
+\r
 VOID\r
 ExceptionHandlersStart (\r
   VOID\r
@@ -120,7 +122,16 @@ RegisterDebuggerInterruptHandler (
   return EFI_SUCCESS;\r
 }\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
 VOID\r
 EFIAPI\r
@@ -130,6 +141,8 @@ CommonCExceptionHandler (
   )\r
 {\r
   BOOLEAN Dispatched = FALSE;\r
\r
+  gExceptionContext = TRUE; \r
   \r
   if (ExceptionType <= MAX_ARM_EXCEPTION) {\r
     if (gDebuggerExceptionHandlers[ExceptionType]) {\r
@@ -144,8 +157,13 @@ CommonCExceptionHandler (
       gExceptionHandlers[ExceptionType] (ExceptionType, SystemContext);\r
       Dispatched = TRUE;\r
     }\r
+  } else {\r
+    DEBUG ((EFI_D_ERROR, "Unknown exception type %d from %08x\n", ExceptionType, SystemContext.SystemContextArm->PC));\r
+    ASSERT (FALSE);\r
   }\r
 \r
+  gExceptionContext = FALSE; \r
+\r
   if (Dispatched) {\r
     //\r
     // We did work so this was an expected ExceptionType\r
@@ -163,7 +181,7 @@ CommonCExceptionHandler (
   //\r
   // Code after here is the default exception handler...\r
   //\r
-  DEBUG ((EFI_D_ERROR, "Exception %d from %08x\n", ExceptionType, SystemContext.SystemContextArm->PC));\r
+  DEBUG ((EFI_D_ERROR, "%a Exception from %08x\n", gExceptionTypeString[ExceptionType], SystemContext.SystemContextArm->PC));\r
   ASSERT (FALSE);\r
 \r
 }\r
@@ -200,7 +218,7 @@ InitializeExceptions (
   }\r
   \r
   //\r
-  // Copy an implementation of the ARM exception vectors to 0x0.\r
+  // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.\r
   //\r
   Length = (UINTN)ExceptionHandlersEnd - (UINTN)ExceptionHandlersStart;\r
 \r
@@ -213,7 +231,7 @@ InitializeExceptions (
   // on embedded systems, for example, we don't want to hang up.  So we'll check here for a status of \r
   // EFI_NOT_FOUND, and continue in that case.\r
   if (EFI_ERROR(Status) && (Status != EFI_NOT_FOUND)) {\r
-  ASSERT_EFI_ERROR (Status);\r
+    ASSERT_EFI_ERROR (Status);\r
   }\r
 \r
   CopyMem ((VOID *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress), (VOID *)ExceptionHandlersStart, Length);\r
@@ -225,7 +243,7 @@ InitializeExceptions (
   *(UINTN *) ((UINT8 *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress) + Offset) = (UINTN)AsmCommonExceptionEntry;\r
 \r
   // Flush Caches since we updated executable stuff\r
-  InvalidateInstructionCacheRange((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);\r
+  InvalidateInstructionCacheRange ((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);\r
 \r
   if (Enabled) {\r
     // \r
index 98fc962..6579a9c 100755 (executable)
@@ -1,6 +1,6 @@
 //------------------------------------------------------------------------------ 
 //
-// Copyright (c) 2008-2009 Apple Inc. All rights reserved.
+// Copyright (c) 2008-2010 Apple Inc. All rights reserved.
 //
 // All rights reserved. This program and the accompanying materials
 // are licensed and made available under the terms and conditions of the BSD License
 
 /*
 
-This is the stack constructed by the exception handler
-
+This is the stack constructed by the exception handler (low address to high address)
+                # R0 - IFAR is EFI_SYSTEM_CONTEXT for ARM
+  Reg   Offset
+  ===   ======              
   R0    0x00    # stmfd     SP!,{R0-R12}
   R1    0x04
   R2    0x08
@@ -43,7 +45,7 @@ This is the stack constructed by the exception handler
   LR    0x54    # SVC Link register (we need to restore it)
   
   LR    0x58    # pushed by srsfd    
-  CPSR  0x5c    # pushed by srsfd
+  CPSR  0x5c    
 
  */
  
@@ -57,6 +59,10 @@ This is the stack constructed by the exception handler
   PRESERVE8
   AREA  DxeExceptionHandlers, CODE, READONLY
   
+//
+// This code gets copied to the ARM vector table
+// ExceptionHandlersStart - ExceptionHandlersEnd gets copied
+//
 ExceptionHandlersStart
 
 Reset
@@ -85,101 +91,109 @@ Fiq
 
 ResetEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     if,#0x13                  ; Switch to SVC for common stack
+                                      ; We are already in SVC mode
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
   
-  mov       R0,#0
+  mov       R0,#0                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 UndefinedInstructionEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     i,#0x13                   ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#1
-  ldr       R1,CommonExceptionEntry
+  mov       R0,#1                     ; ExceptionType
+  ldr       R1,CommonExceptionEntry
   bx        R1
 
 SoftwareInterruptEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     i,#0x13                   ; Switch to SVC for common stack
+                                      ; We are already in SVC mode
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#2
+  mov       R0,#2                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 PrefetchAbortEntry
   sub       LR,LR,#4
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     i,#0x13                   ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#3
+  mov       R0,#3                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 DataAbortEntry
   sub       LR,LR,#8
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     i,#0x13                   ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#4
+  mov       R0,#4                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 ReservedExceptionEntry
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     if,#0x13                  ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#5
+  mov       R0,#5                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 IrqEntry
   sub       LR,LR,#4
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     i,#0x13                   ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
 
-  mov       R0,#6
+  mov       R0,#6                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
 FiqEntry
   sub       LR,LR,#4
   srsfd     #0x13!                    ; Store return state on SVC stack
-  cpsid     if,#0x13                  ; Switch to SVC for common stack
+  cps       #0x13                     ; Switch to SVC for common stack
   stmfd     SP!,{LR}                  ; Store the link register for the current mode
   sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
   stmfd     SP!,{R0-R12}              ; Store the register state
-
-  mov       R0,#7
+                                      ; Since we have already switch to SVC R8_fiq - R12_fiq
+                                      ; never get used or saved
+  mov       R0,#7                     ; ExceptionType
   ldr       R1,CommonExceptionEntry
   bx        R1
 
+//
+// This gets patched by the C code that patches in the vector table
+//
 CommonExceptionEntry
   dcd       0x12345678
 
 ExceptionHandlersEnd
 
+//
+// This code runs from CpuDxe driver loaded address. It is patched into 
+// CommonExceptionEntry.
+//
 AsmCommonExceptionEntry
   mrc       p15, 0, R1, c6, c0, 2   ; Read IFAR
   str       R1, [SP, #0x50]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR 
@@ -196,25 +210,50 @@ AsmCommonExceptionEntry
   ldr       R1, [SP, #0x5c]         ; srsfd saved pre-exception CPSR on the stack 
   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               ;   
+  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
-  str       R1, [SP, #0x38]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.LR
   
   sub       R1, SP, #0x60           ; We pused 0x60 bytes on the stack 
   str       R1, [SP, #0x34]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP
   
-                                    ; R0 is exception type 
-  mov       R1,SP                   ; Prepare System Context pointer as an argument for the exception handler
+                                    ; R0 is ExceptionType 
+  mov       R1,SP                   ; R1 is SystemContext 
+
+/* 
+VOID\r
+EFIAPI\r
+CommonCExceptionHandler (\r
+  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0\r
+  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1\r
+  )\r
+*/
   blx       CommonCExceptionHandler ; Call exception handler
   
-  ldr       R2,[SP,#0x40]           ; EFI_SYSTEM_CONTEXT_ARM.CPSR
-  str       R2,[SP,#0x5c]           ; Store it back to srsfd stack slot so it can be restored 
-
-  ldr       R2,[SP,#0x3c]           ; EFI_SYSTEM_CONTEXT_ARM.PC
-  str       R2,[SP,#0x58]           ; Store it back to srsfd stack slot so it can be restored 
+  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 
 
+  ldr       R1,[SP,#0x40]           ; EFI_SYSTEM_CONTEXT_ARM.CPSR
+  str       R1,[SP,#0x5c]           ; Store it back to srsfd stack slot so it can be restored 
+  
+  add       R3, SP, #0x54           ; Make R3 point to SVC LR saved on entry
+  add       R2, SP, #0x38           ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR
+  and       R1, R1, #0x1f           ; Check to see if User or System Mode
+  cmp       R1, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1df))
+  cmpne     R1, #0x10               ;   
+  ldmeqed   R2, {lr}^               ;   restore unbanked lr
+                                    ; else
+  ldmneed   R3, {lr}                ;   restore SVC lr, via ldmfd SP!, {LR}
+  
   ldmfd     SP!,{R0-R12}            ; Restore general purpose registers
-                                    ; Exception handler can not change SP or LR as we would blow chunks
+                                    ; Exception handler can not change SP
                                     
   add       SP,SP,#0x20             ; Clear out the remaining stack space
   ldmfd     SP!,{LR}                ; restore the link register for this context
index 8574af6..f52cab4 100755 (executable)
@@ -1,6 +1,6 @@
 #------------------------------------------------------------------------------ 
 #
-# Copyright (c) 2008-2009 Apple Inc. All rights reserved.
+# Copyright (c) 2008-2010 Apple Inc. All rights reserved.
 #
 # All rights reserved. This program and the accompanying materials
 # are licensed and made available under the terms and conditions of the BSD License
@@ -48,54 +48,92 @@ ASM_PFX(Fiq):
   b ASM_PFX(FiqEntry)
 
 ASM_PFX(ResetEntry):
-  stmfd     sp!,{r0-r1}
-  mov       r0,#0
-  ldr       r1,ASM_PFX(CommonExceptionEntry)
-  bx        r1
+  srsdb     #0x13!                    @ Store return state on SVC stack
+  stmfd     SP!,{LR}                  @ Store the link register for the current mode
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+  
+  mov       R0,#0
+  ldr       R1,ASM_PFX(CommonExceptionEntry)
+  bx        R1
 
 ASM_PFX(UndefinedInstructionEntry):
-  stmfd     sp!,{r0-r1}
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#1
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(SoftwareInterruptEntry):
-  stmfd     sp!,{r0-r1}
+  srsdb     #0x13!                    @ Store return state on SVC stack
+  stmfd     SP!,{LR}                  @ Store the link register for the current mode
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#2
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(PrefetchAbortEntry):
-  stmfd     sp!,{r0-r1}
+  sub       LR,LR,#4
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#3
-  sub       lr,lr,#4
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(DataAbortEntry):
-  stmfd     sp!,{r0-r1}
+  sub       LR,LR,#8
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#4
-  sub       lr,lr,#8
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(ReservedExceptionEntry):
-  stmfd     sp!,{r0-r1}
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#5
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(IrqEntry):
-  stmfd     sp!,{r0-r1}
+  sub       LR,LR,#4
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#6
-  sub       lr,lr,#4
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
 ASM_PFX(FiqEntry):
-  stmfd     sp!,{r0-r1}
+  sub       LR,LR,#4
+  srsdb     #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
+  sub       SP,SP,#0x20               @ Save space for SP, LR, PC, IFAR - CPSR
+  stmfd     SP!,{R0-R12}              @ Store the register state
+
   mov       r0,#7
-  sub       lr,lr,#4
   ldr       r1,ASM_PFX(CommonExceptionEntry)
   bx        r1
 
@@ -108,45 +146,46 @@ ASM_PFX(CommonExceptionEntry):
 ASM_PFX(ExceptionHandlersEnd):
 
 ASM_PFX(AsmCommonExceptionEntry):
-  mrc       p15, 0, r1, c6, c0, 2   @ Read IFAR
-  stmfd     sp!,{r1}                @ Store the IFAR
-  
-  mrc       p15, 0, r1, c5, c0, 1   @ Read IFSR
-  stmfd     sp!,{r1}                @ Store the IFSR
-  
-  mrc       p15, 0, r1, c6, c0, 0   @ Read DFAR
-  stmfd     sp!,{r1}                @ Store the DFAR
-  
-  mrc       p15, 0, r1, c5, c0, 0   @ Read DFSR
-  stmfd     sp!,{r1}                @ Store the DFSR
+  mrc       p15, 0, R1, c6, c0, 2   @ Read IFAR
+  str       R1, [SP, #0x50]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR 
   
-  mrs       r1,spsr                 @ Read SPSR (which is the pre-exception CPSR)
-  stmfd     sp!,{r1}                @ Store the SPSR
+  mrc       p15, 0, R1, c5, c0, 1   @ Read IFSR
+  str       R1, [SP, #0x4c]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.IFSR
   
-  stmfd     sp!,{lr}                @ Store the link register (which is the pre-exception PC)
-  stmfd     sp,{sp,lr}^             @ Store user/system mode stack pointer and link register
-  nop                               @ Required by ARM architecture
-  sub       sp,sp,#0x08             @ Adjust stack pointer
-  stmfd     sp!,{r2-r12}            @ Store general purpose registers
+  mrc       p15, 0, R1, c6, c0, 0   @ Read DFAR
+  str       R1, [SP, #0x48]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.DFAR
   
-  ldr       r3,[sp,#0x50]           @ Read saved R1 from the stack (it was saved by the exception entry routine)
-  ldr       r2,[sp,#0x4C]           @ Read saved R0 from the stack (it was saved by the exception entry routine)
-  stmfd     sp!,{r2-r3}                    @ Store general purpose registers R0 and R1
+  mrc       p15, 0, R1, c5, c0, 0   @ Read DFSR
+  str       R1, [SP, #0x44]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.DFSR
   
-  mov       r1,sp                   @ Prepare System Context pointer as an argument for the exception handler
+  ldr       R1, [SP, #0x5c]         @ srsdb saved pre-exception CPSR on the stack 
+  str       R1, [SP, #0x40]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR
+  and       r1, r1, #0x1f           @ Check to see if User or System Mode
+  cmp       r1, #0x1f
+  cmpne     r1, #0x10
+  add       R2, SP, #0x38           @ Store it in EFI_SYSTEM_CONTEXT_ARM.LR
+  ldmneed   r2, {lr}^               @ User or System mode, use unbanked register
+  ldmneed   r2, {lr}                @ All other modes used banked register
+
+  ldr       R1, [SP, #0x58]         @ PC is the LR pushed by srsdb 
+  str       R1, [SP, #0x3c]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.PC
   
-  sub       sp,sp,#4                @ Adjust SP to preserve 8-byte alignment
-  bl        ASM_PFX(CommonCExceptionHandler) @ Call exception handler
-  add       sp,sp,#4                @ Adjust SP back to where we were
+  sub       R1, SP, #0x60           @ We pused 0x60 bytes on the stack 
+  str       R1, [SP, #0x34]         @ Store it in EFI_SYSTEM_CONTEXT_ARM.SP
   
-  ldr       r2,[sp,#0x40]           @ Load CPSR from context, in case it has changed
-  msr       SPSR_cxsf,r2            @ Store it back to the SPSR to be restored when exiting this handler
-
-  ldmfd     sp!,{r0-r12}            @ Restore general purpose registers
-  ldmia     sp,{sp,lr}^             @ Restore user/system mode stack pointer and link register
-  nop                               @ Required by ARM architecture
-  add       sp,sp,#0x08             @ Adjust stack pointer
-  ldmfd     sp!,{lr}                @ Restore the link register (which is the pre-exception PC)
-  add       sp,sp,#0x1C             @ Clear out the remaining stack space
-  movs      pc,lr                   @ Return from exception
+                                              @ R0 is exception type 
+  mov       R1,SP                             @ Prepare System Context pointer as an argument for the exception handler
+  blx       ASM_PFX(CommonCExceptionHandler)  @ Call exception handler
   
+  ldr       R2,[SP,#0x40]           @ EFI_SYSTEM_CONTEXT_ARM.CPSR
+  str       R2,[SP,#0x5c]           @ Store it back to srsdb stack slot so it can be restored 
+
+  ldr       R2,[SP,#0x3c]           @ EFI_SYSTEM_CONTEXT_ARM.PC
+  str       R2,[SP,#0x58]           @ Store it back to srsdb stack slot so it can be restored 
+
+  ldmfd     SP!,{R0-R12}            @ Restore general purpose registers
+                                    @ Exception handler can not change SP or LR as we would blow chunks
+                                    
+  add       SP,SP,#0x20             @ Clear out the remaining stack space
+  ldmfd     SP!,{LR}                @ restore the link register for this context
+  rfefd     SP!                     @ return from exception via srsdb stack slot