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

//
// Copyright 2024, Colias Group, LLC
//
// SPDX-License-Identifier: BSD-2-Clause
//

use core::arch::asm;
use core::ptr;

use cfg_if::cfg_if;

use crate::TlsImage;

mod reserve;

use reserve::{reserve_on_stack, ReserveOnStackContArg};

pub type ContFn = unsafe extern "C" fn(cont_arg: *mut ContArg) -> !;

pub enum ContArg {}

impl TlsImage {
    #[allow(clippy::missing_safety_doc)]
    pub unsafe fn initialize_on_stack(
        &self,
        set_thread_pointer_fn: SetThreadPointerFn,
        cont_fn: ContFn,
        cont_arg: *mut ContArg,
    ) -> ! {
        let mut cont_arg = InternalContArg {
            tls_image: ptr::addr_of!(*self),
            set_thread_pointer_fn,
            cont_fn,
            cont_arg,
        };
        reserve_on_stack(
            self.reservation_layout().footprint(),
            reserve_on_stack_cont_fn,
            ptr::addr_of_mut!(cont_arg).cast(),
        )
    }

    unsafe fn continue_with(
        &self,
        tls_reservation_start: *mut u8,
        set_thread_pointer_fn: SetThreadPointerFn,
        cont_fn: ContFn,
        cont_arg: *mut ContArg,
    ) -> ! {
        self.initialize_tls_reservation(tls_reservation_start);

        let thread_pointer = tls_reservation_start
            .wrapping_byte_add(self.reservation_layout().thread_pointer_offset());

        (set_thread_pointer_fn)(thread_pointer as usize);

        (cont_fn)(cont_arg)
    }
}

#[repr(C)]
#[derive(Debug, Clone, PartialEq, Eq)]
struct InternalContArg {
    tls_image: *const TlsImage,
    set_thread_pointer_fn: SetThreadPointerFn,
    cont_fn: ContFn,
    cont_arg: *mut ContArg,
}

unsafe extern "C" fn reserve_on_stack_cont_fn(
    reservation_start: *mut u8,
    arg: *mut ReserveOnStackContArg,
) -> ! {
    let arg = arg.cast::<InternalContArg>().as_ref().unwrap();
    let tls_image = arg.tls_image.as_ref().unwrap();
    tls_image.continue_with(
        reservation_start,
        arg.set_thread_pointer_fn,
        arg.cont_fn,
        arg.cont_arg,
    )
}

pub type SetThreadPointerFn = unsafe extern "C" fn(thread_pointer: usize);

pub const DEFAULT_SET_THREAD_POINTER_FN: SetThreadPointerFn = default_set_thread_pointer;

unsafe extern "C" fn default_set_thread_pointer(thread_pointer: usize) {
    let val = thread_pointer;

    cfg_if! {
        if #[cfg(target_arch = "aarch64")] {
            asm!("msr tpidr_el0, {val}", val = in(reg) val);
        } else if #[cfg(target_arch = "arm")] {
            asm!("mcr p15, 0, {val}, c13, c0, 2", val = in(reg) val); // tpidrurw
        } else if #[cfg(any(target_arch = "riscv32", target_arch = "riscv64"))] {
            asm!("mv tp, {val}", val = in(reg) val);
        } else if #[cfg(target_arch = "x86_64")] {
            asm!("wrfsbase {val}", val = in(reg) val);
        } else {
            compile_error!("unsupported architecture");
        }
    }
}