shared_mem_queue/

byte_queue.rs

// Copyright Open Logistics Foundation
//
// Licensed under the Open Logistics Foundation License 1.3.
// For details on the licensing terms, see the LICENSE file.
// SPDX-License-Identifier: OLFL-1.3

//! FIFO queue with a byte-oriented interface
//!
//! The `ByteQueue` operates on a shared memory region and keeps track of a write-pointer and a
//! read-pointer. To access both pointers from both processors, the pointers are stored in the
//! shared memory region itself so the capacity of the queue is `2*size_of::<u32>()` smaller than
//! the memory region size.
//!
//! When initializing the `ByteQueue` from an array buffer, the buffer should be defined with the
//! type `u32` to ensure alignment regardless of its size, and then cast into an `u8` pointer when
//! passed into the `ByteQueue` constructor. Additionally, the size parameter for the constructor
//! needs to be adjusted accordingly, i.e. it is 4 times larger than the array length.
//!
//! The main contract for the `ByteQueue` is that only the writer may write to the
//! write-pointer, only the reader may change the read-pointer. The memory
//! region in front of the write-pointer and up to the read-pointer is owned by
//! the writer (it may be changed by the writer), the memory region in front of the read-pointer
//! and up to the write-pointer is owned by the reader (it may not be changed by the writer and can
//! safely be read by the reader). For initialization, both pointers have
//! to be set to 0 at the beginning. This is in contrast to the contract above because the
//! initializing processor needs to write both pointers. Therefore, this has to be done by processor
//! A while it is guaranteed that processor B does not access the queue yet to prevent race
//! conditions.
//!
//! Because processor A has to initialize the byte queue and processor B should not
//! reset the write- and read-pointers, there are two methods for
//! initialization: [`ByteQueue::create`] should be called by the first processor and
//! sets both pointers to 0, [`ByteQueue::attach`] should be called by the second one.
//!
//! The `ByteQueue` implements both the write- and the read-methods but
//! each processor should have either the writing side or the reading side
//! assigned to it and must not call the other methods. It would also be
//! possible to have a `SharedMemWriter` and a `SharedMemReader` but this
//! design was initially chosen so that the queue can also be used as a simple
//! ring buffer on a single processor.

use core::convert::{Infallible, TryFrom, TryInto};
use core::ptr::read_volatile;
use core::ptr::write_volatile;
use core::sync::atomic;
use core::sync::atomic::Ordering;

/// The `ByteQueue` queue type. Read the crate and module documentation for further information and
/// usage examples.
#[derive(Debug)]
pub struct ByteQueue {
    write_pos_ptr: *mut u32,
    read_pos_ptr: *mut u32,
    data_ptr: *mut u8,
    capacity: usize,
}

/// The `ByteQueue` is not automatically `Send` because it contains raw pointers. According to
/// [the Nomicon](https://doc.rust-lang.org/1.81.0/nomicon/send-and-sync.html), raw pointers
/// could be considered "fine [...] to be marked as thread safe" but their "non-trivial untracked
/// ownership" requires to decide manually if a type containing raw pointers is `Send`.
///
/// Regarding the `ByteQueue`, every instantiation and usage is so unsafe that the user needs to be
/// careful anyway. If all usage requirements are still met, the `ByteQueue` can safely be used
/// from another thread, too. Therefore, `Send` is implemented manually here to increase the
/// flexibility for the users.
///
/// An other perspective on implementing `Send` is that the `ByteQueue` is fundamentally designed
/// to be used for inter-processor communication which is in many ways equivalent to inter-thread
/// operation. Thus, implementing `Send` does not introduce any new requirements.
unsafe impl Send for ByteQueue {}

impl ByteQueue {
    /// Creates a new queue in the given memory region and initializes both pointers.
    ///
    /// # Safety
    /// This method has to be called before the other processor tries to access the queue because
    /// the other processor might access an uninitialized memory region otherwise which will most
    /// likely result in crashes.
    ///
    /// Obviously, the memory pointer and the memory region length must be correct, reserved for
    /// this purpose and known to the other processor.
    pub unsafe fn create(mem: *mut u8, mem_len: usize) -> Self {
        let mut slf = Self::attach(mem, mem_len);
        slf.set_write_pos(0);
        slf.set_read_pos(0);
        slf
    }
    /// Attaches to a queue which has previously been initialized by [`ByteQueue::create`],
    /// possibly by an other processor.
    ///
    /// # Safety
    /// This method must not be called before the other processor has properly initialized the
    /// queue because this will most likely result in crashes.
    ///
    /// Obviously, the memory pointer rand the memory region length must be correct, reserved for
    /// this purpose and known to the other processor.
    pub unsafe fn attach(mem: *mut u8, mem_len: usize) -> Self {
        ByteQueue {
            write_pos_ptr: mem as *mut u32,
            read_pos_ptr: (mem as *mut u32).offset(1),
            data_ptr: mem.offset(
                isize::try_from(2 * core::mem::size_of::<u32>())
                    .expect("~8u should be convertible to isize"),
            ),
            capacity: mem_len - 2 * core::mem::size_of::<u32>(),
        }
    }
    fn get_write_pos(&self) -> usize {
        unsafe { read_volatile(self.write_pos_ptr) as usize }
    }
    fn get_read_pos(&self) -> usize {
        unsafe { read_volatile(self.read_pos_ptr) as usize }
    }
    fn set_write_pos(&mut self, wpos: usize) {
        unsafe {
            write_volatile(
                self.write_pos_ptr,
                wpos.try_into().expect("cannot convert usize into u32"),
            )
        }
    }
    fn set_read_pos(&mut self, rpos: usize) {
        unsafe {
            write_volatile(
                self.read_pos_ptr,
                rpos.try_into().expect("cannot convert usize into u32"),
            )
        }
    }

