[mirror_edk2.git] / ArmPkg / Drivers / CpuDxe / ArmV6 / ExceptionSupport.asm

//------------------------------------------------------------------------------\r
//\r
// Use ARMv6 instruction to operate on a single stack\r
//\r
// Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>\r
// Copyright (c) 2014, ARM Limited. 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
\r
/*\r
\r
This is the stack constructed by the exception handler (low address to high address)\r
                # R0 - IFAR is EFI_SYSTEM_CONTEXT for ARM\r
  Reg   Offset\r
  ===   ======\r
  R0    0x00    # stmfd     SP!,{R0-R12}\r
  R1    0x04\r
  R2    0x08\r
  R3    0x0c\r
  R4    0x10\r
  R5    0x14\r
  R6    0x18\r
  R7    0x1c\r
  R8    0x20\r
  R9    0x24\r
  R10   0x28\r
  R11   0x2c\r
  R12   0x30\r
  SP    0x34    # reserved via subtraction 0x20 (32) from SP\r
  LR    0x38\r
  PC    0x3c\r
  CPSR  0x40\r
  DFSR  0x44\r
  DFAR  0x48\r
  IFSR  0x4c\r
  IFAR  0x50\r
\r
  LR    0x54    # SVC Link register (we need to restore it)\r
\r
  LR    0x58    # pushed by srsfd\r
  CPSR  0x5c\r
\r
 */\r
\r
\r
  EXPORT  ExceptionHandlersStart\r
  EXPORT  ExceptionHandlersEnd\r
  EXPORT  CommonExceptionEntry\r
  EXPORT  AsmCommonExceptionEntry\r
  IMPORT  CommonCExceptionHandler\r
\r
  PRESERVE8\r
  AREA  DxeExceptionHandlers, CODE, READONLY, CODEALIGN, ALIGN=5\r
\r
//\r
// This code gets copied to the ARM vector table\r
// ExceptionHandlersStart - ExceptionHandlersEnd gets copied\r
//\r
ExceptionHandlersStart\r
\r
Reset\r
  b   ResetEntry\r
\r
UndefinedInstruction\r
  b   UndefinedInstructionEntry\r
\r
SoftwareInterrupt\r
  b   SoftwareInterruptEntry\r
\r
PrefetchAbort\r
  b   PrefetchAbortEntry\r
\r
DataAbort\r
  b   DataAbortEntry\r
\r
ReservedException\r
  b   ReservedExceptionEntry\r
\r
Irq\r
  b   IrqEntry\r
\r
Fiq\r
  b   FiqEntry\r
\r
ResetEntry\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
                                      ; We are already in SVC mode\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#0                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
UndefinedInstructionEntry\r
  sub       LR, LR, #4                ; Only -2 for Thumb, adjust in CommonExceptionEntry\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#1                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry;\r
  bx        R1\r
\r
SoftwareInterruptEntry\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
                                      ; We are already in SVC mode\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#2                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
PrefetchAbortEntry\r
  sub       LR,LR,#4\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#3                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
DataAbortEntry\r
  sub       LR,LR,#8\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#4                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
ReservedExceptionEntry\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#5                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
IrqEntry\r
  sub       LR,LR,#4\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
\r
  mov       R0,#6                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
FiqEntry\r
  sub       LR,LR,#4\r
  srsfd     #0x13!                    ; Store return state on SVC stack\r
  cps       #0x13                     ; Switch to SVC for common stack\r
  stmfd     SP!,{LR}                  ; Store the link register for the current mode\r
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR\r
  stmfd     SP!,{R0-R12}              ; Store the register state\r
                                      ; Since we have already switch to SVC R8_fiq - R12_fiq\r
                                      ; never get used or saved\r
  mov       R0,#7                     ; ExceptionType\r
  ldr       R1,CommonExceptionEntry\r
  bx        R1\r
\r
//\r
// This gets patched by the C code that patches in the vector table\r
//\r
CommonExceptionEntry\r
  dcd       AsmCommonExceptionEntry\r
