diff --git a/src/satman/src/main.rs b/src/satman/src/main.rs index ceae24d..c451b50 100644 --- a/src/satman/src/main.rs +++ b/src/satman/src/main.rs @@ -227,7 +227,22 @@ fn process_aux_packet(_repeaters: &mut [repeater::Repeater], }, drtioaux::Packet::InjectionRequest { destination: _destination, channel, overrd, value } => { - forward!(_routing_table, _destination, *_rank, _repeaters, &packet, timer); + #[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/src/satman/src/repeater.rs b/src/satman/src/repeater.rs index ed12365..159bcc0 100644 --- a/src/satman/src/repeater.rs +++ b/src/satman/src/repeater.rs @@ -18,7 +18,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: Milliseconds }, @@ -35,8 +35,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)]