diff --git a/artiq/firmware/libboard_artiq/drtio_routing.rs b/artiq/firmware/libboard_artiq/drtio_routing.rs
index 562517342..6c8469ae2 100644
--- a/artiq/firmware/libboard_artiq/drtio_routing.rs
+++ b/artiq/firmware/libboard_artiq/drtio_routing.rs
@@ -30,6 +30,18 @@ impl RoutingTable {
     pub fn default_empty() -> RoutingTable {
         RoutingTable([[INVALID_HOP; MAX_HOPS]; DEST_COUNT])
     }
+
+    pub fn hop_count(&self, destination: u8) -> u8 {
+        let mut count = 0;
+        for i in 0..MAX_HOPS {
+            if self.0[destination as usize][i] == INVALID_HOP {
+                break;
+            } else {
+                count += 1;
+            }
+        }
+        count
+    }
 }
 
 impl fmt::Display for RoutingTable {
diff --git a/artiq/firmware/runtime/rtio_mgt.rs b/artiq/firmware/runtime/rtio_mgt.rs
index 69233dd60..95b43c586 100644
--- a/artiq/firmware/runtime/rtio_mgt.rs
+++ b/artiq/firmware/runtime/rtio_mgt.rs
@@ -54,6 +54,12 @@ pub mod drtio {
         unsafe {
             csr::drtio_transceiver::stable_clkin_write(1);
         }
+
+        {
+            let routing_table = routing_table.borrow();
+            program_underflow_margins(&routing_table);
+        }
+
         let aux_mutex = aux_mutex.clone();
         let routing_table = routing_table.clone();
         let up_destinations = up_destinations.clone();
@@ -63,6 +69,23 @@ pub mod drtio {
         });
     }
 
+    fn program_underflow_margins(routing_table: &drtio_routing::RoutingTable) {
+        for destination in 0..drtio_routing::DEST_COUNT {
+            let hop_count = routing_table.hop_count(destination as u8);
+            if hop_count > 1 {
+                let underflow_margin = (hop_count as u16 - 1)*300;
+                info!("[DEST#{}] setting underflow margin to {}", destination, underflow_margin);
+                let linkno = (routing_table.0[destination][0] - 1) as usize;
+                unsafe {
+                    (csr::DRTIO[linkno].destination_write)(destination as u8);
+                    (csr::DRTIO[linkno].force_destination_write)(1);
+                    (csr::DRTIO[linkno].set_underflow_margin_write)(underflow_margin);
+                    (csr::DRTIO[linkno].force_destination_write)(0);
+                }
+            }
+        }
+    }
+
     fn link_rx_up(linkno: u8) -> bool {
         let linkno = linkno as usize;
         unsafe {
diff --git a/artiq/gateware/drtio/rt_controller_master.py b/artiq/gateware/drtio/rt_controller_master.py
index 40014dce0..3209f1739 100644
--- a/artiq/gateware/drtio/rt_controller_master.py
+++ b/artiq/gateware/drtio/rt_controller_master.py
@@ -17,11 +17,13 @@ class _CSRs(AutoCSR):
         self.protocol_error = CSR(3)
 
         self.set_time = CSR()
-        self.underflow_margin = CSRStorage(16, reset=300)
 
         self.force_destination = CSRStorage()
         self.destination = CSRStorage(8)
 
+        self.set_underflow_margin = CSRStorage(16)
+        self.dbg_underflow_margin = CSRStatus(16)
+
         self.o_get_buffer_space = CSR()
         self.o_dbg_buffer_space = CSRStatus(16)
         self.o_dbg_buffer_space_req_cnt = CSRStatus(32)
@@ -116,9 +118,19 @@ class RTController(Module):
         timeout_counter = WaitTimer(8191)
         self.submodules += timeout_counter
 
+        underflow_margin = Memory(16, 256, init=[100]*256)
+        underflow_margin_port = underflow_margin.get_port(write_capable=True)
+        self.specials += underflow_margin, underflow_margin_port
+        self.comb += [
+            underflow_margin_port.adr.eq(chan_sel[16:]),
+            underflow_margin_port.dat_w.eq(self.csrs.set_underflow_margin.storage),
+            underflow_margin_port.we.eq(self.csrs.set_underflow_margin.re),
+            self.csrs.dbg_underflow_margin.status.eq(underflow_margin_port.dat_r)
+        ]
+
         cond_underflow = Signal()
         self.comb += cond_underflow.eq((self.cri.timestamp[tsc.glbl_fine_ts_width:]
-                           - self.csrs.underflow_margin.storage[tsc.glbl_fine_ts_width:]) < tsc.coarse_ts_sys)
+                           - underflow_margin_port.dat_r[tsc.glbl_fine_ts_width:]) < tsc.coarse_ts_sys)
 
         # buffer space
         buffer_space = Memory(16, 256)
diff --git a/artiq/gateware/test/drtio/test_full_stack.py b/artiq/gateware/test/drtio/test_full_stack.py
index 579d78f9b..a4e239b99 100644
--- a/artiq/gateware/test/drtio/test_full_stack.py
+++ b/artiq/gateware/test/drtio/test_full_stack.py
@@ -89,7 +89,6 @@ class OutputsTestbench:
         self.now = 0
 
     def init(self):
-        yield from self.dut.master.rt_controller.csrs.underflow_margin.write(100)
         while not (yield from self.dut.master.link_layer.rx_up.read()):
             yield
         yield from self.get_buffer_space()
@@ -228,7 +227,7 @@ class TestFullStack(unittest.TestCase):
             yield from tb.init()
             errors = yield from saterr.protocol_error.read()
             self.assertEqual(errors, 0)
-            yield from csrs.underflow_margin.write(0)
+            yield from csrs.set_underflow_margin.write(0)
             tb.delay(100)
             yield from tb.write(42, 1)
             for i in range(12):
diff --git a/artiq/gateware/test/drtio/test_switching.py b/artiq/gateware/test/drtio/test_switching.py
index 66a9c9e69..589115abe 100644
--- a/artiq/gateware/test/drtio/test_switching.py
+++ b/artiq/gateware/test/drtio/test_switching.py
@@ -70,7 +70,6 @@ class Testbench:
         self.now = 0
 
     def init(self):
-        yield from self.dut.master.rt_controller.csrs.underflow_margin.write(100)
         while not (yield from self.dut.master.link_layer.rx_up.read()):
             yield
         yield from self.get_buffer_space()