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

//------------------------------------------------------------------------------ 
//
// Use ARMv6 instruction to operate on a single stack
//
// Copyright (c) 2008 - 2010, Apple Inc. All rights reserved.<BR>
//
// This program and the accompanying materials
// are licensed and made available under the terms and conditions of the BSD License
// which accompanies this distribution.  The full text of the license may be found at
// http://opensource.org/licenses/bsd-license.php
//
// THE PROGRAM IS DISTRIBUTED UNDER THE BSD LICENSE ON AN "AS IS" BASIS,
// WITHOUT WARRANTIES OR REPRESENTATIONS OF ANY KIND, EITHER EXPRESS OR IMPLIED.
//
//------------------------------------------------------------------------------

#include <Library/PcdLib.h>

/*

This is the stack constructed by the exception handler (low address to high address)
                # R0 - IFAR is EFI_SYSTEM_CONTEXT for ARM
  Reg   Offset
  ===   ======              
  R0    0x00    # stmfd     SP!,{R0-R12}
  R1    0x04
  R2    0x08
  R3    0x0c
  R4    0x10
  R5    0x14
  R6    0x18
  R7    0x1c
  R8    0x20
  R9    0x24
  R10   0x28
  R11   0x2c
  R12   0x30
  SP    0x34    # reserved via adding 0x20 (32) to the SP
  LR    0x38
  PC    0x3c
  CPSR  0x40
  DFSR  0x44
  DFAR  0x48
  IFSR  0x4c
  IFAR  0x50
  
  LR    0x54    # SVC Link register (we need to restore it)
  
  LR    0x58    # pushed by srsfd    
  CPSR  0x5c    

 */
 
 
  EXPORT  ExceptionHandlersStart
  EXPORT  ExceptionHandlersEnd
  EXPORT  CommonExceptionEntry
  EXPORT  AsmCommonExceptionEntry
  IMPORT  CommonCExceptionHandler

  PRESERVE8
  AREA  DxeExceptionHandlers, CODE, READONLY
  
  ALIGN   32
  
//
// This code gets copied to the ARM vector table
// ExceptionHandlersStart - ExceptionHandlersEnd gets copied
//
ExceptionHandlersStart

Reset
  b   ResetEntry

UndefinedInstruction
  b   UndefinedInstructionEntry

SoftwareInterrupt
  b   SoftwareInterruptEntry

PrefetchAbort
  b   PrefetchAbortEntry

DataAbort
  b   DataAbortEntry

ReservedException
  b   ReservedExceptionEntry

Irq
  b   IrqEntry

Fiq
  b   FiqEntry

ResetEntry
  srsfd     #0x13!                    ; Store return state on SVC stack
                                      ; We are already in SVC mode
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state
  
  mov       R0,#0                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

UndefinedInstructionEntry
  sub       LR, LR, #4                ; Only -2 for Thumb, adjust in CommonExceptionEntry
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#1                     ; ExceptionType
  ldr       R1,CommonExceptionEntry; 
  bx        R1

SoftwareInterruptEntry
  sub       LR, LR, #4                ; Only -2 for Thumb, adjust in CommonExceptionEntry
  srsfd     #0x13!                    ; Store return state on SVC stack
                                      ; We are already in SVC mode
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#2                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

PrefetchAbortEntry
  sub       LR,LR,#4
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#3                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

DataAbortEntry
  sub       LR,LR,#8
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#4                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

ReservedExceptionEntry
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#5                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

IrqEntry
  sub       LR,LR,#4
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state

  mov       R0,#6                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

FiqEntry
  sub       LR,LR,#4
  srsfd     #0x13!                    ; Store return state on SVC stack
  cps       #0x13                     ; Switch to SVC for common stack
  stmfd     SP!,{LR}                  ; Store the link register for the current mode
  sub       SP,SP,#0x20               ; Save space for SP, LR, PC, IFAR - CPSR
  stmfd     SP!,{R0-R12}              ; Store the register state
                                      ; Since we have already switch to SVC R8_fiq - R12_fiq
                                      ; never get used or saved
  mov       R0,#7                     ; ExceptionType
  ldr       R1,CommonExceptionEntry
  bx        R1

