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

#------------------------------------------------------------------------------ 
#
# 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.
#
#------------------------------------------------------------------------------

/*

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    

 */
 

.globl ASM_PFX(ExceptionHandlersStart)
INTERWORK_FUNC(ExceptionHandlersStart)
.globl ASM_PFX(ExceptionHandlersEnd)
INTERWORK_FUNC(ExceptionHandlersEnd)
.globl ASM_PFX(CommonExceptionEntry)
INTERWORK_FUNC(CommonExceptionEntry)
.globl ASM_PFX(AsmCommonExceptionEntry)
INTERWORK_FUNC(AsmCommonExceptionEntry)
.globl ASM_PFX(CommonCExceptionHandler)
INTERWORK_FUNC(CommonCExceptionHandler)

.text
#if !defined(__APPLE__)
.fpu neon    @ makes vpush/vpop assemble
#endif
.align 3


//
// This code gets copied to the ARM vector table
// ExceptionHandlersStart - ExceptionHandlersEnd gets copied
//
ASM_PFX(ExceptionHandlersStart):

ASM_PFX(Reset):
  b ASM_PFX(ResetEntry)

ASM_PFX(UndefinedInstruction):
  b ASM_PFX(UndefinedInstructionEntry)

ASM_PFX(SoftwareInterrupt):
  b ASM_PFX(SoftwareInterruptEntry)

ASM_PFX(PrefetchAbort):
  b ASM_PFX(PrefetchAbortEntry)

ASM_PFX(DataAbort):
  b ASM_PFX(DataAbortEntry)

ASM_PFX(ReservedException):
  b ASM_PFX(ReservedExceptionEntry)

ASM_PFX(Irq):
  b ASM_PFX(IrqEntry)

ASM_PFX(Fiq):
  b ASM_PFX(FiqEntry)

ASM_PFX(ResetEntry):
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(UndefinedInstructionEntry):
  sub       LR, LR, #4                @ Only -2 for Thumb, adjust in CommonExceptionEntry
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(SoftwareInterruptEntry):
  sub       LR, LR, #4                @ Only -2 for Thumb, adjust in CommonExceptionEntry
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(PrefetchAbortEntry):
  sub       LR,LR,#4
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(DataAbortEntry):
  sub       LR,LR,#8
  srsdb     #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
  ldr       R1,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(ReservedExceptionEntry):
  srsdb     #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
  ldr       R1,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(IrqEntry):
  sub       LR,LR,#4
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

ASM_PFX(FiqEntry):
  sub       LR,LR,#4
  srsdb     #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,ASM_PFX(CommonExceptionEntry)
  bx        R1

//
// This gets patched by the C code that patches in the vector table
//
ASM_PFX(CommonExceptionEntry):
  .byte       0x12
  .byte       0x34
  .byte       0x56
  .byte       0x78

ASM_PFX(ExceptionHandlersEnd):

//
// This code runs from CpuDxe driver loaded address. It is patched into 
// CommonExceptionEntry.
//
ASM_PFX(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]         @ srsdb 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 

  vpush     {d0-d15}                @ save vstm registers in case they are used in optimizations


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

*/  
  blx       ASM_PFX(CommonCExceptionHandler)  @ Call exception handler

  vpop      {d0-d15}  

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