artiq-zynq/src/satman/src/dma.rs

use alloc::{collections::btree_map::BTreeMap, vec::Vec};

use libboard_artiq::{drtioaux_proto::PayloadStatus, pl::csr};
use libcortex_a9::cache::dcci_slice;

const ALIGNMENT: usize = 64;

#[derive(Debug, PartialEq)]
enum ManagerState {
    Idle,
    Playback,
}

pub struct RtioStatus {
    pub source: u8,
    pub id: u32,
    pub error: u8,
    pub channel: u32,
    pub timestamp: u64,
}

pub enum Error {
    IdNotFound,
    PlaybackInProgress,
    EntryNotComplete,
}

#[derive(Debug)]
struct Entry {
    trace: Vec<u8>,
    padding_len: usize,
    complete: bool,
}

#[derive(Debug)]
pub struct Manager {
    entries: BTreeMap<(u8, u32), Entry>,
    state: ManagerState,
    current_id: u32,
    current_source: u8,
}

impl Manager {
    pub fn new() -> Manager {
        // in case Manager is created during a DMA in progress
        // wait for it to end
        unsafe { while csr::rtio_dma::enable_read() != 0 {} }
        Manager {
            entries: BTreeMap::new(),
            current_id: 0,
            current_source: 0,
            state: ManagerState::Idle,
        }
    }

    pub fn add(
        &mut self,
        source: u8,
        id: u32,
        status: PayloadStatus,
        trace: &[u8],
        trace_len: usize,
    ) -> Result<(), Error> {
        let entry = match self.entries.get_mut(&(source, id)) {
            Some(entry) => {
                if entry.complete || status.is_first() {
                    // replace entry
                    self.entries.remove(&(source, id));
                    self.entries.insert(
                        (source, id),
                        Entry {
                            trace: Vec::new(),
                            padding_len: 0,
                            complete: false,
                        },
                    );
                    self.entries.get_mut(&(source, id)).unwrap()
                } else {
                    entry
                }
            }
            None => {
                self.entries.insert(
                    (source, id),
                    Entry {
                        trace: Vec::new(),
                        padding_len: 0,
                        complete: false,
                    },
                );
                self.entries.get_mut(&(source, id)).unwrap()
            }
        };
        entry.trace.extend(&trace[0..trace_len]);

        if status.is_last() {
            entry.trace.push(0);
            let data_len = entry.trace.len();

            // Realign.
            entry.trace.reserve(ALIGNMENT - 1);
            let padding = ALIGNMENT - entry.trace.as_ptr() as usize % ALIGNMENT;
            let padding = if padding == ALIGNMENT { 0 } else { padding };
            for _ in 0..padding {
                // Vec guarantees that this will not reallocate
                entry.trace.push(0)
            }
            for i in 1..data_len + 1 {
                entry.trace[data_len + padding - i] = entry.trace[data_len - i]
            }
            entry.complete = true;
            entry.padding_len = padding;
            dcci_slice(&entry.trace);
        }
        Ok(())
    }

    pub fn erase(&mut self, source: u8, id: u32) -> Result<(), Error> {
        match self.entries.remove(&(source, id)) {
            Some(_) => Ok(()),
            None => Err(Error::IdNotFound),
        }
    }

    pub fn playback(&mut self, source: u8, id: u32, timestamp: u64) -> Result<(), Error> {
        if self.state != ManagerState::Idle {
            return Err(Error::PlaybackInProgress);
        }

        let entry = match self.entries.get(&(source, id)) {
            Some(entry) => entry,
            None => {
                return Err(Error::IdNotFound);
            }
        };
        if !entry.complete {
            return Err(Error::EntryNotComplete);
        }
        let ptr = entry.trace[entry.padding_len..].as_ptr();
        assert!(ptr as u32 % 64 == 0);

        self.state = ManagerState::Playback;
        self.current_id = id;
        self.current_source = source;

        unsafe {
            csr::rtio_dma::base_address_write(ptr as u32);
            csr::rtio_dma::time_offset_write(timestamp as u64);

            csr::cri_con::selected_write(1);
            csr::rtio_dma::enable_write(1);
            // playback has begun here, for status call check_state
        }
        Ok(())
    }

    pub fn check_state(&mut self) -> Option<RtioStatus> {
        if self.state != ManagerState::Playback {
            // nothing to report
            return None;
        }
        let dma_enable = unsafe { csr::rtio_dma::enable_read() };
        if dma_enable != 0 {
            return None;
        } else {
            self.state = ManagerState::Idle;
            unsafe {
                csr::cri_con::selected_write(0);
                let error = csr::rtio_dma::error_read();
                let channel = csr::rtio_dma::error_channel_read();
                let timestamp = csr::rtio_dma::error_timestamp_read();
                if error != 0 {
                    csr::rtio_dma::error_write(1);
                }
                return Some(RtioStatus {
                    source: self.current_source,
                    id: self.current_id,
                    error: error,
                    channel: channel,
                    timestamp: timestamp,
                });
            }
        }
    }

    pub fn running(&self) -> bool {
        self.state == ManagerState::Playback
    }
}