291 lines
12 KiB
ArmAsm
291 lines
12 KiB
ArmAsm
|
/***************************************************************************
|
||
|
|
*
|
||
|
|
* Copyright 2015-2019 BES.
|
||
|
|
* All rights reserved. All unpublished rights reserved.
|
||
|
|
*
|
||
|
|
* No part of this work may be used or reproduced in any form or by any
|
||
|
|
* means, or stored in a database or retrieval system, without prior written
|
||
|
|
* permission of BES.
|
||
|
|
*
|
||
|
|
* Use of this work is governed by a license granted by BES.
|
||
|
|
* This work contains confidential and proprietary information of
|
||
|
|
* BES. which is protected by copyright, trade secret,
|
||
|
|
* trademark and other intellectual property rights.
|
||
|
|
*
|
||
|
|
****************************************************************************/
|
||
|
|
|
||
|
|
.syntax unified
|
||
|
|
.align 4
|
||
|
|
|
||
|
|
.equ MODE_USR, 0x10
|
||
|
|
.equ MODE_FIQ, 0x11
|
||
|
|
.equ MODE_IRQ, 0x12
|
||
|
|
.equ MODE_SVC, 0x13
|
||
|
|
.equ MODE_ABT, 0x17
|
||
|
|
.equ MODE_UND, 0x1B
|
||
|
|
.equ MODE_SYS, 0x1F
|
||
|
|
.equ MODE_MASK, 0x1F
|
||
|
|
|
||
|
|
.equ CPSR_BIT_T, 0x20
|
||
|
|
|
||
|
|
.equ SAVED_REG_NUM, 17
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.IRQ_Handler"
|
||
|
|
.type IRQ_Handler, %function
|
||
|
|
.global IRQ_Handler
|
||
|
|
.weak IRQ_Handler
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
IRQ_Handler:
|
||
|
|
SUB LR, LR, #4 // Pre-adjust LR
|
||
|
|
SRSFD SP!, #MODE_SVC // Save LR_irq and SPSR_irq on to the SVC stack
|
||
|
|
CPS #MODE_SVC // Change to SVC mode
|
||
|
|
PUSH {R0-R3, R12, LR} // Save APCS corruptible registers
|
||
|
|
|
||
|
|
MOV R3, SP // Move SP into R3
|
||
|
|
AND R3, R3, #4 // Get stack adjustment to ensure 8-byte alignment
|
||
|
|
SUB SP, SP, R3 // Adjust stack
|
||
|
|
PUSH {R3, R4} // Store stack adjustment(R3) and user data(R4)
|
||
|
|
|
||
|
|
BLX c_irq_handler
|
||
|
|
|
||
|
|
POP {R3, R4} // Restore stack adjustment(R3) and user data(R4)
|
||
|
|
ADD SP, SP, R3 // Unadjust stack
|
||
|
|
|
||
|
|
CLREX // Clear exclusive monitor for interrupted code
|
||
|
|
POP {R0-R3, R12, LR} // Restore stacked APCS registers
|
||
|
|
RFEFD SP! // Return from IRQ handler
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size IRQ_Handler, .-IRQ_Handler
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.Save_Registers"
|
||
|
|
.type Save_Registers, %function
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
Save_Registers:
|
||
|
|
PUSH {R12}
|
||
|
|
SUB SP, SP, #SAVED_REG_NUM*4
|
||
|
|
|
||
|
|
// Save r0-r7
|
||
|
|
STMFD SP, {R0-R7}
|
||
|
|
|
||
|
|
// Switch to the mode in SPSR
|
||
|
|
LDR R12,[SP, #(SAVED_REG_NUM+2)*4]
|
||
|
|
MRS R1, CPSR
|
||
|
|
AND R2, R1, #MODE_MASK
|
||
|
|
BIC R3, R1, #MODE_MASK
|
||
|
|
AND R0, R12, #MODE_MASK
|
||
|
|
CMP R0, #MODE_USR
|
||
|
|
MOVEQ R0, #MODE_SYS
|
||
|
|
ORR R3, R3, R0
|
||
|
|
MSR CPSR, R3
|
||
|
|
|
||
|
|
// Save r8-r14
|
||
|
|
ADD R0, SP, #8*4
|
||
|
|
STMFD R0, {R8-R14}
|
||
|
|
|
||
|
|
// Switch back to the original CPSR
|
||
|
|
MSR CPSR, R1
|
||
|
|
|
||
|
|
// LR already saved by SRSFD
|
||
|
|
LDR R2, [SP, #(SAVED_REG_NUM+1)*4]
|
||
|
|
|
||
|
|
// Save pc, spsr
|
||
|
|
STR R2, [SP, #15*4]
|
||
|
|
STR R12,[SP, #16*4]
|
||
|
|
|
||
|
|
BX LR
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size Save_Registers, .-Save_Registers
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.Restore_Registers"
|
||
|
|
.type Restore_Registers, %function
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
Restore_Registers:
|
||
|
|
// Restore APCS corruptible registers
|
||
|
|
LDMFD SP, {R0-R3}
|
||
|
|
ADD SP, SP, #SAVED_REG_NUM*4
|
||
|
|
POP {R12}
|
||
|
|
BX LR
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size Restore_Registers, .-Restore_Registers
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.SVC_Handler"
|
||
|
|
.type SVC_Handler, %function
|
||
|
|
.global SVC_Handler
|
||
|
|
.weak SVC_Handler
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
SVC_Handler:
|
||
|
|
SRSFD SP!, #MODE_SVC // Store SPSR_svc and LR_svc onto SVC stack
|
||
|
|
|
||
|
|
BLX Save_Registers
|
||
|
|
|
||
|
|
MRS R12, SPSR // Load SPSR
|
||
|
|
TST R12, #CPSR_BIT_T // Thumb bit set?
|
||
|
|
LDRHNE R12, [LR,#-2] // Thumb: load halfword
|
||
|
|
BICNE R12, R12, #0xFF00 // extract SVC number
|
||
|
|
LDREQ R12, [LR,#-4] // ARM: load word
|
||
|
|
BICEQ R12, R12, #0xFF000000 // extract SVC number
|
||
|
|
MOV R0, R12 // Save SVC number
|
||
|
|
|
||
|
|
MOV R1, SP
|
||
|
|
|
||
|
|
MOV R3, SP // Move SP into R3
|
||
|
|
AND R3, R3, #4 // Get stack adjustment to ensure 8-byte alignment
|
||
|
|
SUB SP, SP, R3 // Adjust stack
|
||
|
|
PUSH {R3, R4} // Store stack adjustment(R3) and user data(R4)
|
||
|
|
|
||
|
|
BLX c_svc_handler // Call SVC Function
|
||
|
|
|
||
|
|
POP {R3, R4} // Restore stack adjustment(R3) and user data(R4)
|
||
|
|
ADD SP, SP, R3 // Unadjust stack
|
||
|
|
|
||
|
|
BLX Restore_Registers
|
||
|
|
CLREX // Clear exclusive monitor
|
||
|
|
RFEFD SP! // Return from exception
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size SVC_Handler, .-SVC_Handler
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.Undef_Handler"
|
||
|
|
.type Undef_Handler, %function
|
||
|
|
.global Undef_Handler
|
||
|
|
.weak Undef_Handler
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
Undef_Handler:
|
||
|
|
SRSFD SP!, #MODE_SVC // Save LR_irq and SPSR_irq on to the SVC stack
|
||
|
|
CPS #MODE_SVC // Change to SVC mode
|
||
|
|
|
||
|
|
BLX Save_Registers
|
||
|
|
|
||
|
|
TST R12,#CPSR_BIT_T // Check mode
|
||
|
|
MOVEQ R1, #4 // R1 = 4 ARM mode
|
||
|
|
MOVNE R1, #2 // R1 = 2 Thumb mode
|
||
|
|
|
||
|
|
// Get offending instruction address
|
||
|
|
LDR LR, [SP, #15*4]
|
||
|
|
SUB LR, LR, R1
|
||
|
|
STR LR, [SP, #15*4]
|
||
|
|
|
||
|
|
SUB R0, LR, R1
|
||
|
|
CMP R1, 4
|
||
|
|
LDREQ R0, [R0] // ARM mode - R0 points to offending instruction
|
||
|
|
BEQ Undef_Cont
|
||
|
|
|
||
|
|
// Thumb instruction
|
||
|
|
// Determine if it is a 32-bit Thumb instruction
|
||
|
|
LDRH R0, [R0]
|
||
|
|
MOV R2, #0x1C
|
||
|
|
CMP R2, R0, LSR #11
|
||
|
|
BHS Undef_Cont // 16-bit Thumb instruction
|
||
|
|
|
||
|
|
// 32-bit Thumb instruction. Unaligned - reconstruct the offending instruction
|
||
|
|
LDRH R2, [LR]
|
||
|
|
ORR R0, R2, R0, LSL #16
|
||
|
|
Undef_Cont:
|
||
|
|
MOV R2, SP // Set SP to third argument
|
||
|
|
|
||
|
|
AND R12, SP, #4 // Ensure stack is 8-byte aligned
|
||
|
|
SUB SP, SP, R12 // Adjust stack
|
||
|
|
PUSH {R12, LR} // Store stack adjustment and dummy LR
|
||
|
|
|
||
|
|
// R0 =Offending instruction, R1 =2(Thumb) or =4(ARM)
|
||
|
|
BLX c_undef_handler
|
||
|
|
|
||
|
|
POP {R12, LR} // Get stack adjustment & discard dummy LR
|
||
|
|
ADD SP, SP, R12 // Unadjust stack
|
||
|
|
|
||
|
|
LDR LR, [SP, #15*4] // Restore stacked LR and possibly adjust for retry
|
||
|
|
PUSH {LR}
|
||
|
|
BLX Restore_Registers
|
||
|
|
POP {LR}
|
||
|
|
STR LR, [SP]
|
||
|
|
CLREX // Clear exclusive monitor
|
||
|
|
RFEFD SP! // Return from exception
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size Undef_Handler, .-Undef_Handler
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.PAbt_Handler"
|
||
|
|
.type PAbt_Handler, %function
|
||
|
|
.global PAbt_Handler
|
||
|
|
.weak PAbt_Handler
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
PAbt_Handler:
|
||
|
|
SUB LR, LR, #4 // Pre-adjust LR
|
||
|
|
SRSFD SP!, #MODE_SVC // Save LR_irq and SPSR_irq on to the SVC stack
|
||
|
|
CPS #MODE_SVC // Change to SVC mode
|
||
|
|
|
||
|
|
BLX Save_Registers
|
||
|
|
|
||
|
|
MRC p15, 0, R0, c5, c0, 1 // IFSR
|
||
|
|
MRC p15, 0, R1, c6, c0, 2 // IFAR
|
||
|
|
|
||
|
|
MOV R2, SP // Set SP to third argument
|
||
|
|
|
||
|
|
AND R12, SP, #4 // Ensure stack is 8-byte aligned
|
||
|
|
SUB SP, SP, R12 // Adjust stack
|
||
|
|
PUSH {R12, LR} // Store stack adjustment and dummy LR
|
||
|
|
|
||
|
|
BLX c_pabt_handler
|
||
|
|
|
||
|
|
POP {R12, LR} // Get stack adjustment & discard dummy LR
|
||
|
|
ADD SP, SP, R12 // Unadjust stack
|
||
|
|
|
||
|
|
CLREX // Clear exclusive monitor
|
||
|
|
BLX Restore_Registers
|
||
|
|
RFEFD SP! // Return from exception
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size PAbt_Handler, .-PAbt_Handler
|
||
|
|
|
||
|
|
//-------------------------------------------------------
|
||
|
|
.section ".text.DAbt_Handler"
|
||
|
|
.type DAbt_Handler, %function
|
||
|
|
.global DAbt_Handler
|
||
|
|
.weak DAbt_Handler
|
||
|
|
.fnstart
|
||
|
|
.cantunwind
|
||
|
|
DAbt_Handler:
|
||
|
|
SUB LR, LR, #8 // Pre-adjust LR
|
||
|
|
SRSFD SP!, #MODE_SVC // Save LR_irq and SPSR_irq on to the SVC stack
|
||
|
|
CPS #MODE_SVC // Change to SVC mode
|
||
|
|
|
||
|
|
BLX Save_Registers
|
||
|
|
|
||
|
|
MRC p15, 0, R0, c5, c0, 0 // DFSR
|
||
|
|
MRC p15, 0, R1, c6, c0, 0 // DFAR
|
||
|
|
|
||
|
|
MOV R2, SP // Set SP to third argument
|
||
|
|
|
||
|
|
AND R12, SP, #4 // Ensure stack is 8-byte aligned
|
||
|
|
SUB SP, SP, R12 // Adjust stack
|
||
|
|
PUSH {R12, LR} // Store stack adjustment and dummy LR
|
||
|
|
|
||
|
|
BLX c_dabt_handler
|
||
|
|
|
||
|
|
POP {R12, LR} // Get stack adjustment & discard dummy LR
|
||
|
|
ADD SP, SP, R12 // Unadjust stack
|
||
|
|
|
||
|
|
CLREX // Clear exclusive monitor
|
||
|
|
BLX Restore_Registers
|
||
|
|
RFEFD SP! // Return from exception
|
||
|
|
|
||
|
|
.fnend
|
||
|
|
.size DAbt_Handler, .-DAbt_Handler
|
||
|
|
|
||
|
|
.end
|