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

//\r
// Copyright (c) 2011 - 2014 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
// which accompanies this distribution.  The full text of the license may be found at\r
// http://opensource.org/licenses/bsd-license.php\r
//\r
// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
//\r
//------------------------------------------------------------------------------\r
\r
#include <Library/PcdLib.h>\r
#include <AsmMacroIoLibV8.h>\r
\r
/*\r
  This is the stack constructed by the exception handler (low address to high address).\r
  X0 to FAR makes up the EFI_SYSTEM_CONTEXT for AArch64.\r
\r
  UINT64  X0;     0x000\r
  UINT64  X1;     0x008\r
  UINT64  X2;     0x010\r
  UINT64  X3;     0x018\r
  UINT64  X4;     0x020\r
  UINT64  X5;     0x028\r
  UINT64  X6;     0x030\r
  UINT64  X7;     0x038\r
  UINT64  X8;     0x040\r
  UINT64  X9;     0x048\r
  UINT64  X10;    0x050\r
  UINT64  X11;    0x058\r
  UINT64  X12;    0x060\r
  UINT64  X13;    0x068\r
  UINT64  X14;    0x070\r
  UINT64  X15;    0x078\r
  UINT64  X16;    0x080\r
  UINT64  X17;    0x088\r
  UINT64  X18;    0x090\r
  UINT64  X19;    0x098\r
  UINT64  X20;    0x0a0\r
  UINT64  X21;    0x0a8\r
  UINT64  X22;    0x0b0\r
  UINT64  X23;    0x0b8\r
  UINT64  X24;    0x0c0\r
  UINT64  X25;    0x0c8\r
  UINT64  X26;    0x0d0\r
  UINT64  X27;    0x0d8\r
  UINT64  X28;    0x0e0\r
  UINT64  FP;     0x0e8   // x29 - Frame Pointer\r
  UINT64  LR;     0x0f0   // x30 - Link Register\r
  UINT64  SP;     0x0f8   // x31 - Stack Pointer\r
\r
  // FP/SIMD Registers. 128bit if used as Q-regs.\r
  UINT64  V0[2];  0x100\r
  UINT64  V1[2];  0x110\r
  UINT64  V2[2];  0x120\r
  UINT64  V3[2];  0x130\r
  UINT64  V4[2];  0x140\r
  UINT64  V5[2];  0x150\r
  UINT64  V6[2];  0x160\r
  UINT64  V7[2];  0x170\r
  UINT64  V8[2];  0x180\r
  UINT64  V9[2];  0x190\r
  UINT64  V10[2]; 0x1a0\r
  UINT64  V11[2]; 0x1b0\r
  UINT64  V12[2]; 0x1c0\r
  UINT64  V13[2]; 0x1d0\r
  UINT64  V14[2]; 0x1e0\r
  UINT64  V15[2]; 0x1f0\r
  UINT64  V16[2]; 0x200\r
  UINT64  V17[2]; 0x210\r
  UINT64  V18[2]; 0x220\r
  UINT64  V19[2]; 0x230\r
  UINT64  V20[2]; 0x240\r
  UINT64  V21[2]; 0x250\r
  UINT64  V22[2]; 0x260\r
  UINT64  V23[2]; 0x270\r
  UINT64  V24[2]; 0x280\r
  UINT64  V25[2]; 0x290\r
  UINT64  V26[2]; 0x2a0\r
  UINT64  V27[2]; 0x2b0\r
  UINT64  V28[2]; 0x2c0\r
  UINT64  V29[2]; 0x2d0\r
  UINT64  V30[2]; 0x2e0\r
  UINT64  V31[2]; 0x2f0\r
\r
  // System Context\r
  UINT64  ELR;    0x300   // Exception Link Register\r
  UINT64  SPSR;   0x308   // Saved Processor Status Register\r
  UINT64  FPSR;   0x310   // Floating Point Status Register\r
  UINT64  ESR;    0x318   // Exception syndrome register\r
  UINT64  FAR;    0x320   // Fault Address Register\r
  UINT64  Padding;0x328   // Required for stack alignment\r
*/\r
\r
GCC_ASM_EXPORT(ExceptionHandlersStart)\r
GCC_ASM_EXPORT(ExceptionHandlersEnd)\r
GCC_ASM_EXPORT(CommonExceptionEntry)\r
GCC_ASM_EXPORT(AsmCommonExceptionEntry)\r
GCC_ASM_EXPORT(CommonCExceptionHandler)\r
\r
.text\r
.align 11\r
\r
#define GP_CONTEXT_SIZE    (32 *  8)\r
#define FP_CONTEXT_SIZE    (32 * 16)\r
#define SYS_CONTEXT_SIZE   ( 6 *  8) // 5 SYS regs + Alignment requirement (ie: the stack must be aligned on 0x10)\r
\r
// Cannot str x31 directly\r
#define ALL_GP_REGS                                     \\r
        REG_PAIR (x0,  x1,  0x000, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x2,  x3,  0x010, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x4,  x5,  0x020, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x6,  x7,  0x030, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x8,  x9,  0x040, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x10, x11, 0x050, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x12, x13, 0x060, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x14, x15, 0x070, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x16, x17, 0x080, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x18, x19, 0x090, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x20, x21, 0x0a0, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x22, x23, 0x0b0, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x24, x25, 0x0c0, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x26, x27, 0x0d0, GP_CONTEXT_SIZE);    \\r
        REG_PAIR (x28, x29, 0x0e0, GP_CONTEXT_SIZE);    \\r
        REG_ONE  (x30,      0x0f0, GP_CONTEXT_SIZE);\r
