esp-rtos 0.3.0

A task scheduler for Espressif devices
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

#[cfg(feature = "alloc")]
use core::ffi::c_void;

#[cfg(feature = "alloc")]
use esp_rom_sys::SYSCALL_TABLE;

#[cfg(feature = "alloc")]
extern "C" fn _getreent() -> *mut esp_rom_sys::_reent {
    use allocator_api2::boxed::Box;

    use crate::SCHEDULER;

    // SAFETY: `current_task` always returns a valid pointer to the current task.
    let task = unsafe { SCHEDULER.current_task().as_mut() };
    task.thread_local
        .reent
        .get_or_insert_with(|| unsafe {
            // Safety: _reent is a C-type containing integers and pointers, it is safe to
            // zero-initialize it.
            Box::new_zeroed().assume_init()
        })
        .as_mut() as *mut _
}

#[cfg(feature = "esp-alloc")]
#[repr(C)]
struct AllocationHeader {
    size: usize,
}

#[cfg(feature = "esp-alloc")]
extern "C" fn _malloc_r(_reent: *mut esp_rom_sys::_reent, size: usize) -> *mut c_void {
    use alloc::alloc::alloc;
    use core::{alloc::Layout, mem, ptr};

    let header_layout = Layout::new::<AllocationHeader>();
    let user_layout = match Layout::from_size_align(size, mem::align_of::<AllocationHeader>()) {
        Ok(l) => l,
        Err(_) => return ptr::null_mut(),
    };

    let (layout, user_offset) = match header_layout.extend(user_layout) {
        Ok(v) => v,
        Err(_) => return ptr::null_mut(),
    };

    unsafe {
        let p = alloc(layout);
        if p.is_null() {
            return ptr::null_mut();
        }

        let header_ptr = p as *mut AllocationHeader;
        header_ptr.write(AllocationHeader { size });

        let user_data_ptr = p.add(user_offset);

        debug!(
            "Alloc called for {} bytes, returned {:?}",
            size, user_data_ptr
        );
        user_data_ptr as *mut c_void
    }
}

#[cfg(feature = "esp-alloc")]
extern "C" fn _free_r(_reent: *mut esp_rom_sys::_reent, ptr: *mut c_void) {
    use alloc::alloc::dealloc;
    use core::{alloc::Layout, mem};

    if ptr.is_null() {
        return;
    }

    debug!("Freeing {:?}", ptr);
    unsafe {
        let user_ptr = ptr as *mut u8;
        let header_layout = Layout::new::<AllocationHeader>();
        let header_ptr = user_ptr.sub(header_layout.size()) as *mut AllocationHeader;
        let header = header_ptr.read();

        let user_layout =
            Layout::from_size_align_unchecked(header.size, mem::align_of::<AllocationHeader>());
        let (layout, _offset) = header_layout.extend(user_layout).unwrap();

        let p = header_ptr as *mut u8;
        dealloc(p, layout);
    }
}

#[cfg(all(feature = "alloc", not(feature = "esp-alloc")))]
unsafe extern "C" {
    fn malloc_internal(size: usize) -> *mut u8;
}

#[cfg(all(feature = "alloc", not(feature = "esp-alloc")))]
unsafe extern "C" {
    fn free_internal(ptr: *mut u8);
}

#[cfg(all(feature = "alloc", not(feature = "esp-alloc")))]
extern "C" fn _malloc_r(_reent: *mut esp_rom_sys::_reent, size: usize) -> *mut c_void {
    unsafe { malloc_internal(size) as *mut c_void }
}

#[cfg(all(feature = "alloc", not(feature = "esp-alloc")))]
extern "C" fn _free_r(_reent: *mut esp_rom_sys::_reent, ptr: *mut c_void) {
    unsafe { free_internal(ptr as *mut u8) }
}

pub fn setup_syscalls() {
    #[cfg(feature = "alloc")]
    unsafe {
        SYSCALL_TABLE.__getreent = Some(_getreent);
        SYSCALL_TABLE._malloc_r = Some(_malloc_r);
        SYSCALL_TABLE._free_r = Some(_free_r);
    }
}