satman: add dma support

src/libboard_artiq/src/drtioaux_proto.rs

@@ -1,6 +1,9 @@
 use core_io::{Error as IoError, Read, Write};
 use io::proto::{ProtoRead, ProtoWrite};
 
+/* 512 (max size) - 4 (CRC) - 1 (packet ID) - 1 (destination) - 4 (trace ID) - 1 (last) - 2 (length) */
+pub const DMA_TRACE_MAX_SIZE: usize = 499;
+
 #[derive(Debug)]
 pub enum Error {
     UnknownPacket(u8),
@@ -132,6 +135,39 @@ pub enum Packet {
     SpiBasicReply {
         succeeded: bool,
     },
+
+    DmaAddTraceRequest { 
+        destination: u8, 
+        id: u32, 
+        last: bool, 
+        length: u16, 
+        trace: [u8; DMA_TRACE_MAX_SIZE] 
+    },
+    DmaAddTraceReply { 
+        succeeded: bool 
+    },
+    DmaRemoveTraceRequest { 
+        destination: u8, 
+        id: u32 
+    },
+    DmaRemoveTraceReply { 
+        succeeded: bool 
+    },
+    DmaPlaybackRequest { 
+        destination: u8, 
+        id: u32, 
+        timestamp: u64 
+    },
+    DmaPlaybackReply { 
+        succeeded: bool 
+    },
+    DmaPlaybackStatus { 
+        destination: u8, 
+        id: u32, 
+        error: u8, 
+        channel: u32, 
+        timestamp: u64 
+    },
 }
 
 impl Packet {
@@ -262,6 +298,47 @@ impl Packet {
                 succeeded: reader.read_bool()?,
             },
 
+            0xb0 => { 
+                let destination = reader.read_u8()?;
+                let id = reader.read_u32()?;
+                let last = reader.read_bool()?;
+                let length = reader.read_u16()?;
+                let mut trace: [u8; DMA_TRACE_MAX_SIZE] = [0; DMA_TRACE_MAX_SIZE];
+                reader.read_exact(&mut trace[0..length as usize])?;
+                Packet::DmaAddTraceRequest {
+                    destination: destination,
+                    id: id,
+                    last: last,
+                    length: length as u16,
+                    trace: trace,
+                }
+            },
+            0xb1 => Packet::DmaAddTraceReply {
+                succeeded: reader.read_bool()?
+            },
+            0xb2 => Packet::DmaRemoveTraceRequest {
+                destination: reader.read_u8()?,
+                id: reader.read_u32()?
+            },
+            0xb3 => Packet::DmaRemoveTraceReply {
+                succeeded: reader.read_bool()?
+            },
+            0xb4 => Packet::DmaPlaybackRequest {
+                destination: reader.read_u8()?,
+                id: reader.read_u32()?,
+                timestamp: reader.read_u64()?
+            },
+            0xb5 => Packet::DmaPlaybackReply {
+                succeeded: reader.read_bool()?
+            },
+            0xb6 => Packet::DmaPlaybackStatus {
+                destination: reader.read_u8()?,
+                id: reader.read_u32()?,
+                error: reader.read_u8()?,
+                channel: reader.read_u32()?,
+                timestamp: reader.read_u64()?
+            },
+
             ty => return Err(Error::UnknownPacket(ty)),
         })
     }
@@ -448,6 +525,64 @@ impl Packet {
                 writer.write_u8(0x95)?;
                 writer.write_bool(succeeded)?;
             }