\r
// In order to save the SP we need to put it somewhere else first.\r
// STR only works with XZR/WZR directly\r
#define SAVE_SP \\r
        add x1, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE); \\r
        REG_ONE (x1,        0x0f8, GP_CONTEXT_SIZE);\r
\r
#define ALL_FP_REGS                                     \\r
        REG_PAIR (q0,  q1,  0x000, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q2,  q3,  0x020, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q4,  q5,  0x040, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q6,  q7,  0x060, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q8,  q9,  0x080, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q10, q11, 0x0a0, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q12, q13, 0x0c0, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q14, q15, 0x0e0, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q16, q17, 0x100, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q18, q19, 0x120, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q20, q21, 0x140, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q22, q23, 0x160, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q24, q25, 0x180, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q26, q27, 0x1a0, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q28, q29, 0x1c0, FP_CONTEXT_SIZE);    \\r
        REG_PAIR (q30, q31, 0x1e0, FP_CONTEXT_SIZE);\r
\r
#define ALL_SYS_REGS                                    \\r
        REG_PAIR (x1,  x2,  0x000, SYS_CONTEXT_SIZE);   \\r
        REG_PAIR (x3,  x4,  0x010, SYS_CONTEXT_SIZE);   \\r
        REG_ONE  (x5,       0x020, SYS_CONTEXT_SIZE);\r
\r
//\r
// This code gets copied to the ARM vector table\r
// VectorTableStart - VectorTableEnd gets copied\r
//\r
ASM_PFX(ExceptionHandlersStart):\r
\r
//\r
// Current EL with SP0 : 0x0 - 0x180\r
//\r
.align 7\r
ASM_PFX(SynchronousExceptionSP0):\r
  b   ASM_PFX(SynchronousExceptionEntry)\r
\r
.align 7\r
ASM_PFX(IrqSP0):\r
  b   ASM_PFX(IrqEntry)\r
\r
.align 7\r
ASM_PFX(FiqSP0):\r
  b   ASM_PFX(FiqEntry)\r
\r
.align 7\r
ASM_PFX(SErrorSP0):\r
  b   ASM_PFX(SErrorEntry)\r
\r
//\r
// Current EL with SPx: 0x200 - 0x380\r
//\r
.align 7\r
ASM_PFX(SynchronousExceptionSPx):\r
  b   ASM_PFX(SynchronousExceptionEntry)\r
\r
.align 7\r
ASM_PFX(IrqSPx):\r
  b   ASM_PFX(IrqEntry)\r
\r
.align 7\r
ASM_PFX(FiqSPx):\r
  b   ASM_PFX(FiqEntry)\r
\r
.align 7\r
ASM_PFX(SErrorSPx):\r
  b   ASM_PFX(SErrorEntry)\r
\r
//\r
// Lower EL using AArch64 : 0x400 - 0x580\r
//\r
.align 7\r
ASM_PFX(SynchronousExceptionA64):\r
  b   ASM_PFX(SynchronousExceptionEntry)\r
\r
.align 7\r
ASM_PFX(IrqA64):\r
  b   ASM_PFX(IrqEntry)\r
\r
.align 7\r
ASM_PFX(FiqA64):\r
  b   ASM_PFX(FiqEntry)\r
\r
.align 7\r
ASM_PFX(SErrorA64):\r
  b   ASM_PFX(SErrorEntry)\r
\r
//\r
// Lower EL using AArch32 : 0x0 - 0x180\r
//\r
.align 7\r
ASM_PFX(SynchronousExceptionA32):\r
  b   ASM_PFX(SynchronousExceptionEntry)\r
