diff options
-rw-r--r-- | ArmPkg/Drivers/CpuDxe/CpuDxe.c | 31 | ||||
-rw-r--r-- | ArmPkg/Drivers/CpuDxe/CpuDxe.inf | 15 | ||||
-rw-r--r-- | ArmPkg/Drivers/CpuDxe/Exception.c | 28 | ||||
-rwxr-xr-x | ArmPkg/Drivers/CpuDxe/ExceptionSupport.ARMv6.asm | 101 | ||||
-rwxr-xr-x | ArmPkg/Drivers/CpuDxe/ExceptionSupport.S | 141 |
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 |