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

use std::collections::HashMap;

use thiserror::Error;

use crate::framebuffer::AsFrameBuffer;

#[derive(Debug, Error)]
pub enum MemoryMappedFrameBufferError {
    #[error("Plane {index} with offset {offset} and size {len} exceeds file descriptor size of {fd_len}")]
    PlaneOutOfBounds {
        index: usize,
        offset: usize,
        len: usize,
        fd_len: usize,
    },
    #[error("mmap failed with {0:?}")]
    MemoryMapError(std::io::Error),
}

struct MappedPlane {
    fd: i32,
    offset: usize,
    len: usize,
}

/// FrameBuffer wrapper, which exposes internal file descriptors as memory mapped [&[u8]] plane slices.
pub struct MemoryMappedFrameBuffer<T: AsFrameBuffer> {
    fb: T,
    mmaps: HashMap<i32, (*const core::ffi::c_void, usize)>,
    planes: Vec<MappedPlane>,
}

impl<T: AsFrameBuffer> MemoryMappedFrameBuffer<T> {
    /// Memory map framebuffer, which implements [AsFrameBuffer].
    ///
    /// This might fail if framebuffer has invalid plane sizes/offsets or if [libc::mmap] fails itself.
    pub fn new(fb: T) -> Result<Self, MemoryMappedFrameBufferError> {
        struct MapInfo {
            /// Maximum offset used by data planes
            mapped_len: usize,
            /// Total file descriptor size
            total_len: usize,
        }

        let mut planes = Vec::new();
        let mut map_info: HashMap<i32, MapInfo> = HashMap::new();

        for (index, plane) in fb.planes().into_iter().enumerate() {
            let fd = plane.fd();
            let offset = plane.offset().unwrap();
            let len = plane.len();

            planes.push(MappedPlane { fd, offset, len });

            // Find total FD length if not known yet
            map_info.entry(fd).or_insert_with(|| {
                let total_len = unsafe { libc::lseek64(fd, 0, libc::SEEK_END) } as usize;
                MapInfo {
                    mapped_len: 0,
                    total_len,
                }
            });

            let info = map_info.get_mut(&fd).unwrap();

            if offset + len > info.total_len {
                return Err(MemoryMappedFrameBufferError::PlaneOutOfBounds {
                    index,
                    offset,
                    len,
                    fd_len: info.total_len,
                });
            }

            info.mapped_len = info.mapped_len.max(offset + len);
        }

        let mmaps = map_info
            .iter()
            .map(|(fd, info)| {
                let addr = unsafe {
                    libc::mmap64(
                        core::ptr::null_mut(),
                        info.mapped_len,
                        libc::PROT_READ,
                        libc::MAP_SHARED,
                        *fd,
                        0,
                    )
                };

                if addr == libc::MAP_FAILED {
                    Err(MemoryMappedFrameBufferError::MemoryMapError(
                        std::io::Error::last_os_error(),
                    ))
                } else {
                    Ok((*fd, (addr.cast_const(), info.mapped_len)))
                }
            })
            .collect::<Result<HashMap<i32, (*const core::ffi::c_void, usize)>, MemoryMappedFrameBufferError>>()
            .unwrap();

        Ok(Self { fb, mmaps, planes })
    }

    /// Returns data slice for each plane within the framebuffer.
    pub fn data(&self) -> Vec<&[u8]> {
        self.planes
            .iter()
            .map(|plane| {
                let mmap_ptr: *const u8 = self.mmaps[&plane.fd].0.cast();
                unsafe { core::slice::from_raw_parts(mmap_ptr.add(plane.offset), plane.len) }
            })
            .collect()
    }
}

impl<T: AsFrameBuffer> AsFrameBuffer for MemoryMappedFrameBuffer<T> {
    unsafe fn ptr(&self) -> std::ptr::NonNull<libcamera_sys::libcamera_framebuffer_t> {
        self.fb.ptr()
    }
}

unsafe impl<T: AsFrameBuffer> Send for MemoryMappedFrameBuffer<T> {}

impl<T: AsFrameBuffer> Drop for MemoryMappedFrameBuffer<T> {
    fn drop(&mut self) {
        // Unmap
        for (_fd, (ptr, size)) in self.mmaps.drain() {
            unsafe {
                libc::munmap(ptr.cast_mut(), size);
            }
        }
    }
}