+
+            Packet::DmaAddTraceRequest { 
+                destination, 
+                id, 
+                last, 
+                trace, 
+                length 
+            } => {
+                writer.write_u8(0xb0)?;
+                writer.write_u8(destination)?;
+                writer.write_u32(id)?;
+                writer.write_bool(last)?;
+                // trace may be broken down to fit within drtio aux memory limit
+                // will be reconstructed by satellite
+                writer.write_u16(length)?;
+                writer.write_all(&trace[0..length as usize])?;
+            }
+            Packet::DmaAddTraceReply { succeeded } => {
+                writer.write_u8(0xb1)?;
+                writer.write_bool(succeeded)?;
+            }
+            Packet::DmaRemoveTraceRequest { destination, id } => {
+                writer.write_u8(0xb2)?;
+                writer.write_u8(destination)?;
+                writer.write_u32(id)?;
+            }
+            Packet::DmaRemoveTraceReply { succeeded } => {
+                writer.write_u8(0xb3)?;
+                writer.write_bool(succeeded)?;
+            }
+            Packet::DmaPlaybackRequest { 
+                destination, 
+                id, 
+                timestamp 
+            } => {
+                writer.write_u8(0xb4)?;
+                writer.write_u8(destination)?;
+                writer.write_u32(id)?;
+                writer.write_u64(timestamp)?;
+            }
+            Packet::DmaPlaybackReply { succeeded } => {
+                writer.write_u8(0xb5)?;
+                writer.write_bool(succeeded)?;
+            }
+            Packet::DmaPlaybackStatus { 
+                destination, 
+                id, 
+                error, 
+                channel, 
+                timestamp 
+            } => {
+                writer.write_u8(0xb6)?;
+                writer.write_u8(destination)?;
+                writer.write_u32(id)?;
+                writer.write_u8(error)?;
+                writer.write_u32(channel)?;
+                writer.write_u64(timestamp)?;
+            }
         }
         Ok(())
     }

src/satman/src/dma.rs

@@ -0,0 +1,164 @@
+use libcortex_a9::cache::dcci_slice;
+use libboard_artiq::pl::csr;
+use alloc::{vec::Vec, collections::btree_map::BTreeMap};
+
+const ALIGNMENT: usize = 64;
+
+#[derive(Debug, PartialEq)]
+enum ManagerState {
+    Idle,
+    Playback
+}
+
+pub struct RtioStatus {
+    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<u32, Entry>,
+    state: ManagerState,
+    currentid: u32
+}
+
+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(),
+            currentid: 0,
+            state: ManagerState::Idle,
+        }
+    }
+
+    pub fn add(&mut self, id: u32, last: bool, trace: &[u8], trace_len: usize) -> Result<(), Error> {
+        let entry = match self.entries.get_mut(&id) {
+            Some(entry) => {
+                if entry.complete {
+                    // replace entry
+                    self.entries.remove(&id);
+                    self.entries.insert(id, Entry {
+                        trace: Vec::new(),
+                        padding_len: 0,
+                        complete: false });
+                    self.entries.get_mut(&id).unwrap()
+                } else {
+                    entry
+                }
+            },
+            None => {
+                self.entries.insert(id, Entry {
+                    trace: Vec::new(),
+                    padding_len: 0,
+                    complete: false });
+                self.entries.get_mut(&id).unwrap()
+            },
+        };
+        entry.trace.extend(&trace[0..trace_len]);
+
+        if 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, id: u32) -> Result<(), Error> {
+        match self.entries.remove(&id) {
+            Some(_) => Ok(()),
+            None => Err(Error::IdNotFound)
+        }
+    }
+
+    pub fn playback(&mut self, id: u32, timestamp: u64) -> Result<(), Error> {
+        if self.state != ManagerState::Idle {
+            return Err(Error::PlaybackInProgress);
+        }
+
+        let entry = match self.entries.get(&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.currentid = id;
+
+        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 { 
+                    id: self.currentid, 
+                    error: error,
+                    channel: channel, 
+                    timestamp: timestamp });
+            }
+        }
+    }
+
+}

src/satman/src/main.rs

@@ -36,8 +36,10 @@ use libcortex_a9::{asm, interrupt_handler,
                    spin_lock_yield};
 use libregister::{RegisterR, RegisterW};
 use libsupport_zynq::ram;
+use dma::Manager as DmaManager;
 
 mod repeater;