\r
ExceptionHandlersEnd\r
\r
//\r
// This code runs from CpuDxe driver loaded address. It is patched into\r
// CommonExceptionEntry.\r
//\r
AsmCommonExceptionEntry\r
  mrc       p15, 0, R1, c6, c0, 2   ; Read IFAR\r
  str       R1, [SP, #0x50]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR\r
\r
  mrc       p15, 0, R1, c5, c0, 1   ; Read IFSR\r
  str       R1, [SP, #0x4c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFSR\r
\r
  mrc       p15, 0, R1, c6, c0, 0   ; Read DFAR\r
  str       R1, [SP, #0x48]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFAR\r
\r
  mrc       p15, 0, R1, c5, c0, 0   ; Read DFSR\r
  str       R1, [SP, #0x44]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFSR\r
\r
  ldr       R1, [SP, #0x5c]         ; srsfd saved pre-exception CPSR on the stack\r
  str       R1, [SP, #0x40]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR\r
\r
  add       R2, SP, #0x38           ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR\r
  and       R3, R1, #0x1f           ; Check CPSR to see if User or System Mode\r
  cmp       R3, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1f))\r
  cmpne     R3, #0x10               ;\r
  stmeqed   R2, {lr}^               ;   save unbanked lr\r
                                    ; else\r
  stmneed   R2, {lr}                ;   save SVC lr\r
\r
\r
  ldr       R5, [SP, #0x58]         ; PC is the LR pushed by srsfd\r
                                    ; Check to see if we have to adjust for Thumb entry\r
  sub       r4, r0, #1              ; if (ExceptionType == 1 || ExceptionType == 2)) {\r
  cmp       r4, #1                  ;   // UND & SVC have differnt LR adjust for Thumb\r
  bhi       NoAdjustNeeded\r
\r
  tst       r1, #0x20               ;   if ((CPSR & T)) == T) {  // Thumb Mode on entry\r
  addne     R5, R5, #2              ;     PC += 2;\r
  strne     R5,[SP,#0x58]           ; Update LR value pushed by srsfd\r
\r
NoAdjustNeeded\r
\r
  str       R5, [SP, #0x3c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC\r
\r
  add       R1, SP, #0x60           ; We pushed 0x60 bytes on the stack\r
  str       R1, [SP, #0x34]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP\r
\r
                                    ; R0 is ExceptionType\r
  mov       R1,SP                   ; R1 is SystemContext\r
\r
#if (FixedPcdGet32(PcdVFPEnabled))\r
  vpush    {d0-d15}                 ; save vstm registers in case they are used in optimizations\r
#endif\r
\r
  mov       R4, SP                  ; Save current SP\r
  tst       R4, #4\r
  subne     SP, SP, #4              ; Adjust SP if not 8-byte aligned\r
\r
/*\r
VOID\r
EFIAPI\r
CommonCExceptionHandler (\r
  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0\r
  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1\r
  )\r
\r
*/\r
  blx       CommonCExceptionHandler ; Call exception handler\r
\r
  mov       SP, R4                  ; Restore SP\r
\r
#if (FixedPcdGet32(PcdVFPEnabled))\r
  vpop      {d0-d15}\r
#endif\r
\r
  ldr       R1, [SP, #0x4c]         ; Restore EFI_SYSTEM_CONTEXT_ARM.IFSR\r
  mcr       p15, 0, R1, c5, c0, 1   ; Write IFSR\r
\r
  ldr       R1, [SP, #0x44]         ; Restore EFI_SYSTEM_CONTEXT_ARM.DFSR\r
  mcr       p15, 0, R1, c5, c0, 0   ; Write DFSR\r
\r
  ldr       R1,[SP,#0x3c]           ; EFI_SYSTEM_CONTEXT_ARM.PC\r
  str       R1,[SP,#0x58]           ; Store it back to srsfd stack slot so it can be restored\r
\r
  ldr       R1,[SP,#0x40]           ; EFI_SYSTEM_CONTEXT_ARM.CPSR\r
  str       R1,[SP,#0x5c]           ; Store it back to srsfd stack slot so it can be restored\r
\r
  add       R3, SP, #0x54           ; Make R3 point to SVC LR saved on entry\r
  add       R2, SP, #0x38           ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR\r
  and       R1, R1, #0x1f           ; Check to see if User or System Mode\r
  cmp       R1, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1f))\r
  cmpne     R1, #0x10               ;\r
  ldmeqed   R2, {lr}^               ;   restore unbanked lr\r
                                    ; else\r
  ldmneed   R3, {lr}                ;   restore SVC lr, via ldmfd SP!, {LR}\r
\r
  ldmfd     SP!,{R0-R12}            ; Restore general purpose registers\r
                                    ; Exception handler can not change SP\r
\r
  add       SP,SP,#0x20             ; Clear out the remaining stack space\r
  ldmfd     SP!,{LR}                ; restore the link register for this context\r
  rfefd     SP!                     ; return from exception via srsfd stack slot\r
\r
  END\r
\r
\r
Commit	Line	Data
3402aac7	1	//------------------------------------------------------------------------------\r
a6caee65	2	//\r
	3	// Use ARMv6 instruction to operate on a single stack\r
	4	//\r
	5	// Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>\r
ec17f0f5	6	// Copyright (c) 2014, ARM Limited. All rights reserved.<BR>\r
a6caee65	7	//\r
	8	// This program and the accompanying materials\r
	9	// are licensed and made available under the terms and conditions of the BSD License\r
	10	// which accompanies this distribution. The full text of the license may be found at\r
	11	// http://opensource.org/licenses/bsd-license.php\r
	12	//\r
	13	// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,\r
	14	// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.\r
	15	//\r
	16	//------------------------------------------------------------------------------\r
	17	\r
	18	#include <Library/PcdLib.h>\r
	19	\r
	20	/*\r
	21	\r
	22	This is the stack constructed by the exception handler (low address to high address)\r
	23	# R0 - IFAR is EFI_SYSTEM_CONTEXT for ARM\r
	24	Reg Offset\r
3402aac7	25	=== ======\r
a6caee65	26	R0 0x00 # stmfd SP!,{R0-R12}\r
	27	R1 0x04\r
	28	R2 0x08\r
	29	R3 0x0c\r
	30	R4 0x10\r
	31	R5 0x14\r
	32	R6 0x18\r
	33	R7 0x1c\r
	34	R8 0x20\r
	35	R9 0x24\r
	36	R10 0x28\r
	37	R11 0x2c\r
	38	R12 0x30\r
ec17f0f5	39	SP 0x34 # reserved via subtraction 0x20 (32) from SP\r
a6caee65	40	LR 0x38\r
	41	PC 0x3c\r
	42	CPSR 0x40\r
	43	DFSR 0x44\r
	44	DFAR 0x48\r
	45	IFSR 0x4c\r
	46	IFAR 0x50\r
3402aac7	47	\r
a6caee65	48	LR 0x54 # SVC Link register (we need to restore it)\r
3402aac7 RC	49	\r
	50	LR 0x58 # pushed by srsfd\r
	51	CPSR 0x5c\r
a6caee65	52	\r
a6caee65	53	*/\r
3402aac7 RC	54	\r
3402aac7 RC	55	\r
a6caee65	56	EXPORT ExceptionHandlersStart\r
	57	EXPORT ExceptionHandlersEnd\r
	58	EXPORT CommonExceptionEntry\r
	59	EXPORT AsmCommonExceptionEntry\r
	60	IMPORT CommonCExceptionHandler\r
	61	\r
	62	PRESERVE8\r
21de944e	63	AREA DxeExceptionHandlers, CODE, READONLY, CODEALIGN, ALIGN=5\r
3402aac7	64	\r
a6caee65	65	//\r
	66	// This code gets copied to the ARM vector table\r
	67	// ExceptionHandlersStart - ExceptionHandlersEnd gets copied\r
	68	//\r
	69	ExceptionHandlersStart\r
	70	\r
	71	Reset\r
	72	b ResetEntry\r
	73	\r
	74	UndefinedInstruction\r
	75	b UndefinedInstructionEntry\r
	76	\r
	77	SoftwareInterrupt\r
	78	b SoftwareInterruptEntry\r
	79	\r
	80	PrefetchAbort\r
	81	b PrefetchAbortEntry\r
	82	\r
	83	DataAbort\r
	84	b DataAbortEntry\r
	85	\r
	86	ReservedException\r
	87	b ReservedExceptionEntry\r
	88	\r
	89	Irq\r
	90	b IrqEntry\r
	91	\r
	92	Fiq\r
	93	b FiqEntry\r
	94	\r
	95	ResetEntry\r
	96	srsfd #0x13! ; Store return state on SVC stack\r
	97	; We are already in SVC mode\r
	98	stmfd SP!,{LR} ; Store the link register for the current mode\r
	99	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	100	stmfd SP!,{R0-R12} ; Store the register state\r
3402aac7	101	\r
a6caee65	102	mov R0,#0 ; ExceptionType\r
	103	ldr R1,CommonExceptionEntry\r
	104	bx R1\r
	105	\r
	106	UndefinedInstructionEntry\r
	107	sub LR, LR, #4 ; Only -2 for Thumb, adjust in CommonExceptionEntry\r
	108	srsfd #0x13! ; Store return state on SVC stack\r
	109	cps #0x13 ; Switch to SVC for common stack\r
	110	stmfd SP!,{LR} ; Store the link register for the current mode\r
	111	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	112	stmfd SP!,{R0-R12} ; Store the register state\r
	113	\r
	114	mov R0,#1 ; ExceptionType\r
3402aac7	115	ldr R1,CommonExceptionEntry;\r
a6caee65	116	bx R1\r
	117	\r
	118	SoftwareInterruptEntry\r
a6caee65	119	srsfd #0x13! ; Store return state on SVC stack\r
	120	; We are already in SVC mode\r
	121	stmfd SP!,{LR} ; Store the link register for the current mode\r
	122	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	123	stmfd SP!,{R0-R12} ; Store the register state\r
	124	\r
	125	mov R0,#2 ; ExceptionType\r
	126	ldr R1,CommonExceptionEntry\r
	127	bx R1\r
	128	\r
	129	PrefetchAbortEntry\r
	130	sub LR,LR,#4\r
	131	srsfd #0x13! ; Store return state on SVC stack\r
	132	cps #0x13 ; Switch to SVC for common stack\r
	133	stmfd SP!,{LR} ; Store the link register for the current mode\r
	134	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	135	stmfd SP!,{R0-R12} ; Store the register state\r
	136	\r
	137	mov R0,#3 ; ExceptionType\r
	138	ldr R1,CommonExceptionEntry\r
	139	bx R1\r
	140	\r
	141	DataAbortEntry\r
	142	sub LR,LR,#8\r
	143	srsfd #0x13! ; Store return state on SVC stack\r
	144	cps #0x13 ; Switch to SVC for common stack\r
	145	stmfd SP!,{LR} ; Store the link register for the current mode\r
	146	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	147	stmfd SP!,{R0-R12} ; Store the register state\r
	148	\r
	149	mov R0,#4 ; ExceptionType\r
	150	ldr R1,CommonExceptionEntry\r
	151	bx R1\r
	152	\r
	153	ReservedExceptionEntry\r
	154	srsfd #0x13! ; Store return state on SVC stack\r
	155	cps #0x13 ; Switch to SVC for common stack\r
	156	stmfd SP!,{LR} ; Store the link register for the current mode\r
	157	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	158	stmfd SP!,{R0-R12} ; Store the register state\r
	159	\r
	160	mov R0,#5 ; ExceptionType\r
	161	ldr R1,CommonExceptionEntry\r
	162	bx R1\r
	163	\r
	164	IrqEntry\r
	165	sub LR,LR,#4\r
	166	srsfd #0x13! ; Store return state on SVC stack\r
	167	cps #0x13 ; Switch to SVC for common stack\r
	168	stmfd SP!,{LR} ; Store the link register for the current mode\r
	169	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	170	stmfd SP!,{R0-R12} ; Store the register state\r
	171	\r
	172	mov R0,#6 ; ExceptionType\r
	173	ldr R1,CommonExceptionEntry\r
	174	bx R1\r
	175	\r
	176	FiqEntry\r
	177	sub LR,LR,#4\r
	178	srsfd #0x13! ; Store return state on SVC stack\r
	179	cps #0x13 ; Switch to SVC for common stack\r
	180	stmfd SP!,{LR} ; Store the link register for the current mode\r
	181	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR\r
	182	stmfd SP!,{R0-R12} ; Store the register state\r
183	; Since we have already switch to SVC R8_fiq - R12_fiq\r
184	; never get used or saved\r
185	mov R0,#7 ; ExceptionType\r
186	ldr R1,CommonExceptionEntry\r
187	bx R1\r
188	\r
189	//\r
190	// This gets patched by the C code that patches in the vector table\r
191	//\r
192	CommonExceptionEntry\r
193	dcd AsmCommonExceptionEntry\r
194	\r
195	ExceptionHandlersEnd\r
196	\r
197	//\r
3402aac7	198	// This code runs from CpuDxe driver loaded address. It is patched into\r
a6caee65	199	// CommonExceptionEntry.\r
	200	//\r
	201	AsmCommonExceptionEntry\r
	202	mrc p15, 0, R1, c6, c0, 2 ; Read IFAR\r
3402aac7 RC	203	str R1, [SP, #0x50] ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR\r
3402aac7 RC	204	\r
a6caee65	205	mrc p15, 0, R1, c5, c0, 1 ; Read IFSR\r
a6caee65	206	str R1, [SP, #0x4c] ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFSR\r
3402aac7	207	\r
a6caee65	208	mrc p15, 0, R1, c6, c0, 0 ; Read DFAR\r
a6caee65	209	str R1, [SP, #0x48] ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFAR\r
3402aac7	210	\r
a6caee65	211	mrc p15, 0, R1, c5, c0, 0 ; Read DFSR\r
a6caee65	212	str R1, [SP, #0x44] ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFSR\r
3402aac7 RC	213	\r
3402aac7 RC	214	ldr R1, [SP, #0x5c] ; srsfd saved pre-exception CPSR on the stack\r
a6caee65	215	str R1, [SP, #0x40] ; Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR\r
	216	\r
	217	add R2, SP, #0x38 ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR\r
	218	and R3, R1, #0x1f ; Check CPSR to see if User or System Mode\r
ec17f0f5	219	cmp R3, #0x1f ; if ((CPSR == 0x10) \|\| (CPSR == 0x1f))\r
3402aac7	220	cmpne R3, #0x10 ;\r
a6caee65	221	stmeqed R2, {lr}^ ; save unbanked lr\r
3402aac7	222	; else\r
a6caee65	223	stmneed R2, {lr} ; save SVC lr\r
	224	\r
	225	\r
3402aac7	226	ldr R5, [SP, #0x58] ; PC is the LR pushed by srsfd\r
a6caee65	227	; Check to see if we have to adjust for Thumb entry\r
ec17f0f5	228	sub r4, r0, #1 ; if (ExceptionType == 1 \|\| ExceptionType == 2)) {\r
3402aac7	229	cmp r4, #1 ; // UND & SVC have differnt LR adjust for Thumb\r
a6caee65	230	bhi NoAdjustNeeded\r
3402aac7 RC	231	\r
3402aac7 RC	232	tst r1, #0x20 ; if ((CPSR & T)) == T) { // Thumb Mode on entry\r
a6caee65	233	addne R5, R5, #2 ; PC += 2;\r
ec17f0f5	234	strne R5,[SP,#0x58] ; Update LR value pushed by srsfd\r
3402aac7	235	\r
a6caee65	236	NoAdjustNeeded\r
	237	\r
	238	str R5, [SP, #0x3c] ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC\r
3402aac7	239	\r
1bc3923d	240	add R1, SP, #0x60 ; We pushed 0x60 bytes on the stack\r
a6caee65	241	str R1, [SP, #0x34] ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP\r
3402aac7 RC	242	\r
	243	; R0 is ExceptionType\r
	244	mov R1,SP ; R1 is SystemContext\r
a6caee65	245	\r
a6caee65	246	#if (FixedPcdGet32(PcdVFPEnabled))\r
ec17f0f5	247	vpush {d0-d15} ; save vstm registers in case they are used in optimizations\r
a6caee65	248	#endif\r
a6caee65	249	\r
ec17f0f5 OM	250	mov R4, SP ; Save current SP\r
	251	tst R4, #4\r
	252	subne SP, SP, #4 ; Adjust SP if not 8-byte aligned\r
	253	\r
3402aac7	254	/*\r
a6caee65	255	VOID\r
	256	EFIAPI\r
	257	CommonCExceptionHandler (\r
	258	IN EFI_EXCEPTION_TYPE ExceptionType, R0\r
	259	IN OUT EFI_SYSTEM_CONTEXT SystemContext R1\r
	260	)\r
	261	\r
	262	*/\r
	263	blx CommonCExceptionHandler ; Call exception handler\r
	264	\r
ec17f0f5 OM	265	mov SP, R4 ; Restore SP\r
ec17f0f5 OM	266	\r
a6caee65	267	#if (FixedPcdGet32(PcdVFPEnabled))\r
fe295078	268	vpop {d0-d15}\r
a6caee65	269	#endif\r
3402aac7	270	\r
a6caee65	271	ldr R1, [SP, #0x4c] ; Restore EFI_SYSTEM_CONTEXT_ARM.IFSR\r
	272	mcr p15, 0, R1, c5, c0, 1 ; Write IFSR\r
	273	\r
ec17f0f5	274	ldr R1, [SP, #0x44] ; Restore EFI_SYSTEM_CONTEXT_ARM.DFSR\r
a6caee65	275	mcr p15, 0, R1, c5, c0, 0 ; Write DFSR\r
3402aac7	276	\r
a6caee65	277	ldr R1,[SP,#0x3c] ; EFI_SYSTEM_CONTEXT_ARM.PC\r
3402aac7	278	str R1,[SP,#0x58] ; Store it back to srsfd stack slot so it can be restored\r
a6caee65	279	\r
a6caee65	280	ldr R1,[SP,#0x40] ; EFI_SYSTEM_CONTEXT_ARM.CPSR\r
3402aac7 RC	281	str R1,[SP,#0x5c] ; Store it back to srsfd stack slot so it can be restored\r
3402aac7 RC	282	\r
a6caee65	283	add R3, SP, #0x54 ; Make R3 point to SVC LR saved on entry\r
	284	add R2, SP, #0x38 ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR\r
	285	and R1, R1, #0x1f ; Check to see if User or System Mode\r
	286	cmp R1, #0x1f ; if ((CPSR == 0x10) \|\| (CPSR == 0x1f))\r
3402aac7	287	cmpne R1, #0x10 ;\r
a6caee65	288	ldmeqed R2, {lr}^ ; restore unbanked lr\r
	289	; else\r
	290	ldmneed R3, {lr} ; restore SVC lr, via ldmfd SP!, {LR}\r
3402aac7	291	\r
a6caee65	292	ldmfd SP!,{R0-R12} ; Restore general purpose registers\r
a6caee65	293	; Exception handler can not change SP\r
3402aac7	294	\r
a6caee65	295	add SP,SP,#0x20 ; Clear out the remaining stack space\r
	296	ldmfd SP!,{LR} ; restore the link register for this context\r
	297	rfefd SP! ; return from exception via srsfd stack slot\r
3402aac7	298	\r
a6caee65	299	END\r
	300	\r
	301	\r