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

Current EDK2 source code does actually trigger nested interrupted (even if
the PI spec says interrupt should not be nested).
This issue has highlighted the lack of restoring ELR_EL2/ELR_EL1 register.

Contributed-under: TianoCore Contribution Agreement 1.0
Signed-off: Vijayakumar Subbu <vsubbu@nvidia.com>
Signed-off: Olivier Martin <olivier.martin@arm.com>

git-svn-id: https://svn.code.sf.net/p/edk2/code/trunk/edk2@15481 6f19259b-4bc3-4df7-8a09-765794883524

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