Adding support for a single stack, GCC check in will follow

git-svn-id: https://edk2.svn.sourceforge.net/svnroot/edk2/trunk/edk2@9697 6f19259b-4bc3-4df7-8a09-765794883524

5 files changed, 214 insertions, 102 deletions

diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.c b/ArmPkg/Drivers/CpuDxe/CpuDxe.c
index c57dac2f74..2b43d2a9c9 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.c
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.c
@@ -14,6 +14,9 @@
 

 #include "CpuDxe.h"

 

+BOOLEAN gExceptionContext = FALSE;

+BOOLEAN mInterruptState   = FALSE;

+

 EFI_STATUS

 EFIAPI

 CpuFlushCpuDataCache (

@@ -25,13 +28,13 @@ CpuFlushCpuDataCache (
 {

   switch (FlushType) {

     case EfiCpuFlushTypeWriteBack:

-      WriteBackDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);

+      WriteBackDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);

       break;

     case EfiCpuFlushTypeInvalidate:

-      InvalidateDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);

+      InvalidateDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);

       break;

     case EfiCpuFlushTypeWriteBackInvalidate:

-      WriteBackInvalidateDataCacheRange((VOID *)(UINTN)Start, (UINTN)Length);

+      WriteBackInvalidateDataCacheRange ((VOID *)(UINTN)Start, (UINTN)Length);

       break;

     default:

       return EFI_INVALID_PARAMETER;

