1/*
2 * Copyright (c) 2014 Travis Geiselbrecht
3 *
4 * Use of this source code is governed by a MIT-style
5 * license that can be found in the LICENSE file or at
6 * https://opensource.org/licenses/MIT
7 */
8#include <lk/asm.h>
9#include <arch/asm_macros.h>
10
11/* void arm64_context_switch(vaddr_t *old_sp, vaddr_t new_sp); */
12FUNCTION(arm64_context_switch)
13    /* save old frame */
14    push x28, x29
15    push x26, x27
16    push x24, x25
17    push x22, x23
18    push x20, x21
19    push x18, x19
20    mrs  x18, tpidr_el0
21    mrs  x19, tpidrro_el0
22    push x18, x19
23    push x30, xzr
24
25    /* save old sp */
26    mov  x15, sp
27    str  x15, [x0]
28
29    /* load new sp */
30    mov  sp, x1
31
32    /* restore new frame */
33    pop  x30, xzr
34    pop  x18, x19
35    msr  tpidr_el0, x18
36    msr  tpidrro_el0, x19
37    pop  x18, x19
38    pop  x20, x21
39    pop  x22, x23
40    pop  x24, x25
41    pop  x26, x27
42    pop  x28, x29
43
44    ret
45
46    /* drop from whatever EL we may already be in to EL1.
47     * carefully avoids using x0-x3 since this is called from start.S
48     * which is trying to preserve them.
49     */
50FUNCTION(arm64_elX_to_el1)
51    mrs x4, CurrentEL
52
53    cmp x4, #(0b01 << 2)
54    bne .notEL1
55    /* Already in EL1 */
56    ret
57
58.notEL1:
59    cmp x4, #(0b10 << 2)
60    beq .inEL2
61
62    /* set EL2 to 64bit */
63    mrs x4, scr_el3
64    orr x4, x4, #(1<<10)
65    msr scr_el3, x4
66
67    /* prep this mode's ELR and SPSR to drop into EL1 */
68    adr x4, .Ltarget
69    msr elr_el3, x4
70
71    mov x4, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
72    msr spsr_el3, x4
73    b   .confEL1
74
75.inEL2:
76    /* prep this mode's ELR and SPSR to drop into EL1 */
77    adr x4, .Ltarget
78    msr elr_el2, x4
79    mov x4, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
80    msr spsr_el2, x4
81
82.confEL1:
83    /* disable EL2 coprocessor traps */
84    mov x4, #0x33ff
85    msr cptr_el2, x4
86
87    /* set EL1 to 64bit and disable EL2 instruction traps */
88    mov x4, #(1<<31)
89    msr hcr_el2, x4
90
91    /* set up the EL1 bounce interrupt */
92    mov x4, sp
93    msr sp_el1, x4
94
95    /* make sure MPIDR_EL1 and MIDR_EL1 are set with the proper values */
96    mrs x4, mpidr_el1
97    msr vmpidr_el2, x4
98    mrs x4, midr_el1
99    msr vpidr_el2, x4
100
101    isb
102    eret
103
104.Ltarget:
105    ret
106