\r
.align 7\r
ASM_PFX(IrqA32):\r
  b   ASM_PFX(IrqEntry)\r
\r
.align 7\r
ASM_PFX(FiqA32):\r
  b   ASM_PFX(FiqEntry)\r
\r
.align 7\r
ASM_PFX(SErrorA32):\r
  b   ASM_PFX(SErrorEntry)\r
\r
\r
#undef  REG_PAIR\r
#undef  REG_ONE\r
#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE)  stp  REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]\r
#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE)         stur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]\r
\r
ASM_PFX(SynchronousExceptionEntry):\r
  // Move the stackpointer so we can reach our structure with the str instruction.\r
  sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
\r
  // Save all the General regs before touching x0 and x1.\r
  // This does not save r31(SP) as it is special. We do that later.\r
  ALL_GP_REGS\r
\r
  // Record the type of exception that occurred.\r
  mov       x0, #EXCEPT_AARCH64_SYNCHRONOUS_EXCEPTIONS\r
\r
  // Jump to our general handler to deal with all the common parts and process the exception.\r
  ldr       x1, ASM_PFX(CommonExceptionEntry)\r
  br        x1\r
\r
ASM_PFX(IrqEntry):\r
  sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
  ALL_GP_REGS\r
  mov       x0, #EXCEPT_AARCH64_IRQ\r
  ldr       x1, ASM_PFX(CommonExceptionEntry)\r
  br        x1\r
\r
ASM_PFX(FiqEntry):\r
  sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
  ALL_GP_REGS\r
  mov       x0, #EXCEPT_AARCH64_FIQ\r
  ldr       x1, ASM_PFX(CommonExceptionEntry)\r
  br        x1\r
\r
ASM_PFX(SErrorEntry):\r
  sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
  ALL_GP_REGS\r
  mov       x0, #EXCEPT_AARCH64_SERROR\r
  ldr       x1, ASM_PFX(CommonExceptionEntry)\r
  br        x1\r
\r
\r
//\r
// This gets patched by the C code that patches in the vector table\r
//\r
.align 3\r
ASM_PFX(CommonExceptionEntry):\r
  .8byte ASM_PFX(AsmCommonExceptionEntry)\r
\r
ASM_PFX(ExceptionHandlersEnd):\r
\r
\r
\r
//\r
// This code runs from CpuDxe driver loaded address. It is patched into\r
// CommonExceptionEntry.\r
//\r
ASM_PFX(AsmCommonExceptionEntry):\r
  /* NOTE:\r
     We have to break up the save code because the immediate value to be used\r
     with the SP is too big to do it all in one step so we need to shuffle the SP\r
     along as we go. (we only have 9bits of immediate to work with) */\r
\r
  // Save the current Stack pointer before we start modifying it.\r
  SAVE_SP\r
\r
  // Preserve the stack pointer we came in with before we modify it\r
  EL1_OR_EL2(x1)\r
1:mrs      x1, elr_el1   // Exception Link Register\r
  mrs      x2, spsr_el1  // Saved Processor Status Register 32bit\r
  mrs      x3, fpsr      // Floating point Status Register  32bit\r
  mrs      x4, esr_el1   // EL1 Exception syndrome register 32bit\r
  mrs      x5, far_el1   // EL1 Fault Address Register\r
  b        3f\r
\r
2:mrs      x1, elr_el2   // Exception Link Register\r
  mrs      x2, spsr_el2  // Saved Processor Status Register 32bit\r
  mrs      x3, fpsr      // Floating point Status Register  32bit\r
  mrs      x4, esr_el2   // EL2 Exception syndrome register 32bit\r
  mrs      x5, far_el2   // EL2 Fault Address Register\r
\r
  // Adjust SP to save next set\r
3:add      sp, sp, #FP_CONTEXT_SIZE\r
\r
  // Push FP regs to Stack.\r
  ALL_FP_REGS\r
\r
  // Adjust SP to save next set\r
  add      sp, sp, #SYS_CONTEXT_SIZE\r
\r
  // Save the SYS regs\r
  ALL_SYS_REGS\r
\r
  // Point to top of struct after all regs saved\r
  sub      sp, sp, #(GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
\r
  // x0 still holds the exception type.\r
  // Set x1 to point to the top of our struct on the Stack\r
  mov      x1, sp\r
\r
// CommonCExceptionHandler (\r
//   IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0\r
//   IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1\r
//   )\r
\r
  // Call the handler as defined above\r
