104 lines
1.8 KiB
Plaintext
104 lines
1.8 KiB
Plaintext
#include <lib/macros.aarch64_asm.h>
|
|
|
|
.set tpl_booted_flag, -56
|
|
.set tpl_ttbr0, -48
|
|
.set tpl_ttbr1, -40
|
|
.set tpl_mair, -32
|
|
.set tpl_tcr, -24
|
|
.set tpl_sctlr, -16
|
|
.set tpl_info_struct, -8
|
|
|
|
.section .text
|
|
|
|
.global smp_trampoline_start
|
|
smp_trampoline_start:
|
|
bl .L_entry
|
|
.L_entry:
|
|
// Mask IRQs
|
|
msr daifset, #0b1111
|
|
|
|
// Address to next page (since our offsets into the boot data are negative)
|
|
add x1, x30, #0xFFC
|
|
|
|
ldr x0, [x1, tpl_info_struct]
|
|
ldr x2, [x1, tpl_sctlr]
|
|
ldr x3, [x1, tpl_mair]
|
|
ldr x4, [x1, tpl_tcr]
|
|
ldr x5, [x1, tpl_ttbr0]
|
|
ldr x6, [x1, tpl_ttbr1]
|
|
|
|
// Configure EL1 state
|
|
msr mair_el1, x3
|
|
msr tcr_el1, x4
|
|
msr ttbr0_el1, x5
|
|
msr ttbr1_el1, x6
|
|
msr sctlr_el1, x2
|
|
dsb sy
|
|
isb
|
|
|
|
PICK_EL x8, 1f, 0f
|
|
0:
|
|
// Configure EL2-specific state for EL1
|
|
|
|
// Don't trap counters to EL2
|
|
mrs x8, cnthctl_el2
|
|
orr x8, x8, #3
|
|
msr cnthctl_el2, x8
|
|
msr cntvoff_el2, xzr
|
|
|
|
// Enable AArch64 in EL1
|
|
mov x8, xzr
|
|
orr x8, x8, #(1 << 31)
|
|
orr x8, x8, #(1 << 1)
|
|
msr hcr_el2, x8
|
|
|
|
// Don't trap FP/SIMD to EL2
|
|
mov x8, #0x33FF
|
|
msr cptr_el2, x8
|
|
msr hstr_el2, xzr
|
|
|
|
// Run rest of trampoline in EL1
|
|
mov x8, #0x3c4
|
|
msr spsr_el2, x8
|
|
adr x8, 1f
|
|
msr elr_el2, x8
|
|
|
|
eret
|
|
|
|
1:
|
|
// Notify BSP we are alive
|
|
mov x8, #1
|
|
add x9, x1, tpl_booted_flag
|
|
stlr x8, [x9]
|
|
|
|
// Wait for BSP to tell us where to go
|
|
add x9, x0, #24
|
|
2:
|
|
ldar x8, [x9]
|
|
cbnz x8, 3f
|
|
yield
|
|
b 2b
|
|
|
|
3:
|
|
msr elr_el1, x8
|
|
|
|
msr spsel, #0
|
|
ldr x8, [x0, #16]
|
|
mov sp, x8
|
|
|
|
// Enter kernel
|
|
mov x8, #0x3c4
|
|
msr spsr_el1, x8
|
|
|
|
ZERO_REGS_EXCEPT_X0
|
|
|
|
eret
|
|
|
|
smp_trampoline_end:
|
|
|
|
.section .rodata
|
|
|
|
.global smp_trampoline_size
|
|
smp_trampoline_size:
|
|
.quad smp_trampoline_end - smp_trampoline_start
|