    /// Returns the size of the available space, which can be written into the queue.
    pub fn space(&self) -> usize {
        (self.capacity + self.get_read_pos() - self.get_write_pos() - 1) % self.capacity
    }

    pub fn capacity(&self) -> usize {
        self.capacity - 1
    }

    /// Returns the size of the written messages, which are to be consumed or read.
    pub fn size(&self) -> usize {
        (self.capacity + self.get_write_pos() - self.get_read_pos()) % self.capacity
    }

    //
    // Write methods
    //

    /// Writes at most `len` bytes into the byte queue, or less depending on the given size of
    /// the data to be written, *and* the currently available space in the byte queue.
    ///
    /// Memory fences are used for proper synchronization.
    pub fn write_at_most(&mut self, data: &[u8]) -> usize {
        let len = data.len().min(self.space());

        atomic::fence(Ordering::Acquire);
        let wpos = self.get_write_pos();
        for (i, byte) in data.iter().enumerate().take(len) {
            let offset = (wpos + i) % self.capacity;
            unsafe {
                let dptr = self.data_ptr.add(offset);
                write_volatile(dptr, *byte);
            }
        }
        atomic::fence(Ordering::Release);
        let wpos = (wpos + len) % self.capacity;
        self.set_write_pos(wpos);

        len
    }

    /// Attempts to write data to the queue in non-blocking mode.
    ///
    /// If there is not enough space to write the entire data, returns an error (`WouldBlock`).
    /// On success, writes the data into the queue.
    pub fn write_or_fail(&mut self, data: &[u8]) -> nb::Result<(), Infallible> {
        if self.space() < data.len() {
            return Err(nb::Error::WouldBlock);
        }
        self.write_at_most(data);
        Ok(())
    }

    /// Blocks until there is enough space in the queue to write the data.
    ///
    /// Once space is available, writes `data.len()` bytes of data to the queue.
    pub fn write_blocking(&mut self, data: &[u8]) {
        loop {
            if self.space() >= data.len() {
                break;
            }
        }
        self.write_at_most(data);
    }

    //
    // Skip methods
    //

    /// Skips at most `len` bytes, or less depending on the size of the written data in the byte
    /// queue.
    pub fn skip_at_most(&mut self, len: usize) -> usize {
        let len = len.min(self.size());
        self.set_read_pos((self.get_read_pos() + len) % self.capacity);

        len
    }

    /// Attempts to skip `len` bytes in non-blocking mode.
    ///
    /// If there is not enough data to be skipped, returns an error (`WouldBlock`).
    /// On success, skips `len` bytes of written data.
    pub fn skip_or_fail(&mut self, len: usize) -> nb::Result<(), Infallible> {
        if self.size() < len {
            return Err(nb::Error::WouldBlock);
        }
        self.skip_at_most(len);
        Ok(())
    }

    /// Blocks until there is enough data in the queue to be skipped.
    ///
    /// Once enough data is available, skips `len` bytes of data in the queue.
    pub fn skip_blocking(&mut self, len: usize) {
        loop {
            if self.size() >= len {
                break;
            }
        }
        self.skip_at_most(len);
    }

    //
    // Peek methods
    //

    /// Read at most `len` bytes without losing them in the queue, or less depending on the
    /// size of the written data in the byte queue.
    pub fn peek_at_most(&self, buf: &mut [u8], len: usize) -> usize {
        let len = len.min(buf.len()).min(self.size());

        atomic::fence(Ordering::Acquire);
        let rpos = self.get_read_pos();
        for (i, byte) in buf.iter_mut().enumerate().take(len) {
            let offset = (rpos + i) % self.capacity;
            unsafe {
                let dptr = self.data_ptr.add(offset);
                *byte = read_volatile(dptr);
            }
        }
        atomic::fence(Ordering::Release);

        // Here is where we would update the read position pointer in a consuming implementation.
        // Since consume = peek + skip, the skip function does not need memory fencing because it
        // is done here already.
        len
    }

