| 1 | #include <lib/macros.aarch64_asm.h> |
| 2 | |
| 3 | .set tpl_ap_el, -72 |
| 4 | .set tpl_booted_flag, -64 |
| 5 | .set tpl_hhdm_offset, -56 |
| 6 | .set tpl_ttbr0, -48 |
| 7 | .set tpl_ttbr1, -40 |
| 8 | .set tpl_mair, -32 |
| 9 | .set tpl_tcr, -24 |
| 10 | .set tpl_sctlr, -16 |
| 11 | .set tpl_info_struct, -8 |
| 12 | |
| 13 | .section .text |
| 14 | |
| 15 | .global smp_trampoline_start |
| 16 | smp_trampoline_start: |
| 17 | bl .L_entry |
| 18 | .L_entry: |
| 19 | // Mask IRQs |
| 20 | msr daifset, #0b1111 |
| 21 | |
| 22 | // Address to next page (since our offsets into the boot data are negative) |
| 23 | add x1, x30, #0xFFC |
| 24 | |
| 25 | ldr x0, [x1, tpl_info_struct] |
| 26 | ldr x2, [x1, tpl_sctlr] |
| 27 | ldr x3, [x1, tpl_mair] |
| 28 | ldr x4, [x1, tpl_tcr] |
| 29 | ldr x5, [x1, tpl_ttbr0] |
| 30 | ldr x6, [x1, tpl_ttbr1] |
| 31 | ldr x7, [x1, tpl_hhdm_offset] |
| 32 | |
| 33 | PICK_EL x8, 1f, 0f |
| 34 | 0: |
| 35 | // EL2 path - enable VHE and stay at EL2 |
| 36 | |
| 37 | // Enable E2H if not already set |
| 38 | mrs x8, hcr_el2 |
| 39 | orr x8, x8, #(1 << 34) |
| 40 | msr hcr_el2, x8 |
| 41 | isb |
| 42 | |
| 43 | // Configure EL2 state for VHE |
| 44 | msr spsel, #0 |
| 45 | |
| 46 | mov x8, #3 |
| 47 | msr cnthctl_el2, x8 |
| 48 | msr cntvoff_el2, xzr |
| 49 | |
| 50 | // HCR: E2H + TGE + RW + SWIO |
| 51 | mov x8, xzr |
| 52 | orr x8, x8, #(1 << 34) |
| 53 | orr x8, x8, #(1 << 31) |
| 54 | orr x8, x8, #(1 << 27) |
| 55 | orr x8, x8, #(1 << 1) |
| 56 | msr hcr_el2, x8 |
| 57 | |
| 58 | // Disable FP/SIMD/SVE (VHE CPTR_EL2 layout, CPACR-like) |
| 59 | msr cptr_el2, xzr |
| 60 | msr hstr_el2, xzr |
| 61 | |
| 62 | // Set up VBAR for page table switch (VHE redirects VBAR_EL1 to VBAR_EL2) |
| 63 | adrp x8, 2f |
| 64 | add x8, x8, :lo12:2f |
| 65 | add x8, x8, x7 |
| 66 | msr vbar_el1, x8 |
| 67 | isb |
| 68 | dsb sy |
| 69 | isb |
| 70 | |
| 71 | // Switch page tables using VHE-redirected register names |
| 72 | msr mair_el1, x3 |
| 73 | msr tcr_el1, x4 |
| 74 | msr ttbr0_el1, x5 |
| 75 | msr ttbr1_el1, x6 |
| 76 | msr sctlr_el1, x2 |
| 77 | isb |
| 78 | dsb sy |
| 79 | isb |
| 80 | |
| 81 | // Jump to the higher half mapping in case we didn't immediately crash |
| 82 | br x8 |
| 83 | |
| 84 | 1: |
| 85 | // EL1 path |
| 86 | msr spsel, #0 |
| 87 | |
| 88 | // Switch to the new page tables |
| 89 | |
| 90 | // Point the EL1t handler to the continuation, such that after we page fault, |
| 91 | // execution continues as expected. |
| 92 | adrp x8, 2f |
| 93 | add x8, x8, #:lo12:2f |
| 94 | add x8, x8, x7 |
| 95 | msr vbar_el1, x8 |
| 96 | isb |
| 97 | dsb sy |
| 98 | isb |
| 99 | |
| 100 | // Switch the page table registers |
| 101 | msr mair_el1, x3 |
| 102 | msr tcr_el1, x4 |
| 103 | msr ttbr0_el1, x5 |
| 104 | msr ttbr1_el1, x6 |
| 105 | msr sctlr_el1, x2 |
| 106 | isb |
| 107 | dsb sy |
| 108 | isb |
| 109 | |
| 110 | // Jump to the higher half mapping in case we didn't immediately crash |
| 111 | br x8 |
| 112 | |
| 113 | // Alignment required by VBAR register |
| 114 | .align 12 |
| 115 | 2: |
| 116 | // Zero out VBAR to avoid confusion |
| 117 | msr vbar_el1, xzr |
| 118 | |
| 119 | // Disable FP/SIMD/SVE |
| 120 | msr cpacr_el1, xzr |
| 121 | |
| 122 | 3: |
| 123 | // Add HHDM offset to data pointer |
| 124 | add x1, x1, x7 |
| 125 | |
| 126 | // Report our EL to the BSP |
| 127 | mrs x8, currentel |
| 128 | lsr x8, x8, #2 |
| 129 | add x9, x1, tpl_ap_el |
| 130 | stlr x8, [x9] |
| 131 | |
| 132 | // Notify BSP we are alive |
| 133 | mov x8, #1 |
| 134 | add x9, x1, tpl_booted_flag |
| 135 | stlr x8, [x9] |
| 136 | |
| 137 | // Wait for BSP to tell us where to go |
| 138 | // Add HHDM offset to our info struct pointer |
| 139 | add x0, x0, x7 |
| 140 | add x9, x0, #24 |
| 141 | 4: |
| 142 | ldar x8, [x9] |
| 143 | cbnz x8, 5f |
| 144 | yield |
| 145 | b 4b |
| 146 | |
| 147 | 5: |
| 148 | msr elr_el1, x8 |
| 149 | |
| 150 | msr spsel, #0 |
| 151 | ldr x8, [x0, #16] |
| 152 | mov sp, x8 |
| 153 | |
| 154 | // Enter kernel - determine SPSR from current EL |
| 155 | mov x8, #0x3c4 |
| 156 | mrs x9, currentel |
| 157 | cmp x9, #0b1000 |
| 158 | b.ne 9f |
| 159 | mov x8, #0x3c8 |
| 160 | 9: |
| 161 | msr spsr_el1, x8 |
| 162 | |
| 163 | ZERO_REGS_EXCEPT_X0 |
| 164 | |
| 165 | eret |
| 166 | |
| 167 | smp_trampoline_end: |
| 168 | |
| 169 | .section .rodata |
| 170 | |
| 171 | .global smp_trampoline_size |
| 172 | smp_trampoline_size: |
| 173 | .quad smp_trampoline_end - smp_trampoline_start |
| 174 | |
| 175 | .section .note.GNU-stack,"",%progbits |