\r
  // For now we spin in the handler if we received an abort of some kind.\r
  // We do not try to recover.\r
  bl       ASM_PFX(CommonCExceptionHandler) // Call exception handler\r
\r
\r
// Defines for popping from stack\r
\r
#undef REG_PAIR\r
#undef REG_ONE\r
#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE)    ldp  REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]\r
#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE)           ldur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]\r
\r
  //\r
  // Disable interrupt(IRQ and FIQ) before restoring context,\r
  // or else the context will be corrupted by interrupt reentrance.\r
  // Interrupt mask will be restored from spsr by hardware when we call eret\r
  //\r
  msr   daifset, #3\r
  isb\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
  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   // EL2 Exception syndrome register 32bit\r
  msr      far_el2, x5   // EL2 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
  add      sp, sp, #FP_CONTEXT_SIZE\r
  // Pop FP regs to Stack.\r
  ALL_FP_REGS\r
\r
  // Adjust SP to be where we started from when we came into the handler.\r
  // The handler can not change the SP.\r
  add      sp, sp, #SYS_CONTEXT_SIZE\r
\r
  eret\r
\r
#undef REG_PAIR\r
#undef REG_ONE\r
Commit	Line	Data
25402f5d	1	//\r
0db208ea	2	// Copyright (c) 2011 - 2014 ARM LTD. All rights reserved.<BR>\r
aa4cf2ef	3	// Portion of Copyright (c) 2014 NVIDIA Corporation. All rights reserved.<BR>\r
25402f5d HL	4	//\r
	5	// This program and the accompanying materials\r
	6	// are licensed and made available under the terms and conditions of the BSD License\r
	7	// which accompanies this distribution. The full text of the license may be found at\r
	8	// http://opensource.org/licenses/bsd-license.php\r
	9	//\r
	10	// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
	11	// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
	12	//\r
	13	//------------------------------------------------------------------------------\r
	14	\r
	15	#include <Library/PcdLib.h>\r
	16	#include <AsmMacroIoLibV8.h>\r
	17	\r
	18	/*\r
	19	This is the stack constructed by the exception handler (low address to high address).\r
	20	X0 to FAR makes up the EFI_SYSTEM_CONTEXT for AArch64.\r
	21	\r
	22	UINT64 X0; 0x000\r
	23	UINT64 X1; 0x008\r
	24	UINT64 X2; 0x010\r
	25	UINT64 X3; 0x018\r
	26	UINT64 X4; 0x020\r
	27	UINT64 X5; 0x028\r
	28	UINT64 X6; 0x030\r
	29	UINT64 X7; 0x038\r
	30	UINT64 X8; 0x040\r
	31	UINT64 X9; 0x048\r
	32	UINT64 X10; 0x050\r
	33	UINT64 X11; 0x058\r
	34	UINT64 X12; 0x060\r
	35	UINT64 X13; 0x068\r
	36	UINT64 X14; 0x070\r
	37	UINT64 X15; 0x078\r
	38	UINT64 X16; 0x080\r
	39	UINT64 X17; 0x088\r
	40	UINT64 X18; 0x090\r
	41	UINT64 X19; 0x098\r
	42	UINT64 X20; 0x0a0\r
	43	UINT64 X21; 0x0a8\r
	44	UINT64 X22; 0x0b0\r
	45	UINT64 X23; 0x0b8\r
	46	UINT64 X24; 0x0c0\r
	47	UINT64 X25; 0x0c8\r
	48	UINT64 X26; 0x0d0\r
	49	UINT64 X27; 0x0d8\r
	50	UINT64 X28; 0x0e0\r
	51	UINT64 FP; 0x0e8 // x29 - Frame Pointer\r
	52	UINT64 LR; 0x0f0 // x30 - Link Register\r
	53	UINT64 SP; 0x0f8 // x31 - Stack Pointer\r
	54	\r
	55	// FP/SIMD Registers. 128bit if used as Q-regs.\r
	56	UINT64 V0[2]; 0x100\r
	57	UINT64 V1[2]; 0x110\r
	58	UINT64 V2[2]; 0x120\r
	59	UINT64 V3[2]; 0x130\r
	60	UINT64 V4[2]; 0x140\r
	61	UINT64 V5[2]; 0x150\r
	62	UINT64 V6[2]; 0x160\r
	63	UINT64 V7[2]; 0x170\r
	64	UINT64 V8[2]; 0x180\r
	65	UINT64 V9[2]; 0x190\r
	66	UINT64 V10[2]; 0x1a0\r
	67	UINT64 V11[2]; 0x1b0\r
68	UINT64 V12[2]; 0x1c0\r
69	UINT64 V13[2]; 0x1d0\r
70	UINT64 V14[2]; 0x1e0\r
71	UINT64 V15[2]; 0x1f0\r
72	UINT64 V16[2]; 0x200\r
73	UINT64 V17[2]; 0x210\r
74	UINT64 V18[2]; 0x220\r
75	UINT64 V19[2]; 0x230\r
76	UINT64 V20[2]; 0x240\r
77	UINT64 V21[2]; 0x250\r
78	UINT64 V22[2]; 0x260\r
79	UINT64 V23[2]; 0x270\r
80	UINT64 V24[2]; 0x280\r
81	UINT64 V25[2]; 0x290\r
82	UINT64 V26[2]; 0x2a0\r
83	UINT64 V27[2]; 0x2b0\r
84	UINT64 V28[2]; 0x2c0\r
85	UINT64 V29[2]; 0x2d0\r
86	UINT64 V30[2]; 0x2e0\r
87	UINT64 V31[2]; 0x2f0\r
88	\r
89	// System Context\r
90	UINT64 ELR; 0x300 // Exception Link Register\r
91	UINT64 SPSR; 0x308 // Saved Processor Status Register\r
92	UINT64 FPSR; 0x310 // Floating Point Status Register\r
3ab3d138 OM	93	UINT64 ESR; 0x318 // Exception syndrome register\r
3ab3d138 OM	94	UINT64 FAR; 0x320 // Fault Address Register\r
25402f5d HL	95	UINT64 Padding;0x328 // Required for stack alignment\r
	96	*/\r
	97	\r
