133 lines
5.0 KiB
ArmAsm
133 lines
5.0 KiB
ArmAsm
/*
|
|
* \brief TrustZone monitor mode exception vector
|
|
* \author Stefan Kalkowski
|
|
* \date 2015-02-16
|
|
*/
|
|
|
|
/*
|
|
* Copyright (C) 2015-2017 Genode Labs GmbH
|
|
*
|
|
* This file is part of the Genode OS framework, which is distributed
|
|
* under the terms of the GNU Affero General Public License version 3.
|
|
*/
|
|
|
|
|
|
/*********************
|
|
** Constant values **
|
|
*********************/
|
|
|
|
.set RST_TYPE, 1
|
|
.set UND_TYPE, 2
|
|
.set SVC_TYPE, 3
|
|
.set PAB_TYPE, 4
|
|
.set DAB_TYPE, 5
|
|
.set IRQ_TYPE, 6
|
|
.set FIQ_TYPE, 7
|
|
.set LR_OFFSET, 14 * 4
|
|
|
|
|
|
/**
|
|
* Switch from normal into secure world
|
|
*
|
|
* \param exception_type immediate exception type ID
|
|
* \param pc_adjust immediate value that gets subtracted from the
|
|
* vm's PC before it gets saved
|
|
*/
|
|
.macro _nonsecure_to_secure exception_type, pc_adjust
|
|
stmia sp, {r0-lr}^ /* save user regs r0-r12,sp,lr */
|
|
add r0, sp, #15*4
|
|
.if \pc_adjust != 0 /* adjust pc if necessary */
|
|
sub lr, lr, #\pc_adjust
|
|
.endif
|
|
stmia r0!, {lr} /* save pc */
|
|
mrs r1, spsr /* spsr to r0 */
|
|
mov r2, #\exception_type /* exception reason to r1 */
|
|
b _nonsecure_kernel_entry
|
|
.endm /* _non_to_secure */
|
|
|
|
|
|
.section .text
|
|
.p2align 12
|
|
.global monitor_mode_exception_vector
|
|
monitor_mode_exception_vector:
|
|
b _mon_rst_entry /* reset */
|
|
b _mon_und_entry /* undefined instruction */
|
|
b _mon_svc_entry /* supervisor call */
|
|
b _mon_pab_entry /* prefetch abort */
|
|
b _mon_dab_entry /* data abort */
|
|
nop /* reserved */
|
|
b _mon_irq_entry /* interrupt request */
|
|
_nonsecure_to_secure FIQ_TYPE, 4 /* fast interrupt request */
|
|
|
|
_mon_rst_entry: _nonsecure_to_secure RST_TYPE, 0
|
|
_mon_und_entry: _nonsecure_to_secure UND_TYPE, 4
|
|
_mon_svc_entry: _nonsecure_to_secure SVC_TYPE, 0
|
|
_mon_pab_entry: _nonsecure_to_secure PAB_TYPE, 4
|
|
_mon_dab_entry: _nonsecure_to_secure DAB_TYPE, 8
|
|
_mon_irq_entry: _nonsecure_to_secure IRQ_TYPE, 4
|
|
|
|
_nonsecure_kernel_entry:
|
|
ldr lr, [sp, #17*4] /* load kernel sp from vm context */
|
|
stmia r0!, {r1-r2} /* save spsr, and exception reason */
|
|
mov r1, #1 /* clear exception state of the VFP */
|
|
lsl r1, #30
|
|
vmsr fpexc, r1
|
|
mrc p15, 0, r3, c6, c0, 0 /* move DFAR to r3 */
|
|
mrc p15, 0, r4, c2, c0, 0 /* move TTBR0 to r4 */
|
|
mrc p15, 0, r5, c2, c0, 1 /* move TTBR1 to r5 */
|
|
mrc p15, 0, r6, c2, c0, 2 /* move TTBRC to r6 */
|
|
vmrs r7, fpscr /* move FPU ctrl to r7 */
|
|
mov r1, #0
|
|
mcr p15, 0, r1, c1, c1, 0 /* disable non-secure bit */
|
|
.irp mode,27,19,23,18,17 /* save mode specific registers */
|
|
cps #\mode /* switch to given mode */
|
|
mrs r1, spsr /* store mode-specific spsr */
|
|
stmia r0!, {r1,sp,lr} /* store mode-specific sp and lr */
|
|
.endr
|
|
stmia r0!, {r8-r12} /* save fiq r8-r12 */
|
|
stmia r0!, {r3-r7} /* save MMU registers */
|
|
vstm r0!, {d0-d15} /* save FPU registers */
|
|
vstm r0!, {d16-d31}
|
|
cps #22 /* switch back to monitor mode */
|
|
mov r0, #0b111010011 /* spsr to SVC mode, irqs masked */
|
|
msr spsr_cxsf, r0
|
|
mov r1, lr
|
|
cps #19
|
|
mov sp, r1
|
|
cps #22
|
|
adr lr, _kernel_entry
|
|
ldr lr, [lr]
|
|
subs pc, lr, #0 /* jump back into kernel */
|
|
|
|
_kernel_entry: .long kernel
|
|
|
|
|
|
/* jump to this point to switch to TrustZone's normal world */
|
|
.global monitor_mode_enter_normal_world
|
|
monitor_mode_enter_normal_world:
|
|
cps #22 /* switch to monitor mode */
|
|
mov sp, r0 /* store vm context pointer */
|
|
mov lr, r1 /* store kernel sp temporarily */
|
|
add r0, r0, #18*4 /* add offset of banked modes */
|
|
.irp mode,27,19,23,18,17 /* save mode specific registers */
|
|
cps #\mode /* switch to given mode */
|
|
ldmia r0!, {r2,sp,lr} /* load mode's sp, lr, and spsr */
|
|
msr spsr_cxfs, r2 /* load mode's spsr */
|
|
.endr
|
|
ldmia r0!, {r8 - r12} /* load fiq r8-r12 */
|
|
add r0, r0, #4*4
|
|
ldr r1, [r0]
|
|
vmsr fpscr, r1
|
|
add r1, r0, #4
|
|
vldm r1!, {d0-d15}
|
|
vldm r1!, {d16-d31}
|
|
cps #22 /* switch to monitor mode */
|
|
ldmia sp, {r0-lr}^ /* load user r0-r12,sp,lr */
|
|
str lr, [sp, #17*4] /* store kernel sp in vm context */
|
|
ldr lr, [sp, #16*4] /* load vm's cpsr to lr */
|
|
msr spsr_cxfs, lr /* save cpsr to be load */
|
|
mov lr, #13
|
|
mcr p15, 0, lr, c1, c1, 0 /* enable EA, FIQ, NS bit in SCTRL */
|
|
ldr lr, [sp, #15*4] /* load vm's ip */
|
|
subs pc, lr, #0
|