1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

use core::arch::asm;
use core::fmt;
use core::ops;
use gimli::{AArch64, Register};

// Match DWARF_FRAME_REGISTERS in libgcc
pub const MAX_REG_RULES: usize = 97;

#[repr(C)]
#[derive(Clone, Default)]
pub struct Context {
    pub gp: [usize; 31],
    pub sp: usize,
    pub fp: [usize; 32],
}

impl fmt::Debug for Context {
    fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
        let mut fmt = fmt.debug_struct("Context");
        for i in 0..=30 {
            fmt.field(
                AArch64::register_name(Register(i as _)).unwrap(),
                &self.gp[i],
            );
        }
        fmt.field("sp", &self.sp);
        for i in 0..=31 {
            fmt.field(
                AArch64::register_name(Register((i + 64) as _)).unwrap(),
                &self.fp[i],
            );
        }
        fmt.finish()
    }
}

impl ops::Index<Register> for Context {
    type Output = usize;

    fn index(&self, reg: Register) -> &usize {
        match reg {
            Register(0..=30) => &self.gp[reg.0 as usize],
            AArch64::SP => &self.sp,
            Register(64..=95) => &self.fp[(reg.0 - 64) as usize],
            _ => unimplemented!(),
        }
    }
}

impl ops::IndexMut<gimli::Register> for Context {
    fn index_mut(&mut self, reg: Register) -> &mut usize {
        match reg {
            Register(0..=30) => &mut self.gp[reg.0 as usize],
            AArch64::SP => &mut self.sp,
            Register(64..=95) => &mut self.fp[(reg.0 - 64) as usize],
            _ => unimplemented!(),
        }
    }
}

macro_rules! save {
    (gp$(, $fp:ident)?) => {
        // No need to save caller-saved registers here.
        asm!(
            "
            stp x29, x30, [sp, -16]!
            sub sp, sp, 512
            mov x8, x0
            mov x0, sp
            ",
            save!(maybesavefp($($fp)?)),
            "
            str x19, [sp, 0x98]
            stp x20, x21, [sp, 0xA0]
            stp x22, x23, [sp, 0xB0]
            stp x24, x25, [sp, 0xC0]
            stp x26, x27, [sp, 0xD0]
            stp x28, x29, [sp, 0xE0]
            add x2, sp, 528
            stp x30, x2, [sp, 0xF0]

            blr x8

            add sp, sp, 512
            ldp x29, x30, [sp], 16
            ret
            ",
            options(noreturn)
        );
    };
    (maybesavefp(fp)) => {
        "
        stp d8, d9, [sp, 0x140]
        stp d10, d11, [sp, 0x150]
        stp d12, d13, [sp, 0x160]
        stp d14, d15, [sp, 0x170]
        "
    };
    (maybesavefp()) => { "" };
}

#[naked]
pub extern "C-unwind" fn save_context(f: extern "C" fn(&mut Context, *mut ()), ptr: *mut ()) {
    unsafe {
        #[cfg(target_feature = "neon")]
        save!(gp, fp);
        #[cfg(not(target_feature = "neon"))]
        save!(gp);
    }
}

macro_rules! restore {
    ($ctx:expr, gp$(, $fp:ident)?) => {
        asm!(
            restore!(mayberestore($($fp)?)),
            "
            ldp x2, x3, [x0, 0x10]
            ldp x4, x5, [x0, 0x20]
            ldp x6, x7, [x0, 0x30]
            ldp x8, x9, [x0, 0x40]
            ldp x10, x11, [x0, 0x50]
            ldp x12, x13, [x0, 0x60]
            ldp x14, x15, [x0, 0x70]
            ldp x16, x17, [x0, 0x80]
            ldp x18, x19, [x0, 0x90]
            ldp x20, x21, [x0, 0xA0]
            ldp x22, x23, [x0, 0xB0]
            ldp x24, x25, [x0, 0xC0]
            ldp x26, x27, [x0, 0xD0]
            ldp x28, x29, [x0, 0xE0]
            ldp x30, x1, [x0, 0xF0]
            mov sp, x1

            ldp x0, x1, [x0, 0x00]
            ret
            ",
            in("x0") $ctx,
            options(noreturn)
        );
    };
    (mayberestore(fp)) => {
        "
        ldp d0, d1, [x0, 0x100]
        ldp d2, d3, [x0, 0x110]
        ldp d4, d5, [x0, 0x120]
        ldp d6, d7, [x0, 0x130]
        ldp d8, d9, [x0, 0x140]
        ldp d10, d11, [x0, 0x150]
        ldp d12, d13, [x0, 0x160]
        ldp d14, d15, [x0, 0x170]
        ldp d16, d17, [x0, 0x180]
        ldp d18, d19, [x0, 0x190]
        ldp d20, d21, [x0, 0x1A0]
        ldp d22, d23, [x0, 0x1B0]
        ldp d24, d25, [x0, 0x1C0]
        ldp d26, d27, [x0, 0x1D0]
        ldp d28, d29, [x0, 0x1E0]
        ldp d30, d31, [x0, 0x1F0]
        "
    };
    (mayberestore()) => { "" };
}

pub unsafe fn restore_context(ctx: &Context) -> ! {
    unsafe {
        #[cfg(target_feature = "neon")]
        restore!(ctx, gp, fp);
        #[cfg(not(target_feature = "neon"))]
        restore!(ctx, gp);
    }
}