919a3a02 BJ	98	GCC_ASM_EXPORT(ExceptionHandlersStart)\r
	99	GCC_ASM_EXPORT(ExceptionHandlersEnd)\r
	100	GCC_ASM_EXPORT(CommonExceptionEntry)\r
	101	GCC_ASM_EXPORT(AsmCommonExceptionEntry)\r
	102	GCC_ASM_EXPORT(CommonCExceptionHandler)\r
25402f5d HL	103	\r
	104	.text\r
	105	.align 11\r
	106	\r
	107	#define GP_CONTEXT_SIZE (32 * 8)\r
	108	#define FP_CONTEXT_SIZE (32 * 16)\r
	109	#define SYS_CONTEXT_SIZE ( 6 * 8) // 5 SYS regs + Alignment requirement (ie: the stack must be aligned on 0x10)\r
	110	\r
	111	// Cannot str x31 directly\r
	112	#define ALL_GP_REGS \\r
	113	REG_PAIR (x0, x1, 0x000, GP_CONTEXT_SIZE); \\r
	114	REG_PAIR (x2, x3, 0x010, GP_CONTEXT_SIZE); \\r
	115	REG_PAIR (x4, x5, 0x020, GP_CONTEXT_SIZE); \\r
	116	REG_PAIR (x6, x7, 0x030, GP_CONTEXT_SIZE); \\r
	117	REG_PAIR (x8, x9, 0x040, GP_CONTEXT_SIZE); \\r
	118	REG_PAIR (x10, x11, 0x050, GP_CONTEXT_SIZE); \\r
	119	REG_PAIR (x12, x13, 0x060, GP_CONTEXT_SIZE); \\r
	120	REG_PAIR (x14, x15, 0x070, GP_CONTEXT_SIZE); \\r
	121	REG_PAIR (x16, x17, 0x080, GP_CONTEXT_SIZE); \\r
	122	REG_PAIR (x18, x19, 0x090, GP_CONTEXT_SIZE); \\r
	123	REG_PAIR (x20, x21, 0x0a0, GP_CONTEXT_SIZE); \\r
	124	REG_PAIR (x22, x23, 0x0b0, GP_CONTEXT_SIZE); \\r
	125	REG_PAIR (x24, x25, 0x0c0, GP_CONTEXT_SIZE); \\r
	126	REG_PAIR (x26, x27, 0x0d0, GP_CONTEXT_SIZE); \\r
	127	REG_PAIR (x28, x29, 0x0e0, GP_CONTEXT_SIZE); \\r
	128	REG_ONE (x30, 0x0f0, GP_CONTEXT_SIZE);\r
	129	\r
