From 504bb5116de31b32a2822c3ec06d4eec313339c0 Mon Sep 17 00:00:00 2001
From: Simon Renblad <srenblad@m-labs.hk>
Date: Tue, 26 Nov 2024 11:23:09 +0800
Subject: [PATCH] satman: replace forward with send for InjectionRequest

---
 artiq/firmware/satman/main.rs     | 16 +++++++++++++++-
 artiq/firmware/satman/repeater.rs |  6 +++---
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/artiq/firmware/satman/main.rs b/artiq/firmware/satman/main.rs
index b867a1bc1..17dd6fb93 100644
--- a/artiq/firmware/satman/main.rs
+++ b/artiq/firmware/satman/main.rs
@@ -215,7 +215,21 @@ fn process_aux_packet(_repeaters: &mut [repeater::Repeater],
             drtioaux::send(0, &reply)
         },
         drtioaux::Packet::InjectionRequest { destination: _destination, channel, overrd, value } => {
-            forward!(_routing_table, _destination, *_rank, _repeaters, &packet);
+            #[cfg(has_drtio_routing)]
+            {
+                let hop = _routing_table.0[_destination as usize][*_rank as usize];
+                if hop != 0 {
+                    let repno = (hop - 1) as usize;
+                    if repno < _repeaters.len() {
+                        if _repeaters[repno].state != repeater::RepeaterState::Up {
+                            return Err(drtioaux::Error::LinkDown);
+                        }
+                        return drtioaux::send(_repeaters[repno].auxno, &packet);
+                    } else {
+                        return Err(drtioaux::Error::RoutingError);
+                    }
+                }
+            }
             #[cfg(has_rtio_moninj)]
             unsafe {
                 csr::rtio_moninj::inj_chan_sel_write(channel as _);
diff --git a/artiq/firmware/satman/repeater.rs b/artiq/firmware/satman/repeater.rs
index 9969d5099..59b3a1970 100644
--- a/artiq/firmware/satman/repeater.rs
+++ b/artiq/firmware/satman/repeater.rs
@@ -12,7 +12,7 @@ fn rep_link_rx_up(repno: u8) -> bool {
 
 #[cfg(has_drtio_routing)]
 #[derive(Clone, Copy, PartialEq)]
-enum RepeaterState {
+pub enum RepeaterState {
     Down,
     SendPing { ping_count: u16 },
     WaitPingReply { ping_count: u16, timeout: u64 },
@@ -29,8 +29,8 @@ impl Default for RepeaterState {
 #[derive(Clone, Copy, Default)]
 pub struct Repeater {
     repno: u8,
-    auxno: u8,
-    state: RepeaterState
+    pub auxno: u8,
+    pub state: RepeaterState
 }
 
 #[cfg(has_drtio_routing)]