ArmPkg/CpuDxe: Restore AArch64 system registers before returning from exception

[mirror_edk2.git] / ArmPkg / Drivers / CpuDxe / AArch64 / ExceptionSupport.S

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