@@ -46,9 +49,11 @@ CpuEnableInterrupt (
   IN EFI_CPU_ARCH_PROTOCOL          *This

   )

 {

-  if (ArmProcessorMode() != ARM_PROCESSOR_MODE_IRQ) {

-    ArmEnableInterrupts(); 

+  if (!gExceptionContext) {

+    ArmEnableInterrupts ();

   }

+

+  mInterruptState  = TRUE;

   return EFI_SUCCESS;

 }

 

@@ -59,9 +64,11 @@ CpuDisableInterrupt (
   IN EFI_CPU_ARCH_PROTOCOL          *This

   )

 {

-  if (ArmProcessorMode() != ARM_PROCESSOR_MODE_IRQ) {

-    ArmDisableInterrupts();

+  if (!gExceptionContext) {

+    ArmDisableInterrupts ();

   }

+

+  mInterruptState = FALSE;

   return EFI_SUCCESS;

 }

 

@@ -76,7 +83,7 @@ CpuGetInterruptState (
     return EFI_INVALID_PARAMETER;

   }

 

-  *State = ArmGetInterruptState();

+  *State = mInterruptState;

   return EFI_SUCCESS;

 }

 

@@ -98,7 +105,7 @@ CpuRegisterInterruptHandler (
   IN EFI_CPU_INTERRUPT_HANDLER      InterruptHandler

   )

 {

-  return RegisterInterruptHandler(InterruptType, InterruptHandler);

+  return RegisterInterruptHandler (InterruptType, InterruptHandler);

 }

 

 EFI_STATUS

@@ -147,8 +154,8 @@ CpuDxeInitialize (
   IN EFI_HANDLE         ImageHandle,

   IN EFI_SYSTEM_TABLE   *SystemTable

   )

-{

-  InitializeExceptions(&mCpu);  

-  return gBS->InstallMultipleProtocolInterfaces(&mCpuHandle, &gEfiCpuArchProtocolGuid, &mCpu, NULL);

+{ 

+  InitializeExceptions (&mCpu);  

+  return gBS->InstallMultipleProtocolInterfaces (&mCpuHandle, &gEfiCpuArchProtocolGuid, &mCpu, NULL);

 }

 

diff --git a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
index 314965ca4a..f22b255c8d 100644
--- a/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
+++ b/ArmPkg/Drivers/CpuDxe/CpuDxe.inf
@@ -1,4 +1,3 @@
-#%HEADER%

 #/** @file

 #  

 #  DXE CPU driver

@@ -28,8 +27,18 @@
   CpuDxe.h

   DebugSupport.c

   Exception.c

-  ExceptionSupport.asm | RVCT

-  ExceptionSupport.S   | GCC

+

+#

+# Prior to ARMv6 we have multiple stacks, one per mode

+#

+#  ExceptionSupport.asm | RVCT

+  ExceptionSupport.S         | GCC

+

+#

+# ARMv6 or later uses a single stack via srs/stm instructions

+#

+  ExceptionSupport.ARMv6.asm | RVCT

+#  ExceptionSupport.ARMv6.S   | GCC

 

 [Packages]

   ArmPkg/ArmPkg.dec

diff --git a/ArmPkg/Drivers/CpuDxe/Exception.c b/ArmPkg/Drivers/CpuDxe/Exception.c
index fa256e60f8..8859ca8667 100644
--- a/ArmPkg/Drivers/CpuDxe/Exception.c
+++ b/ArmPkg/Drivers/CpuDxe/Exception.c
@@ -15,6 +15,8 @@
 #include "CpuDxe.h" 

 #include <Library/CacheMaintenanceLib.h>

 

+extern BOOLEAN gExceptionContext;

+

 VOID

 ExceptionHandlersStart (

   VOID

@@ -120,7 +122,16 @@ RegisterDebuggerInterruptHandler (
   return EFI_SUCCESS;

 }

 

-

+CHAR8 *gExceptionTypeString[] = {

+  "Reset",

+  "Undefined Instruction",

+  "SWI",

+  "Prefetch Abort",

+  "Data Abort",

+  "Undefined",

+  "IRQ",

+  "FIQ"

+};

 

 VOID

 EFIAPI

@@ -130,6 +141,8 @@ CommonCExceptionHandler (
   )

 {

   BOOLEAN Dispatched = FALSE;

+ 

+  gExceptionContext = TRUE; 

   

   if (ExceptionType <= MAX_ARM_EXCEPTION) {

     if (gDebuggerExceptionHandlers[ExceptionType]) {

@@ -144,8 +157,13 @@ CommonCExceptionHandler (
       gExceptionHandlers[ExceptionType] (ExceptionType, SystemContext);

       Dispatched = TRUE;

     }

+  } else {

+    DEBUG ((EFI_D_ERROR, "Unknown exception type %d from %08x\n", ExceptionType, SystemContext.SystemContextArm->PC));

+    ASSERT (FALSE);

   }

 

+  gExceptionContext = FALSE; 

+

   if (Dispatched) {

     //

     // We did work so this was an expected ExceptionType

@@ -163,7 +181,7 @@ CommonCExceptionHandler (
   //

   // Code after here is the default exception handler...

   //

-  DEBUG ((EFI_D_ERROR, "Exception %d from %08x\n", ExceptionType, SystemContext.SystemContextArm->PC));

+  DEBUG ((EFI_D_ERROR, "%a Exception from %08x\n", gExceptionTypeString[ExceptionType], SystemContext.SystemContextArm->PC));

   ASSERT (FALSE);

 

 }

@@ -200,7 +218,7 @@ InitializeExceptions (
   }

   

   //

-  // Copy an implementation of the ARM exception vectors to 0x0.

+  // Copy an implementation of the ARM exception vectors to PcdCpuVectorBaseAddress.

   //

   Length = (UINTN)ExceptionHandlersEnd - (UINTN)ExceptionHandlersStart;

 

@@ -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 

   // EFI_NOT_FOUND, and continue in that case.

   if (EFI_ERROR(Status) && (Status != EFI_NOT_FOUND)) {

-  ASSERT_EFI_ERROR (Status);

+    ASSERT_EFI_ERROR (Status);

   }

 

   CopyMem ((VOID *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress), (VOID *)ExceptionHandlersStart, Length);

@@ -225,7 +243,7 @@ InitializeExceptions (
   *(UINTN *) ((UINT8 *)(UINTN)PcdGet32 (PcdCpuVectorBaseAddress) + Offset) = (UINTN)AsmCommonExceptionEntry;

 

   // Flush Caches since we updated executable stuff

-  InvalidateInstructionCacheRange((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);

+  InvalidateInstructionCacheRange ((VOID *)PcdGet32(PcdCpuVectorBaseAddress), Length);

 

   if (Enabled) {

     // 

diff --git a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
index 98fc96284e..6579a9c6dd 100755
--- a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
+++ b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm
@@ -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
@@ -16,8 +16,10 @@
 
 /*
 
-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

+EFIAPI

+CommonCExceptionHandler (

+  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0

+  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1

+  )

+*/
   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
diff --git a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.S b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.S
index 8574af6d71..f52cab4c93 100755
--- a/ArmPkg/Drivers/CpuDxe/ExceptionSupport.S
+++ b/ArmPkg/Drivers/CpuDxe/ExceptionSupport.S
@@ -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