//
// This gets patched by the C code that patches in the vector table
//
CommonExceptionEntry
  dcd       AsmCommonExceptionEntry

ExceptionHandlersEnd

//
// This code runs from CpuDxe driver loaded address. It is patched into 
// CommonExceptionEntry.
//
AsmCommonExceptionEntry
  mrc       p15, 0, R1, c6, c0, 2   ; Read IFAR
  str       R1, [SP, #0x50]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR 
  
  mrc       p15, 0, R1, c5, c0, 1   ; Read IFSR
  str       R1, [SP, #0x4c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFSR
  
  mrc       p15, 0, R1, c6, c0, 0   ; Read DFAR
  str       R1, [SP, #0x48]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFAR
  
  mrc       p15, 0, R1, c5, c0, 0   ; Read DFSR
  str       R1, [SP, #0x44]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFSR
  
  ldr       R1, [SP, #0x5c]         ; srsfd saved pre-exception CPSR on the stack 
  str       R1, [SP, #0x40]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR

  add       R2, SP, #0x38           ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR
  and       R3, R1, #0x1f           ; Check CPSR to see if User or System Mode
  cmp       R3, #0x1f               ; if ((CPSR == 0x10) || (CPSR == 0x1df))
  cmpne     R3, #0x10               ;   
  stmeqed   R2, {lr}^               ;   save unbanked lr
                                    ; else 
  stmneed   R2, {lr}                ;   save SVC lr


  ldr       R5, [SP, #0x58]         ; PC is the LR pushed by srsfd 
                                    ; Check to see if we have to adjust for Thumb entry
  sub       r4, r0, #1              ; if (ExceptionType == 1 || ExceptionType ==2)) {
  cmp       r4, #1                  ;   // UND & SVC have differnt LR adjust for Thumb 
  bhi       NoAdjustNeeded
  
  tst       r1, #0x20               ;   if ((CPSR & T)) == T) {  // Thumb Mode on entry 
  addne     R5, R5, #2              ;     PC += 2;
  str       R5,[SP,#0x58]           ; Update LR value pused by srsfd 
  
NoAdjustNeeded

  str       R5, [SP, #0x3c]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC
  
  sub       R1, SP, #0x60           ; We pused 0x60 bytes on the stack 
  str       R1, [SP, #0x34]         ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP
  
                                    ; R0 is ExceptionType 
  mov       R1,SP                   ; R1 is SystemContext 

#if (FixedPcdGet32(PcdVFPEnabled))
  vpush    {d0-d15}                  ; save vstm registers in case they are used in optimizations\r
#endif

/* 
VOID
EFIAPI
CommonCExceptionHandler (
  IN     EFI_EXCEPTION_TYPE           ExceptionType,   R0
  IN OUT EFI_SYSTEM_CONTEXT           SystemContext    R1
  )

*/
  blx       CommonCExceptionHandler ; Call exception handler
\r
#if (FixedPcdGet32(PcdVFPEnabled))
  vpop      {d0-d15}\r
#endif
  
  ldr       R1, [SP, #0x4c]         ; Restore EFI_SYSTEM_CONTEXT_ARM.IFSR
  mcr       p15, 0, R1, c5, c0, 1   ; Write IFSR

  ldr       R1, [SP, #0x44]         ; sRestore EFI_SYSTEM_CONTEXT_ARM.DFSR
  mcr       p15, 0, R1, c5, c0, 0   ; Write DFSR
  
  ldr       R1,[SP,#0x3c]           ; EFI_SYSTEM_CONTEXT_ARM.PC
  str       R1,[SP,#0x58]           ; Store it back to srsfd stack slot so it can be restored 

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