firwmare: propagate DRTIO routing table and rank all the way

artiq/firmware/runtime/rtio_mgt.rs

@@ -146,7 +146,8 @@ pub mod drtio {
         }
     }
 
-    fn load_routing_table(io: &Io, linkno: u8, routing_table: &drtio_routing::RoutingTable) -> Result<(), &'static str> {
+    fn load_routing_table(io: &Io, linkno: u8, routing_table: &drtio_routing::RoutingTable)
+            -> Result<(), &'static str> {
         for i in 0..drtio_routing::DEST_COUNT {
             drtioaux::send_link(linkno, &drtioaux::Packet::RoutingSetPath {
                 destination: i as u8,
@@ -160,6 +161,17 @@ pub mod drtio {
         Ok(())
     }
 
+    fn set_rank(io: &Io, linkno: u8, rank: u8) -> Result<(), &'static str> {
+        drtioaux::send_link(linkno, &drtioaux::Packet::RoutingSetRank {
+            rank: rank
+        }).unwrap();
+        let reply = recv_aux_timeout(io, linkno, 200)?;
+        if reply != drtioaux::Packet::RoutingAck {
+            return Err("unexpected reply");
+        }
+        Ok(())
+    }
+
     fn process_local_errors(linkno: u8) {
         let errors;
         let linkidx = linkno as usize;
@@ -224,6 +236,9 @@ pub mod drtio {
                             if let Err(e) = load_routing_table(&io, linkno, routing_table) {
                                 error!("[LINK#{}] failed to load routing table ({})", linkno, e);
                             }
+                            if let Err(e) = set_rank(&io, linkno, 1) {
+                                error!("[LINK#{}] failed to set rank ({})", linkno, e);
+                            }
                             info!("[LINK#{}] link initialization completed", linkno);
                         } else {
                             error!("[LINK#{}] ping failed", linkno);

artiq/firmware/satman/main.rs

@@ -12,6 +12,7 @@ use board_misoc::{csr, irq, ident, clock, uart_logger};
 use board_artiq::{i2c, spi, si5324, drtioaux};
 #[cfg(has_serwb_phy_amc)]
 use board_artiq::serwb;
+use board_artiq::drtio_routing;
 #[cfg(has_hmc830_7043)]
 use board_artiq::hmc830_7043;
 
@@ -45,7 +46,9 @@ fn drtiosat_tsc_loaded() -> bool {
     }
 }
 
-fn process_aux_packet(packet: drtioaux::Packet) -> Result<(), drtioaux::Error<!>> {
+fn process_aux_packet(_repeaters: &mut [repeater::Repeater],
+        _routing_table: &mut drtio_routing::RoutingTable, _rank: &mut u8,
+        packet: drtioaux::Packet) -> 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.
     match packet {
@@ -97,8 +100,40 @@ fn process_aux_packet(packet: drtioaux::Packet) -> Result<(), drtioaux::Error<!>
             }
         }
 
+        #[cfg(has_drtio_routing)]
         drtioaux::Packet::RoutingSetPath { destination, hops } => {
-            info!("routing: {} -> {:?}", destination, hops);
+            _routing_table.0[destination as usize] = hops;
+            for rep in _repeaters.iter() {
+                if let Err(e) = rep.set_path(destination, &hops) {
+                    error!("failed to set path ({})", e);
+                }
+            }
+            drtioaux::send_link(0, &drtioaux::Packet::RoutingAck)
+        }
+        #[cfg(has_drtio_routing)]
+        drtioaux::Packet::RoutingSetRank { rank } => {
+            *_rank = rank;
+            drtio_routing::program_interconnect(_routing_table, rank);
+
+            let rep_rank = rank + 1;
+            for rep in _repeaters.iter() {
+                if let Err(e) = rep.set_rank(rep_rank) {
+                    error!("failed to set rank ({})", e);
+                }
+            }
+
+            info!("rank: {}", rank);
+            info!("routing table: {}", _routing_table);
+
+            drtioaux::send_link(0, &drtioaux::Packet::RoutingAck)
+        }
+
+        #[cfg(not(has_drtio_routing))]
+        drtioaux::Packet::RoutingSetPath { _destination, _hops } => {
+            drtioaux::send_link(0, &drtioaux::Packet::RoutingAck)
+        }
+        #[cfg(not(has_drtio_routing))]
+        drtioaux::Packet::RoutingSetRank { _rank } => {
             drtioaux::send_link(0, &drtioaux::Packet::RoutingAck)
         }
 
@@ -197,11 +232,12 @@ fn process_aux_packet(packet: drtioaux::Packet) -> Result<(), drtioaux::Error<!>
     }
 }
 
-fn process_aux_packets() {
+fn process_aux_packets(repeaters: &mut [repeater::Repeater],
+        routing_table: &mut drtio_routing::RoutingTable, rank: &mut u8) {
     let result =
         drtioaux::recv_link(0).and_then(|packet| {
             if let Some(packet) = packet {
-                process_aux_packet(packet)
+                process_aux_packet(repeaters, routing_table, rank, packet)
             } else {
                 Ok(())
             }
@@ -291,16 +327,17 @@ pub extern fn main() -> i32 {
     let mut repeaters = [repeater::Repeater::default(); csr::DRTIOREP.len()];
     #[cfg(not(has_drtio_routing))]
     let mut repeaters = [repeater::Repeater::default(); 0];
-
     for i in 0..repeaters.len() {
         repeaters[i] = repeater::Repeater::new(i as u8);
     } 
+    let mut routing_table = drtio_routing::RoutingTable::default_empty();
+    let mut rank = 1;
 
     loop {
         while !drtiosat_link_rx_up() {
             drtiosat_process_errors();
             for mut rep in repeaters.iter_mut() {
-                rep.service();
+                rep.service(&routing_table, rank);
             }
         }
 
@@ -333,9 +370,9 @@ pub extern fn main() -> i32 {
 
         while drtiosat_link_rx_up() {
             drtiosat_process_errors();
-            process_aux_packets();
+            process_aux_packets(&mut repeaters, &mut routing_table, &mut rank);
             for mut rep in repeaters.iter_mut() {
-                rep.service();
+                rep.service(&routing_table, rank);
             }
             if drtiosat_tsc_loaded() {
                 info!("TSC loaded from uplink");

artiq/firmware/satman/repeater.rs

@@ -1,5 +1,5 @@
 use board_misoc::{csr, clock};
-use board_artiq::drtioaux;
+use board_artiq::{drtioaux, drtio_routing};
 
 #[cfg(has_drtio_routing)]
 fn rep_link_rx_up(linkno: u8) -> bool {
@@ -39,7 +39,7 @@ impl Repeater {
         }
     }
 
-    pub fn service(&mut self) {
+    pub fn service(&mut self, routing_table: &drtio_routing::RoutingTable, rank: u8) {
         match self.state {
             RepeaterState::Down => {
                 if rep_link_rx_up(self.repno) {
@@ -63,13 +63,22 @@ impl Repeater {
                 if rep_link_rx_up(self.repno) {
                     if let Ok(Some(drtioaux::Packet::EchoReply)) = drtioaux::recv_link(self.auxno) {
                         info!("[REP#{}] remote replied after {} packets", self.repno, ping_count);
+                        self.state = RepeaterState::Up;
                         if let Err(e) = self.sync_tsc() {
                             error!("[REP#{}] failed to sync TSC ({})", self.repno, e);
                             self.state = RepeaterState::Failed;
                             return;
                         }
-                        // TODO: send routing table and rank
-                        self.state = RepeaterState::Up; 
+                        if let Err(e) = self.load_routing_table(routing_table) {
+                            error!("[REP#{}] failed to sync TSC ({})", self.repno, e);
+                            self.state = RepeaterState::Failed;
+                            return;
+                        }
+                        if let Err(e) = self.set_rank(rank) {
+                            error!("[REP#{}] failed to sync TSC ({})", self.repno, e);
+                            self.state = RepeaterState::Failed;
+                            return;
+                        }
                     } else {
                         if clock::get_ms() > timeout {
                             if ping_count > 200 {
@@ -100,6 +109,23 @@ impl Repeater {
         }
     }
 
+    fn recv_aux_timeout(&self, timeout: u32) -> Result<drtioaux::Packet, &'static str> {
+        let max_time = clock::get_ms() + timeout as u64;
+        loop {
+            if !rep_link_rx_up(self.repno) {
+                return Err("link went down");
+            }
+            if clock::get_ms() > max_time {
+                return Err("timeout");
+            }
+            match drtioaux::recv_link(self.auxno) {
+                Ok(Some(packet)) => return Ok(packet),
+                Ok(None) => (),
+                Err(_) => return Err("aux packet error")
+            }
+        }
+    }
+
     pub fn sync_tsc(&self) -> Result<(), &'static str> {
         if self.state != RepeaterState::Up {
             return Ok(());
@@ -111,20 +137,51 @@ impl Repeater {
             while (csr::DRTIOREP[repno].set_time_read)() == 1 {}
         }
 
-        let timeout = clock::get_ms() + 200;
-        loop {
-            if !rep_link_rx_up(self.repno) {
-                return Err("link went down");
-            }
-            if clock::get_ms() > timeout {
-                return Err("timeout");
-            }
         // TSCAck is the only aux packet that is sent spontaneously
         // by the satellite, in response to a TSC set on the RT link.
-            if let Ok(Some(drtioaux::Packet::TSCAck)) = drtioaux::recv_link(self.auxno) {
+        let reply = self.recv_aux_timeout(10000)?;
+        if reply == drtioaux::Packet::TSCAck {
+            return Ok(());
+        } else {
+            return Err("unexpected reply");
+        }
+    }
+
+    pub fn set_path(&self, destination: u8, hops: &[u8; drtio_routing::MAX_HOPS]) -> Result<(), &'static str> {
+        if self.state != RepeaterState::Up {
             return Ok(());
         }
+
+        drtioaux::send_link(self.auxno, &drtioaux::Packet::RoutingSetPath {
+            destination: destination,
+            hops: *hops
+        }).unwrap();
+        let reply = self.recv_aux_timeout(200)?;
+        if reply != drtioaux::Packet::RoutingAck {
+            return Err("unexpected reply");
         }
+        Ok(())
+    }
+
+    pub fn load_routing_table(&self, routing_table: &drtio_routing::RoutingTable) -> Result<(), &'static str> {
+        for i in 0..drtio_routing::DEST_COUNT {
+            self.set_path(i as u8, &routing_table.0[i])?;
+        }
+        Ok(())
+    }
+
+    pub fn set_rank(&self, rank: u8) -> Result<(), &'static str> {
+        if self.state != RepeaterState::Up {
+            return Ok(());
+        }
+        drtioaux::send_link(self.auxno, &drtioaux::Packet::RoutingSetRank {
+            rank: rank
+        }).unwrap();
+        let reply = self.recv_aux_timeout(200)?;
+        if reply != drtioaux::Packet::RoutingAck {
+            return Err("unexpected reply");
+        }
+        Ok(())
     }
 }