diff options
Diffstat (limited to 'ArmPkg')
| -rw-r--r-- | ArmPkg/Drivers/CpuDxe/AArch64/ExceptionSupport.S | 21 |
1 files changed, 19 insertions, 2 deletions
diff --git a/ArmPkg/Drivers/CpuDxe/AArch64/ExceptionSupport.S b/ArmPkg/Drivers/CpuDxe/AArch64/ExceptionSupport.S index a67477df83..8e2b37640f 100644 --- a/ArmPkg/Drivers/CpuDxe/AArch64/ExceptionSupport.S +++ b/ArmPkg/Drivers/CpuDxe/AArch64/ExceptionSupport.S @@ -1,5 +1,6 @@ //
// Copyright (c) 2011 - 2013 ARM LTD. All rights reserved.<BR>
+// Portion of Copyright (c) 2014 NVIDIA Corporation. All rights reserved.<BR>
//
// This program and the accompanying materials
// are licensed and made available under the terms and conditions of the BSD License
@@ -358,9 +359,25 @@ ASM_PFX(AsmCommonExceptionEntry): #define REG_ONE(REG1, OFFSET, CONTEXT_SIZE) ldr REG1, [sp, #(OFFSET-CONTEXT_SIZE)]
+ // Adjust SP to pop system registers
+ add sp, sp, GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE
+ ALL_SYS_REGS
- // pop all regs and return from exception.
- add sp, sp, GP_CONTEXT_SIZE
+ EL1_OR_EL2(x6)
+1:msr elr_el1, x1 // Exception Link Register
+ msr spsr_el1,x2 // Saved Processor Status Register 32bit
+ msr fpsr, x3 // Floating point Status Register 32bit
+ msr esr_el1, x4 // EL1 Exception syndrome register 32bit
+ msr far_el1, x5 // EL1 Fault Address Register
+ b 3f
+2:msr elr_el2, x1 // Exception Link Register
+ msr spsr_el2,x2 // Saved Processor Status Register 32bit
+ msr fpsr, x3 // Floating point Status Register 32bit
+ msr esr_el2, x4 // EL1 Exception syndrome register 32bit
+ msr far_el2, x5 // EL1 Fault Address Register
+
+3:// pop all regs and return from exception.
+ sub sp, sp, FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE
ALL_GP_REGS
// Adjust SP to pop next set
|