diff --git a/artiq/firmware/libboard_artiq/drtio_routing.rs b/artiq/firmware/libboard_artiq/drtio_routing.rs
index 6c8469ae25..562517342d 100644
--- a/artiq/firmware/libboard_artiq/drtio_routing.rs
+++ b/artiq/firmware/libboard_artiq/drtio_routing.rs
@@ -30,18 +30,6 @@ 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 95b43c586e..69233dd609 100644
--- a/artiq/firmware/runtime/rtio_mgt.rs
+++ b/artiq/firmware/runtime/rtio_mgt.rs
@@ -54,12 +54,6 @@ 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();
@@ -69,23 +63,6 @@ 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 3209f1739b..40014dce04 100644
--- a/artiq/gateware/drtio/rt_controller_master.py
+++ b/artiq/gateware/drtio/rt_controller_master.py
@@ -17,13 +17,11 @@ def __init__(self):
         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)
@@ -118,19 +116,9 @@ def __init__(self, tsc, rt_packet):
         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:]
-                           - underflow_margin_port.dat_r[tsc.glbl_fine_ts_width:]) < tsc.coarse_ts_sys)
+                           - self.csrs.underflow_margin.storage[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 a4e239b999..579d78f9b3 100644
--- a/artiq/gateware/test/drtio/test_full_stack.py
+++ b/artiq/gateware/test/drtio/test_full_stack.py
@@ -89,6 +89,7 @@ def __init__(self):
         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()
@@ -227,7 +228,7 @@ def test():
             yield from tb.init()
             errors = yield from saterr.protocol_error.read()
             self.assertEqual(errors, 0)
-            yield from csrs.set_underflow_margin.write(0)
+            yield from csrs.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 589115abe1..66a9c9e695 100644
--- a/artiq/gateware/test/drtio/test_switching.py
+++ b/artiq/gateware/test/drtio/test_switching.py
@@ -70,6 +70,7 @@ def __init__(self):
         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()