+mod dma;
 
 fn drtiosat_reset(reset: bool) {
     unsafe {
@@ -92,6 +94,7 @@ fn process_aux_packet(
     packet: drtioaux::Packet,
     timer: &mut GlobalTimer,
     i2c: &mut I2c,
+    dma_manager: &mut DmaManager
 ) -> Result<(), drtioaux::Error> {
     // In the code below, *_chan_sel_write takes an u8 if there are fewer than 256 channels,
     // and u16 otherwise; hence the `as _` conversion.
@@ -409,6 +412,38 @@ fn process_aux_packet(
             )
         }
 
+        drtioaux::Packet::DmaAddTraceRequest { 
+            destination: _destination, 
+            id, 
+            last, 
+            length, 
+            trace 
+        } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = dma_manager.add(id, last, &trace, length as usize).is_ok();
+            drtioaux::send(0,
+                &drtioaux::Packet::DmaAddTraceReply { succeeded: succeeded })
+        }
+        drtioaux::Packet::DmaRemoveTraceRequest { 
+            destination: _destination, 
+            id 
+        } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = dma_manager.erase(id).is_ok();
+            drtioaux::send(0,
+                &drtioaux::Packet::DmaRemoveTraceReply { succeeded: succeeded })
+        }
+        drtioaux::Packet::DmaPlaybackRequest { 
+            destination: _destination, 
+            id, 
+            timestamp 
+        } => {
+            forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer);
+            let succeeded = dma_manager.playback(id, timestamp).is_ok();
+            drtioaux::send(0,
+                &drtioaux::Packet::DmaPlaybackReply { succeeded: succeeded })
+        }
+
         _ => {
             warn!("received unexpected aux packet");
             Ok(())
@@ -422,10 +457,11 @@ fn process_aux_packets(
     rank: &mut u8,
     timer: &mut GlobalTimer,
     i2c: &mut I2c,
+    dma_manager: &mut DmaManager
 ) {
     let result = drtioaux::recv(0).and_then(|packet| {
         if let Some(packet) = packet {
-            process_aux_packet(repeaters, routing_table, rank, packet, timer, i2c)
+            process_aux_packet(repeaters, routing_table, rank, packet, timer, i2c, dma_manager)
         } else {
             Ok(())
         }
@@ -598,13 +634,18 @@ pub extern "C" fn main_core0() -> i32 {
             si5324::siphaser::calibrate_skew(&mut timer).expect("failed to calibrate skew");
         }
 
+        // DMA manager created here, so when link is dropped, all DMA traces
+        // are cleared out for a clean slate on subsequent connections,
+        // without a manual intervention.
+        let mut dma_manager = DmaManager::new();
+
         drtioaux::reset(0);
         drtiosat_reset(false);
         drtiosat_reset_phy(false);
 
         while drtiosat_link_rx_up() {
             drtiosat_process_errors();
-            process_aux_packets(&mut repeaters, &mut routing_table, &mut rank, &mut timer, &mut i2c);
+            process_aux_packets(&mut repeaters, &mut routing_table, &mut rank, &mut timer, &mut i2c, &mut dma_manager);
             #[allow(unused_mut)]
             for mut rep in repeaters.iter_mut() {
                 rep.service(&routing_table, rank, &mut timer);
@@ -621,6 +662,18 @@ pub extern "C" fn main_core0() -> i32 {
                     error!("aux packet error: {:?}", e);
                 }
             }
+            if let Some(status) = dma_manager.check_state() {
+                info!("playback done, error: {}, channel: {}, timestamp: {}", status.error, status.channel, status.timestamp);
+                if let Err(e) = drtioaux::send(0, &drtioaux::Packet::DmaPlaybackStatus { 
+                    destination: rank, 
+                    id: status.id, 
+                    error: status.error, 
+                    channel: status.channel, 
+                    timestamp: status.timestamp 
+                }) {
+                    error!("error sending DMA playback status: {:?}", e);
+                }
+            }
         }
 
         drtiosat_reset_phy(true);