    /// Attempts to fill the buffer completely with the data in the byte queue in non-blocking
    /// mode.
    ///
    /// If there is not enough data, returns an error (`WouldBlock`).
    /// On success, read `buf.len()` bytes of written data without skipping them in the byte
    /// queue.
    pub fn peek_or_fail(&self, buf: &mut [u8]) -> nb::Result<(), Infallible> {
        if self.size() < buf.len() {
            return Err(nb::Error::WouldBlock);
        }
        self.peek_at_most(buf, buf.len());
        Ok(())
    }

    /// Blocks until there is enough data in the queue to fill the buffer completely.
    ///
    /// On success, read `buf.len()` bytes of written data without skipping them in the byte
    /// queue.
    pub fn peek_blocking(&self, buf: &mut [u8]) {
        loop {
            if self.size() >= buf.len() {
                break;
            }
        }
        self.peek_at_most(buf, buf.len());
    }

    //
    // Consume methods
    //

    /// Reads up to the available data into the provided buffer, returning the number of bytes read.
    ///
    /// This method reads/consumes at most the size of the buffer or the amount of available data,
    /// whichever is smaller.
    pub fn consume_at_most(&mut self, buf: &mut [u8]) -> usize {
        let len = self.peek_at_most(buf, buf.len());

        self.skip_at_most(len)
    }

    /// Attempts to read data from the queue in non-blocking mode. If there is not enough data in
    /// `buf.len()` size available to be read, returns an error (`WouldBlock`).
    ///
    /// On success, reads/consumes the data in `buf.len()` size from the queue into
    /// the provided buffer.
    pub fn consume_or_fail(&mut self, buf: &mut [u8]) -> nb::Result<(), Infallible> {
        self.peek_or_fail(buf)?;
        self.skip_at_most(buf.len());

        Ok(())
    }

    /// Blocks until there is enough data in the queue to fill the buffer completely.
    ///
    /// On success, reads/consumes the data in `buf.len()` size from the queue into
    /// the provided buffer.
    pub fn consume_blocking(&mut self, buf: &mut [u8]) {
        self.peek_blocking(buf);
        self.skip_at_most(buf.len());
    }
}

impl core::fmt::Write for ByteQueue {
    fn write_str(&mut self, s: &str) -> Result<(), core::fmt::Error> {
        self.write_blocking(s.as_bytes());
        Ok(())
    }
}

#[cfg(test)]
mod tests {
    use super::ByteQueue;
    const LEN_U32_TO_U8_SCALER: usize = core::mem::size_of::<u32>();

    #[test]
    fn test_peek() {
        let mut buffer = [123u32; 17];
        let mut writer = unsafe {
            ByteQueue::create(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };
        let mut reader = unsafe {
            ByteQueue::attach(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };
        let tx = [1, 2, 3, 4];
        writer.write_or_fail(&tx).unwrap();

        let mut rx = [0u8; 4];
        reader.peek_or_fail(&mut rx).unwrap();
        assert_eq!(&tx, &rx);
        assert!(reader.size() == tx.len());
        for i in 0..1234 {
            reader.peek_at_most(&mut rx, i);
            assert_eq!(&tx[..i.min(tx.len())], &rx[..i.min(rx.len())]);
            assert!(reader.size() == tx.len());
        }

        reader.consume_or_fail(&mut rx).unwrap();
        assert_eq!(&tx, &rx);
        assert!(reader.size() == 0);
    }

    #[test]
    fn test_skip() {
        let mut buffer = [123u32; 55];
        let mut writer = unsafe {
            ByteQueue::create(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };
        let mut reader = unsafe {
            ByteQueue::attach(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };

        let data = [0xffu8; 10];
        let sum_to_ten = 55;
        for i in 0..=10 {
            writer.write_at_most(&data[..i]);
        }

        let mut skipped = 0;
        for i in 0..=10 {
            reader.skip_or_fail(i).unwrap();
            skipped += i;
            assert_eq!(reader.size(), sum_to_ten - skipped);
        }
    }

    #[test]
    fn write_read() {
        let mut buffer = [123u32; 17];
        let mut writer = unsafe {
            ByteQueue::create(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };
        let mut reader = unsafe {
            ByteQueue::attach(
                buffer.as_mut_ptr() as *mut u8,
                buffer.len() * LEN_U32_TO_U8_SCALER,
            )
        };
        let tx = [1, 2, 3, 4];
        writer.write_blocking(&tx);
        let mut rx = [0u8; 4];
        reader.consume_blocking(&mut rx);
        assert_eq!(&tx, &rx);
    }
}