3ab3d138	130	// In order to save the SP we need to put it somewhere else first.\r
25402f5d HL	131	// STR only works with XZR/WZR directly\r
25402f5d HL	132	#define SAVE_SP \\r
0db208ea	133	add x1, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE); \\r
25402f5d HL	134	REG_ONE (x1, 0x0f8, GP_CONTEXT_SIZE);\r
	135	\r
	136	#define ALL_FP_REGS \\r
	137	REG_PAIR (q0, q1, 0x000, FP_CONTEXT_SIZE); \\r
	138	REG_PAIR (q2, q3, 0x020, FP_CONTEXT_SIZE); \\r
	139	REG_PAIR (q4, q5, 0x040, FP_CONTEXT_SIZE); \\r
	140	REG_PAIR (q6, q7, 0x060, FP_CONTEXT_SIZE); \\r
	141	REG_PAIR (q8, q9, 0x080, FP_CONTEXT_SIZE); \\r
	142	REG_PAIR (q10, q11, 0x0a0, FP_CONTEXT_SIZE); \\r
	143	REG_PAIR (q12, q13, 0x0c0, FP_CONTEXT_SIZE); \\r
	144	REG_PAIR (q14, q15, 0x0e0, FP_CONTEXT_SIZE); \\r
	145	REG_PAIR (q16, q17, 0x100, FP_CONTEXT_SIZE); \\r
	146	REG_PAIR (q18, q19, 0x120, FP_CONTEXT_SIZE); \\r
	147	REG_PAIR (q20, q21, 0x140, FP_CONTEXT_SIZE); \\r
	148	REG_PAIR (q22, q23, 0x160, FP_CONTEXT_SIZE); \\r
	149	REG_PAIR (q24, q25, 0x180, FP_CONTEXT_SIZE); \\r
	150	REG_PAIR (q26, q27, 0x1a0, FP_CONTEXT_SIZE); \\r
	151	REG_PAIR (q28, q29, 0x1c0, FP_CONTEXT_SIZE); \\r
	152	REG_PAIR (q30, q31, 0x1e0, FP_CONTEXT_SIZE);\r
	153	\r
	154	#define ALL_SYS_REGS \\r
	155	REG_PAIR (x1, x2, 0x000, SYS_CONTEXT_SIZE); \\r
	156	REG_PAIR (x3, x4, 0x010, SYS_CONTEXT_SIZE); \\r
	157	REG_ONE (x5, 0x020, SYS_CONTEXT_SIZE);\r
	158	\r
	159	//\r
	160	// This code gets copied to the ARM vector table\r
	161	// VectorTableStart - VectorTableEnd gets copied\r
	162	//\r
	163	ASM_PFX(ExceptionHandlersStart):\r
	164	\r
	165	//\r
	166	// Current EL with SP0 : 0x0 - 0x180\r
	167	//\r
	168	.align 7\r
	169	ASM_PFX(SynchronousExceptionSP0):\r
	170	b ASM_PFX(SynchronousExceptionEntry)\r
	171	\r
	172	.align 7\r
	173	ASM_PFX(IrqSP0):\r
	174	b ASM_PFX(IrqEntry)\r
	175	\r
	176	.align 7\r
	177	ASM_PFX(FiqSP0):\r
	178	b ASM_PFX(FiqEntry)\r
	179	\r
	180	.align 7\r
	181	ASM_PFX(SErrorSP0):\r
	182	b ASM_PFX(SErrorEntry)\r
	183	\r
	184	//\r
	185	// Current EL with SPx: 0x200 - 0x380\r
	186	//\r
	187	.align 7\r
	188	ASM_PFX(SynchronousExceptionSPx):\r
	189	b ASM_PFX(SynchronousExceptionEntry)\r
	190	\r
	191	.align 7\r
	192	ASM_PFX(IrqSPx):\r
	193	b ASM_PFX(IrqEntry)\r
	194	\r
	195	.align 7\r
	196	ASM_PFX(FiqSPx):\r
	197	b ASM_PFX(FiqEntry)\r
