299 lines
6.8 KiB
ArmAsm
299 lines
6.8 KiB
ArmAsm
|
/*
|
||
|
|
* exceptionstub.S
|
||
|
|
*
|
||
|
|
* Copyright (C) 2014-2020 R. Stange <rsta2@o2online.de>
|
||
|
|
* Copyright (C) 2024 Romain Calascibetta <romain.calascibetta@gmail.com>
|
||
|
|
*
|
||
|
|
* This program is free software: you can redistribute it and/or modify
|
||
|
|
* it under the terms of the GNU General Public License as published by
|
||
|
|
* the Free Software Foundation, either version 3 of the License, or
|
||
|
|
* (at your option) any later version.
|
||
|
|
*
|
||
|
|
* This program is distributed in the hope that it will be useful,
|
||
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
* GNU General Public License for more details.
|
||
|
|
*
|
||
|
|
* You should have received a copy of the GNU General Public License
|
||
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||
|
|
*/
|
||
|
|
#include "exception_stub.h"
|
||
|
|
#include "bcm.h"
|
||
|
|
|
||
|
|
.macro vector handler
|
||
|
|
.align 7
|
||
|
|
b \handler
|
||
|
|
.endm
|
||
|
|
|
||
|
|
.macro stub name, exception
|
||
|
|
.globl \name
|
||
|
|
\name:
|
||
|
|
mrs x0, esr_el1
|
||
|
|
mrs x1, spsr_el1
|
||
|
|
mov x2, x30 // lr
|
||
|
|
mrs x3, elr_el1
|
||
|
|
mrs x4, sp_el0
|
||
|
|
mov x5, sp
|
||
|
|
mrs x6, far_el1
|
||
|
|
|
||
|
|
str x6, [sp, #-16]!
|
||
|
|
stp x4, x5, [sp, #-16]!
|
||
|
|
stp x2, x3, [sp, #-16]!
|
||
|
|
stp x0, x1, [sp, #-16]!
|
||
|
|
|
||
|
|
mov x0, #\exception
|
||
|
|
mov x1, sp
|
||
|
|
b gilbraltar_exception_handler // never returns
|
||
|
|
.endm
|
||
|
|
|
||
|
|
.text
|
||
|
|
.align 11
|
||
|
|
.globl VectorTable
|
||
|
|
VectorTable:
|
||
|
|
// from current EL with sp_el0
|
||
|
|
vector synchronous_stub
|
||
|
|
vector IRQ_stub
|
||
|
|
vector FIQ_stub
|
||
|
|
vector system_error_stub
|
||
|
|
|
||
|
|
// from current EL with sp_elx, x != 0
|
||
|
|
vector synchronous_stub
|
||
|
|
vector IRQ_stub
|
||
|
|
vector FIQ_stub
|
||
|
|
vector system_error_stub
|
||
|
|
|
||
|
|
// from lower EL, target EL minus 1 is AArch64
|
||
|
|
vector HVC_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
|
||
|
|
// from lower EL, target EL minus 1 is AArch32
|
||
|
|
vector unexpected_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
vector unexpected_stub
|
||
|
|
|
||
|
|
// Abort stubs
|
||
|
|
stub unexpected_stub, EXCEPTION_UNEXPECTED
|
||
|
|
stub synchronous_stub, EXCEPTION_SYNCHRONOUS
|
||
|
|
stub system_error_stub,EXCEPTION_SYSTEM_ERROR
|
||
|
|
|
||
|
|
// IRQ stub
|
||
|
|
.globl IRQ_stub
|
||
|
|
IRQ_stub:
|
||
|
|
stp x29, x30, [sp, #-16]! // save x29, x30 onto stack
|
||
|
|
|
||
|
|
mrs x29, elr_el1 // save elr_el1, spsr_el1 onto stack
|
||
|
|
mrs x30, spsr_el1
|
||
|
|
stp x29, x30, [sp, #-16]!
|
||
|
|
msr DAIFClr, #1 // enable FIQ
|
||
|
|
|
||
|
|
#ifdef SAVE_VFP_REGS_ON_IRQ
|
||
|
|
stp q30, q31, [sp, #-32]! // save q0-q31 onto stack
|
||
|
|
stp q28, q29, [sp, #-32]!
|
||
|
|
stp q26, q27, [sp, #-32]!
|
||
|
|
stp q24, q25, [sp, #-32]!
|
||
|
|
stp q22, q23, [sp, #-32]!
|
||
|
|
stp q20, q21, [sp, #-32]!
|
||
|
|
stp q18, q19, [sp, #-32]!
|
||
|
|
stp q16, q17, [sp, #-32]!
|
||
|
|
stp q14, q15, [sp, #-32]!
|
||
|
|
stp q12, q13, [sp, #-32]!
|
||
|
|
stp q10, q11, [sp, #-32]!
|
||
|
|
stp q8, q9, [sp, #-32]!
|
||
|
|
stp q6, q7, [sp, #-32]!
|
||
|
|
stp q4, q5, [sp, #-32]!
|
||
|
|
stp q2, q3, [sp, #-32]!
|
||
|
|
stp q0, q1, [sp, #-32]!
|
||
|
|
#endif
|
||
|
|
stp x27, x28, [sp, #-16]! // save x0-x28 onto stack
|
||
|
|
stp x25, x26, [sp, #-16]!
|
||
|
|
stp x23, x24, [sp, #-16]!
|
||
|
|
stp x21, x22, [sp, #-16]!
|
||
|
|
stp x19, x20, [sp, #-16]!
|
||
|
|
stp x17, x18, [sp, #-16]!
|
||
|
|
stp x15, x16, [sp, #-16]!
|
||
|
|
stp x13, x14, [sp, #-16]!
|
||
|
|
stp x11, x12, [sp, #-16]!
|
||
|
|
stp x9, x10, [sp, #-16]!
|
||
|
|
stp x7, x8, [sp, #-16]!
|
||
|
|
stp x5, x6, [sp, #-16]!
|
||
|
|
stp x3, x4, [sp, #-16]!
|
||
|
|
stp x1, x2, [sp, #-16]!
|
||
|
|
str x0, [sp, #-16]!
|
||
|
|
|
||
|
|
ldr x0, =IRQ_return_address // store return address for profiling
|
||
|
|
str x29, [x0]
|
||
|
|
|
||
|
|
bl gilbraltar_interrupt_handler
|
||
|
|
|
||
|
|
ldr x0, [sp], #16 // restore x0-x28 from stack
|
||
|
|
ldp x1, x2, [sp], #16
|
||
|
|
ldp x3, x4, [sp], #16
|
||
|
|
ldp x5, x6, [sp], #16
|
||
|
|
ldp x7, x8, [sp], #16
|
||
|
|
ldp x9, x10, [sp], #16
|
||
|
|
ldp x11, x12, [sp], #16
|
||
|
|
ldp x13, x14, [sp], #16
|
||
|
|
ldp x15, x16, [sp], #16
|
||
|
|
ldp x17, x18, [sp], #16
|
||
|
|
ldp x19, x20, [sp], #16
|
||
|
|
ldp x21, x22, [sp], #16
|
||
|
|
ldp x23, x24, [sp], #16
|
||
|
|
ldp x25, x26, [sp], #16
|
||
|
|
ldp x27, x28, [sp], #16
|
||
|
|
#ifdef SAVE_VFP_REGS_ON_IRQ
|
||
|
|
ldp q0, q1, [sp], #32 // restore q0-q31 from stack
|
||
|
|
ldp q2, q3, [sp], #32
|
||
|
|
ldp q4, q5, [sp], #32
|
||
|
|
ldp q6, q7, [sp], #32
|
||
|
|
ldp q8, q9, [sp], #32
|
||
|
|
ldp q10, q11, [sp], #32
|
||
|
|
ldp q12, q13, [sp], #32
|
||
|
|
ldp q14, q15, [sp], #32
|
||
|
|
ldp q16, q17, [sp], #32
|
||
|
|
ldp q18, q19, [sp], #32
|
||
|
|
ldp q20, q21, [sp], #32
|
||
|
|
ldp q22, q23, [sp], #32
|
||
|
|
ldp q24, q25, [sp], #32
|
||
|
|
ldp q26, q27, [sp], #32
|
||
|
|
ldp q28, q29, [sp], #32
|
||
|
|
ldp q30, q31, [sp], #32
|
||
|
|
#endif
|
||
|
|
|
||
|
|
msr DAIFSet, #1 // disable FIQ
|
||
|
|
ldp x29, x30, [sp], #16 // restore elr_el1, spsr_el1 from stack
|
||
|
|
msr elr_el1, x29
|
||
|
|
msr spsr_el1, x30
|
||
|
|
|
||
|
|
ldp x29, x30, [sp], #16 // restore x29, x30 from stack
|
||
|
|
|
||
|
|
eret
|
||
|
|
|
||
|
|
// FIQ stub
|
||
|
|
.globl FIQ_stub
|
||
|
|
FIQ_stub:
|
||
|
|
#ifdef SAVE_VFP_REGS_ON_FIQ
|
||
|
|
stp q30, q31, [sp, #-32]!
|
||
|
|
stp q28, q29, [sp, #-32]!
|
||
|
|
stp q26, q27, [sp, #-32]!
|
||
|
|
stp q24, q25, [sp, #-32]!
|
||
|
|
stp q22, q23, [sp, #-32]!
|
||
|
|
stp q20, q21, [sp, #-32]!
|
||
|
|
stp q18, q19, [sp, #-32]!
|
||
|
|
stp q16, q17, [sp, #-32]!
|
||
|
|
stp q14, q15, [sp, #-32]!
|
||
|
|
stp q12, q13, [sp, #-32]!
|
||
|
|
stp q10, q11, [sp, #-32]!
|
||
|
|
stp q8, q9, [sp, #-32]!
|
||
|
|
stp q6, q7, [sp, #-32]!
|
||
|
|
stp q4, q5, [sp, #-32]!
|
||
|
|
stp q2, q3, [sp, #-32]!
|
||
|
|
stp q0, q1, [sp, #-32]!
|
||
|
|
#endif
|
||
|
|
stp x29, x30, [sp, #-16]!
|
||
|
|
stp x27, x28, [sp, #-16]!
|
||
|
|
stp x25, x26, [sp, #-16]!
|
||
|
|
stp x23, x24, [sp, #-16]!
|
||
|
|
stp x21, x22, [sp, #-16]!
|
||
|
|
stp x19, x20, [sp, #-16]!
|
||
|
|
stp x17, x18, [sp, #-16]!
|
||
|
|
stp x15, x16, [sp, #-16]!
|
||
|
|
stp x13, x14, [sp, #-16]!
|
||
|
|
stp x11, x12, [sp, #-16]!
|
||
|
|
stp x9, x10, [sp, #-16]!
|
||
|
|
stp x7, x8, [sp, #-16]!
|
||
|
|
stp x5, x6, [sp, #-16]!
|
||
|
|
stp x3, x4, [sp, #-16]!
|
||
|
|
stp x1, x2, [sp, #-16]!
|
||
|
|
str x0, [sp, #-16]!
|
||
|
|
|
||
|
|
ldr x2, =FIQ_data
|
||
|
|
ldr x1, [x2] // get FIQ_data.pHandler
|
||
|
|
cmp x1, #0 // is handler set?
|
||
|
|
b.eq 2f
|
||
|
|
ldr x0, [x2, #8] // get FIQ_data.pParam
|
||
|
|
blr x1 // call handler
|
||
|
|
|
||
|
|
1:
|
||
|
|
ldr x0, [sp], #16
|
||
|
|
ldp x1, x2, [sp], #16
|
||
|
|
ldp x3, x4, [sp], #16
|
||
|
|
ldp x5, x6, [sp], #16
|
||
|
|
ldp x7, x8, [sp], #16
|
||
|
|
ldp x9, x10, [sp], #16
|
||
|
|
ldp x11, x12, [sp], #16
|
||
|
|
ldp x13, x14, [sp], #16
|
||
|
|
ldp x15, x16, [sp], #16
|
||
|
|
ldp x17, x18, [sp], #16
|
||
|
|
ldp x19, x20, [sp], #16
|
||
|
|
ldp x21, x22, [sp], #16
|
||
|
|
ldp x23, x24, [sp], #16
|
||
|
|
ldp x25, x26, [sp], #16
|
||
|
|
ldp x27, x28, [sp], #16
|
||
|
|
ldp x29, x30, [sp], #16
|
||
|
|
#ifdef SAVE_VFP_REGS_ON_FIQ
|
||
|
|
ldp q0, q1, [sp], #32
|
||
|
|
ldp q2, q3, [sp], #32
|
||
|
|
ldp q4, q5, [sp], #32
|
||
|
|
ldp q6, q7, [sp], #32
|
||
|
|
ldp q8, q9, [sp], #32
|
||
|
|
ldp q10, q11, [sp], #32
|
||
|
|
ldp q12, q13, [sp], #32
|
||
|
|
ldp q14, q15, [sp], #32
|
||
|
|
ldp q16, q17, [sp], #32
|
||
|
|
ldp q18, q19, [sp], #32
|
||
|
|
ldp q20, q21, [sp], #32
|
||
|
|
ldp q22, q23, [sp], #32
|
||
|
|
ldp q24, q25, [sp], #32
|
||
|
|
ldp q26, q27, [sp], #32
|
||
|
|
ldp q28, q29, [sp], #32
|
||
|
|
ldp q30, q31, [sp], #32
|
||
|
|
#endif
|
||
|
|
|
||
|
|
eret
|
||
|
|
|
||
|
|
2:
|
||
|
|
ldr x1, =ARM_IC_FIQ_CONTROL // disable FIQ (if handler is not set)
|
||
|
|
mov w0, #0
|
||
|
|
str w0, [x1]
|
||
|
|
b 1b
|
||
|
|
|
||
|
|
// SMC stub
|
||
|
|
.globl SMC_stub
|
||
|
|
SMC_stub:
|
||
|
|
ldr x2, =SMC_stack
|
||
|
|
mov sp, x2
|
||
|
|
str x30, [sp, #-16]!
|
||
|
|
bl gilbraltar_secure_monitor_handler
|
||
|
|
ldr x30, [sp], #16
|
||
|
|
eret
|
||
|
|
|
||
|
|
// HVC stub
|
||
|
|
HVC_stub: // return to EL2h mode
|
||
|
|
mrs x0, spsr_el2
|
||
|
|
bic x0, x0, #0xF
|
||
|
|
mov x1, #9
|
||
|
|
orr x0, x0, x1
|
||
|
|
msr spsr_el2, x0
|
||
|
|
eret
|
||
|
|
|
||
|
|
.data
|
||
|
|
.align 3
|
||
|
|
.globl FIQ_data
|
||
|
|
FIQ_data:
|
||
|
|
.quad 0 // handler
|
||
|
|
.quad 0 // param
|
||
|
|
.word 0 // FIQ_number (unused)
|
||
|
|
|
||
|
|
.align 3
|
||
|
|
.globl IRQ_return_address
|
||
|
|
IRQ_return_address:
|
||
|
|
.quad 0
|
||
|
|
|
||
|
|
.bss
|
||
|
|
.align 4
|
||
|
|
.space 128
|
||
|
|
SMC_stack:
|