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
46FUNCTION(arm64_el3_to_el1)
47    /* set EL2 to 64bit */
48    mrs x0, scr_el3
49    orr x0, x0, #(1<<10)
50    msr scr_el3, x0
51
52    /* set EL1 to 64bit */
53    mov x0, #(1<<31)
54    msr hcr_el2, x0
55
56    /* disable EL2 coprocessor traps */
57    mov x0, #0x33ff
58    msr cptr_el2, x0
59
60    /* disable EL1 FPU traps */
61    mov x0, #(0b11<<20)
62    msr cpacr_el1, x0
63
64    /* set up the EL1 bounce interrupt */
65    mov x0, sp
66    msr sp_el1, x0
67
68    adr x0, .Ltarget
69    msr elr_el3, x0
70
71    mov x0, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
72    msr spsr_el3, x0
73    isb
74
75    eret
76
77FUNCTION(arm64_elX_to_el1)
78    mrs x4, CurrentEL
79
80    cmp x4, #(0b01 << 2)
81    bne .notEL1
82    /* Already in EL1 */
83    ret
84
85.notEL1:
86    cmp x4, #(0b10 << 2)
87    beq .inEL2
88
89
90    /* set EL2 to 64bit */
91    mrs x4, scr_el3
92    orr x4, x4, #(1<<10)
93    msr scr_el3, x4
94
95
96    adr x4, .Ltarget
97    msr elr_el3, x4
98
99    mov x4, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
100    msr spsr_el3, x4
101    b   .confEL1
102
103.inEL2:
104    adr x4, .Ltarget
105    msr elr_el2, x4
106    mov x4, #((0b1111 << 6) | (0b0101)) /* EL1h runlevel */
107    msr spsr_el2, x4
108
109
110
111.confEL1:
112    /* disable EL2 coprocessor traps */
113    mov x0, #0x33ff
114    msr cptr_el2, x0
115
116    /* set EL1 to 64bit */
117    mov x0, #(1<<31)
118    msr hcr_el2, x0
119
120    /* disable EL1 FPU traps */
121    mov x0, #(0b11<<20)
122    msr cpacr_el1, x0
123
124    /* set up the EL1 bounce interrupt */
125    mov x0, sp
126    msr sp_el1, x0
127
128    isb
129    eret
130
131
132.Ltarget:
133    ret
134