[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.
//
//------------------------------------------------------------------------------


/*

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
  
//
// 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       0x12345678

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 

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

*/
  blx       CommonCExceptionHandler ; Call exception handler
  
  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
	17
	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
8a4d81e6 A	64	//
	65	// This code gets copied to the ARM vector table
	66	// ExceptionHandlersStart - ExceptionHandlersEnd gets copied
	67	//
8595f12d A	68	ExceptionHandlersStart
	69
	70	Reset
	71	b ResetEntry
	72
	73	UndefinedInstruction
	74	b UndefinedInstructionEntry
	75
	76	SoftwareInterrupt
	77	b SoftwareInterruptEntry
	78
	79	PrefetchAbort
	80	b PrefetchAbortEntry
	81
	82	DataAbort
	83	b DataAbortEntry
	84
	85	ReservedException
	86	b ReservedExceptionEntry
	87
	88	Irq
	89	b IrqEntry
	90
	91	Fiq
	92	b FiqEntry
	93
	94	ResetEntry
	95	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	96	; We are already in SVC mode
8595f12d A	97	stmfd SP!,{LR} ; Store the link register for the current mode
	98	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	99	stmfd SP!,{R0-R12} ; Store the register state
	100
8a4d81e6	101	mov R0,#0 ; ExceptionType
8595f12d A	102	ldr R1,CommonExceptionEntry
	103	bx R1
	104
	105	UndefinedInstructionEntry
6f72e28d	106	sub LR, LR, #4 ; Only -2 for Thumb, adjust in CommonExceptionEntry
8595f12d	107	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	108	cps #0x13 ; Switch to SVC for common stack
8595f12d A	109	stmfd SP!,{LR} ; Store the link register for the current mode
	110	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	111	stmfd SP!,{R0-R12} ; Store the register state
	112
8a4d81e6 A	113	mov R0,#1 ; ExceptionType
8a4d81e6 A	114	ldr R1,CommonExceptionEntry;
8595f12d A	115	bx R1
	116
	117	SoftwareInterruptEntry
6f72e28d	118	sub LR, LR, #4 ; Only -2 for Thumb, adjust in CommonExceptionEntry
8595f12d	119	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	120	; We are already in SVC mode
8595f12d A	121	stmfd SP!,{LR} ; Store the link register for the current mode
	122	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	123	stmfd SP!,{R0-R12} ; Store the register state
	124
8a4d81e6	125	mov R0,#2 ; ExceptionType
8595f12d A	126	ldr R1,CommonExceptionEntry
	127	bx R1
	128
	129	PrefetchAbortEntry
	130	sub LR,LR,#4
	131	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	132	cps #0x13 ; Switch to SVC for common stack
8595f12d A	133	stmfd SP!,{LR} ; Store the link register for the current mode
	134	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	135	stmfd SP!,{R0-R12} ; Store the register state
	136
8a4d81e6	137	mov R0,#3 ; ExceptionType
8595f12d A	138	ldr R1,CommonExceptionEntry
	139	bx R1
	140
	141	DataAbortEntry
	142	sub LR,LR,#8
	143	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	144	cps #0x13 ; Switch to SVC for common stack
8595f12d A	145	stmfd SP!,{LR} ; Store the link register for the current mode
	146	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	147	stmfd SP!,{R0-R12} ; Store the register state
	148
8a4d81e6	149	mov R0,#4 ; ExceptionType
8595f12d A	150	ldr R1,CommonExceptionEntry
	151	bx R1
	152
	153	ReservedExceptionEntry
	154	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	155	cps #0x13 ; Switch to SVC for common stack
8595f12d A	156	stmfd SP!,{LR} ; Store the link register for the current mode
	157	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	158	stmfd SP!,{R0-R12} ; Store the register state
	159
8a4d81e6	160	mov R0,#5 ; ExceptionType
8595f12d A	161	ldr R1,CommonExceptionEntry
	162	bx R1
	163
	164	IrqEntry
	165	sub LR,LR,#4
	166	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	167	cps #0x13 ; Switch to SVC for common stack
8595f12d A	168	stmfd SP!,{LR} ; Store the link register for the current mode
	169	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	170	stmfd SP!,{R0-R12} ; Store the register state
	171
8a4d81e6	172	mov R0,#6 ; ExceptionType
8595f12d A	173	ldr R1,CommonExceptionEntry
	174	bx R1
	175
	176	FiqEntry
	177	sub LR,LR,#4
	178	srsfd #0x13! ; Store return state on SVC stack
8a4d81e6	179	cps #0x13 ; Switch to SVC for common stack
8595f12d A	180	stmfd SP!,{LR} ; Store the link register for the current mode
	181	sub SP,SP,#0x20 ; Save space for SP, LR, PC, IFAR - CPSR
	182	stmfd SP!,{R0-R12} ; Store the register state
8a4d81e6 A	183	; Since we have already switch to SVC R8_fiq - R12_fiq
	184	; never get used or saved
	185	mov R0,#7 ; ExceptionType
8595f12d A	186	ldr R1,CommonExceptionEntry
	187	bx R1
	188
8a4d81e6 A	189	//
	190	// This gets patched by the C code that patches in the vector table
	191	//
8595f12d A	192	CommonExceptionEntry
	193	dcd 0x12345678
	194
	195	ExceptionHandlersEnd
	196
8a4d81e6 A	197	//
	198	// This code runs from CpuDxe driver loaded address. It is patched into
	199	// CommonExceptionEntry.
	200	//
8595f12d A	201	AsmCommonExceptionEntry
	202	mrc p15, 0, R1, c6, c0, 2 ; Read IFAR
	203	str R1, [SP, #0x50] ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFAR
	204
	205	mrc p15, 0, R1, c5, c0, 1 ; Read IFSR
	206	str R1, [SP, #0x4c] ; Store it in EFI_SYSTEM_CONTEXT_ARM.IFSR
	207
	208	mrc p15, 0, R1, c6, c0, 0 ; Read DFAR
	209	str R1, [SP, #0x48] ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFAR
	210
	211	mrc p15, 0, R1, c5, c0, 0 ; Read DFSR
	212	str R1, [SP, #0x44] ; Store it in EFI_SYSTEM_CONTEXT_ARM.DFSR
	213
	214	ldr R1, [SP, #0x5c] ; srsfd saved pre-exception CPSR on the stack
	215	str R1, [SP, #0x40] ; Store it in EFI_SYSTEM_CONTEXT_ARM.CPSR
	216
8a4d81e6	217	add R2, SP, #0x38 ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR
6f72e28d	218	and R3, R1, #0x1f ; Check CPSR to see if User or System Mode
	219	cmp R3, #0x1f ; if ((CPSR == 0x10) \|\| (CPSR == 0x1df))
	220	cmpne R3, #0x10 ;
8a4d81e6 A	221	stmeqed R2, {lr}^ ; save unbanked lr
	222	; else
	223	stmneed R2, {lr} ; save SVC lr
	224
6f72e28d	225
	226	ldr R5, [SP, #0x58] ; PC is the LR pushed by srsfd
	227	; Check to see if we have to adjust for Thumb entry
	228	sub r4, r0, #1 ; if (ExceptionType == 1 \|\| ExceptionType ==2)) {
	229	cmp r4, #1 ; // UND & SVC have differnt LR adjust for Thumb
	230	bhi NoAdjustNeeded
	231
	232	tst r1, #0x20 ; if ((CPSR & T)) == T) { // Thumb Mode on entry
	233	addne R5, R5, #2 ; PC += 2;
65e27445	234	str R5,[SP,#0x58] ; Update LR value pused by srsfd
6f72e28d	235
	236	NoAdjustNeeded
	237
	238	str R5, [SP, #0x3c] ; Store it in EFI_SYSTEM_CONTEXT_ARM.PC
8595f12d A	239
	240	sub R1, SP, #0x60 ; We pused 0x60 bytes on the stack
	241	str R1, [SP, #0x34] ; Store it in EFI_SYSTEM_CONTEXT_ARM.SP
	242
8a4d81e6 A	243	; R0 is ExceptionType
	244	mov R1,SP ; R1 is SystemContext
	245
	246	/*
6f72e28d	247	VOID
	248	EFIAPI
	249	CommonCExceptionHandler (
	250	IN EFI_EXCEPTION_TYPE ExceptionType, R0
	251	IN OUT EFI_SYSTEM_CONTEXT SystemContext R1
	252	)
	253
8a4d81e6	254	*/
8595f12d A	255	blx CommonCExceptionHandler ; Call exception handler
8595f12d A	256
bb02cb80	257	ldr R1, [SP, #0x4c] ; Restore EFI_SYSTEM_CONTEXT_ARM.IFSR
	258	mcr p15, 0, R1, c5, c0, 1 ; Write IFSR
	259
	260	ldr R1, [SP, #0x44] ; sRestore EFI_SYSTEM_CONTEXT_ARM.DFSR
	261	mcr p15, 0, R1, c5, c0, 0 ; Write DFSR
	262
8a4d81e6 A	263	ldr R1,[SP,#0x3c] ; EFI_SYSTEM_CONTEXT_ARM.PC
8a4d81e6 A	264	str R1,[SP,#0x58] ; Store it back to srsfd stack slot so it can be restored
8595f12d	265
8a4d81e6 A	266	ldr R1,[SP,#0x40] ; EFI_SYSTEM_CONTEXT_ARM.CPSR
	267	str R1,[SP,#0x5c] ; Store it back to srsfd stack slot so it can be restored
	268
	269	add R3, SP, #0x54 ; Make R3 point to SVC LR saved on entry
	270	add R2, SP, #0x38 ; Make R2 point to EFI_SYSTEM_CONTEXT_ARM.LR
	271	and R1, R1, #0x1f ; Check to see if User or System Mode
bbd8fad2	272	cmp R1, #0x1f ; if ((CPSR == 0x10) \|\| (CPSR == 0x1f))
8a4d81e6 A	273	cmpne R1, #0x10 ;
	274	ldmeqed R2, {lr}^ ; restore unbanked lr
	275	; else
	276	ldmneed R3, {lr} ; restore SVC lr, via ldmfd SP!, {LR}
	277
8595f12d	278	ldmfd SP!,{R0-R12} ; Restore general purpose registers
8a4d81e6	279	; Exception handler can not change SP
8595f12d A	280
	281	add SP,SP,#0x20 ; Clear out the remaining stack space
	282	ldmfd SP!,{LR} ; restore the link register for this context
	283	rfefd SP! ; return from exception via srsfd stack slot
	284
	285	END
	286
	287