198	\r
199	.align 7\r
200	ASM_PFX(SErrorSPx):\r
201	b ASM_PFX(SErrorEntry)\r
202	\r
203	//\r
204	// Lower EL using AArch64 : 0x400 - 0x580\r
205	//\r
206	.align 7\r
207	ASM_PFX(SynchronousExceptionA64):\r
208	b ASM_PFX(SynchronousExceptionEntry)\r
209	\r
210	.align 7\r
211	ASM_PFX(IrqA64):\r
212	b ASM_PFX(IrqEntry)\r
213	\r
214	.align 7\r
215	ASM_PFX(FiqA64):\r
216	b ASM_PFX(FiqEntry)\r
217	\r
218	.align 7\r
219	ASM_PFX(SErrorA64):\r
220	b ASM_PFX(SErrorEntry)\r
221	\r
222	//\r
223	// Lower EL using AArch32 : 0x0 - 0x180\r
224	//\r
225	.align 7\r
226	ASM_PFX(SynchronousExceptionA32):\r
227	b ASM_PFX(SynchronousExceptionEntry)\r
228	\r
229	.align 7\r
230	ASM_PFX(IrqA32):\r
231	b ASM_PFX(IrqEntry)\r
232	\r
233	.align 7\r
234	ASM_PFX(FiqA32):\r
235	b ASM_PFX(FiqEntry)\r
236	\r
237	.align 7\r
238	ASM_PFX(SErrorA32):\r
239	b ASM_PFX(SErrorEntry)\r
240	\r
241	\r
242	#undef REG_PAIR\r
243	#undef REG_ONE\r
7eb1d852 BJ	244	#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE) stp REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]\r
7eb1d852 BJ	245	#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE) stur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]\r
25402f5d HL	246	\r
	247	ASM_PFX(SynchronousExceptionEntry):\r
	248	// Move the stackpointer so we can reach our structure with the str instruction.\r
0db208ea	249	sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	250	\r
	251	// Save all the General regs before touching x0 and x1.\r
	252	// This does not save r31(SP) as it is special. We do that later.\r
	253	ALL_GP_REGS\r
	254	\r
3ab3d138	255	// Record the type of exception that occurred.\r
25402f5d HL	256	mov x0, #EXCEPT_AARCH64_SYNCHRONOUS_EXCEPTIONS\r
	257	\r
	258	// Jump to our general handler to deal with all the common parts and process the exception.\r
	259	ldr x1, ASM_PFX(CommonExceptionEntry)\r
	260	br x1\r
	261	\r
	262	ASM_PFX(IrqEntry):\r
0db208ea	263	sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	264	ALL_GP_REGS\r
	265	mov x0, #EXCEPT_AARCH64_IRQ\r
	266	ldr x1, ASM_PFX(CommonExceptionEntry)\r
	267	br x1\r
	268	\r
	269	ASM_PFX(FiqEntry):\r
0db208ea	270	sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	271	ALL_GP_REGS\r
	272	mov x0, #EXCEPT_AARCH64_FIQ\r
	273	ldr x1, ASM_PFX(CommonExceptionEntry)\r
	274	br x1\r
	275	\r
	276	ASM_PFX(SErrorEntry):\r
0db208ea	277	sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	278	ALL_GP_REGS\r
	279	mov x0, #EXCEPT_AARCH64_SERROR\r
	280	ldr x1, ASM_PFX(CommonExceptionEntry)\r
	281	br x1\r
	282	\r
	283	\r
	284	//\r
	285	// This gets patched by the C code that patches in the vector table\r
	286	//\r
	287	.align 3\r
	288	ASM_PFX(CommonExceptionEntry):\r
f8a9910c	289	.8byte ASM_PFX(AsmCommonExceptionEntry)\r
25402f5d HL	290	\r
	291	ASM_PFX(ExceptionHandlersEnd):\r
	292	\r
	293	\r
	294	\r
	295	//\r
	296	// This code runs from CpuDxe driver loaded address. It is patched into\r
	297	// CommonExceptionEntry.\r
	298	//\r
	299	ASM_PFX(AsmCommonExceptionEntry):\r
	300	/* NOTE:\r
3ab3d138 OM	301	We have to break up the save code because the immediate value to be used\r
3ab3d138 OM	302	with the SP is too big to do it all in one step so we need to shuffle the SP\r
25402f5d HL	303	along as we go. (we only have 9bits of immediate to work with) */\r
	304	\r
	305	// Save the current Stack pointer before we start modifying it.\r
	306	SAVE_SP\r
	307	\r
	308	// Preserve the stack pointer we came in with before we modify it\r
	309	EL1_OR_EL2(x1)\r
	310	1:mrs x1, elr_el1 // Exception Link Register\r
	311	mrs x2, spsr_el1 // Saved Processor Status Register 32bit\r
	312	mrs x3, fpsr // Floating point Status Register 32bit\r
	313	mrs x4, esr_el1 // EL1 Exception syndrome register 32bit\r
	314	mrs x5, far_el1 // EL1 Fault Address Register\r
	315	b 3f\r
	316	\r
	317	2:mrs x1, elr_el2 // Exception Link Register\r
	318	mrs x2, spsr_el2 // Saved Processor Status Register 32bit\r
	319	mrs x3, fpsr // Floating point Status Register 32bit\r
3ab3d138 OM	320	mrs x4, esr_el2 // EL2 Exception syndrome register 32bit\r
3ab3d138 OM	321	mrs x5, far_el2 // EL2 Fault Address Register\r
25402f5d HL	322	\r
25402f5d HL	323	// Adjust SP to save next set\r
0db208ea	324	3:add sp, sp, #FP_CONTEXT_SIZE\r
25402f5d HL	325	\r
	326	// Push FP regs to Stack.\r
	327	ALL_FP_REGS\r
	328	\r
	329	// Adjust SP to save next set\r
0db208ea	330	add sp, sp, #SYS_CONTEXT_SIZE\r
25402f5d HL	331	\r
	332	// Save the SYS regs\r
	333	ALL_SYS_REGS\r
	334	\r
	335	// Point to top of struct after all regs saved\r
0db208ea	336	sub sp, sp, #(GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	337	\r
	338	// x0 still holds the exception type.\r
	339	// Set x1 to point to the top of our struct on the Stack\r
	340	mov x1, sp\r
	341	\r
	342	// CommonCExceptionHandler (\r
	343	// IN EFI_EXCEPTION_TYPE ExceptionType, R0\r
	344	// IN OUT EFI_SYSTEM_CONTEXT SystemContext R1\r
	345	// )\r
	346	\r
	347	// Call the handler as defined above\r
	348	\r
	349	// For now we spin in the handler if we received an abort of some kind.\r
	350	// We do not try to recover.\r
	351	bl ASM_PFX(CommonCExceptionHandler) // Call exception handler\r
	352	\r
	353	\r
	354	// Defines for popping from stack\r
	355	\r
	356	#undef REG_PAIR\r
	357	#undef REG_ONE\r
7eb1d852	358	#define REG_PAIR(REG1, REG2, OFFSET, CONTEXT_SIZE) ldp REG1, REG2, [sp, #(OFFSET-CONTEXT_SIZE)]\r
7eb1d852	359	#define REG_ONE(REG1, OFFSET, CONTEXT_SIZE) ldur REG1, [sp, #(OFFSET-CONTEXT_SIZE)]\r
25402f5d	360	\r
36c64c66 HG	361	//\r
	362	// Disable interrupt(IRQ and FIQ) before restoring context,\r
	363	// or else the context will be corrupted by interrupt reentrance.\r
	364	// Interrupt mask will be restored from spsr by hardware when we call eret\r
	365	//\r
	366	msr daifset, #3\r
	367	isb\r
	368	\r
aa4cf2ef	369	// Adjust SP to pop system registers\r
0db208ea	370	add sp, sp, #(GP_CONTEXT_SIZE + FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
aa4cf2ef	371	ALL_SYS_REGS\r
25402f5d	372	\r
aa4cf2ef	373	EL1_OR_EL2(x6)\r
	374	1:msr elr_el1, x1 // Exception Link Register\r
	375	msr spsr_el1,x2 // Saved Processor Status Register 32bit\r
	376	msr fpsr, x3 // Floating point Status Register 32bit\r
	377	msr esr_el1, x4 // EL1 Exception syndrome register 32bit\r
	378	msr far_el1, x5 // EL1 Fault Address Register\r
	379	b 3f\r
	380	2:msr elr_el2, x1 // Exception Link Register\r
	381	msr spsr_el2,x2 // Saved Processor Status Register 32bit\r
	382	msr fpsr, x3 // Floating point Status Register 32bit\r
3ab3d138 OM	383	msr esr_el2, x4 // EL2 Exception syndrome register 32bit\r
3ab3d138 OM	384	msr far_el2, x5 // EL2 Fault Address Register\r
aa4cf2ef	385	\r
aa4cf2ef	386	3:// pop all regs and return from exception.\r
0db208ea	387	sub sp, sp, #(FP_CONTEXT_SIZE + SYS_CONTEXT_SIZE)\r
25402f5d HL	388	ALL_GP_REGS\r
	389	\r
	390	// Adjust SP to pop next set\r
0db208ea	391	add sp, sp, #FP_CONTEXT_SIZE\r
25402f5d HL	392	// Pop FP regs to Stack.\r
	393	ALL_FP_REGS\r
	394	\r
	395	// Adjust SP to be where we started from when we came into the handler.\r
	396	// The handler can not change the SP.\r
0db208ea	397	add sp, sp, #SYS_CONTEXT_SIZE\r
25402f5d HL	398	\r
	399	eret\r
	400	\r
	401	#undef REG_PAIR\r
	402	#undef REG_ONE\r