Implement GAP scan in ClaimToken state

src/fdl/active.rs

@@ -91,6 +91,15 @@ impl UseTokenData {
     }
 }
 
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+#[repr(u8)]
+enum ClaimTokenStep {
+    FirstToken,
+    SecondToken,
+    Scan,
+    ScanAwaitResponse { address: crate::Address },
+}
+
 #[derive(Debug, PartialEq, Eq)]
 enum State {
     Offline,
@@ -109,7 +118,7 @@ enum State {
         first_cycle_done: bool,
     },
     ClaimToken {
-        first: bool,
+        step: ClaimTokenStep,
     },
     AwaitDataResponse {
         address: crate::Address,
@@ -217,7 +226,9 @@ impl State {
             self,
             State::ClaimToken { .. } | State::ListenToken { .. } | State::ActiveIdle { .. }
         );
-        *self = State::ClaimToken { first: true };
+        *self = State::ClaimToken {
+            step: ClaimTokenStep::FirstToken,
+        };
     }
 
     fn transition_await_data_response(&mut self, address: crate::Address, data: UseTokenData) {
@@ -315,9 +326,9 @@ impl State {
         }
     }
 
-    fn get_claim_token_first(&mut self) -> &mut bool {
+    fn get_claim_token_step(&mut self) -> &mut ClaimTokenStep {
         match self {
-            Self::ClaimToken { first, .. } => first,
+            Self::ClaimToken { step, .. } => step,
             _ => unreachable!(),
         }
     }
@@ -654,7 +665,29 @@ impl FdlActiveStation {
             None
         }
     }
+}
+
+#[derive(Debug, PartialEq, Eq, Clone, Copy)]
+#[repr(u8)]
+enum GapPollResponse {
+    /// The slot expired without receiving any response.
+    ///
+    /// Bus is immediately free for further activity.
+    NoResponse,
+
+    /// Station responded.
+    ///
+    /// Bus will need a wait time until ready for next activity.
+    StationResponded,
 
+    /// Unexpected telegram was received.
+    ///
+    /// It is probably best to back off into ActiveIdle and wait for another active station to call
+    /// us again.
+    UnexpectedTelegram,
+}
+
+impl FdlActiveStation {
     fn next_gap_poll(&self, current_address: crate::Address) -> GapState {
         let next_station = self.token_ring.next_station();
         let next_address = if current_address == (self.p.highest_station_address - 1) {
@@ -681,6 +714,77 @@ impl FdlActiveStation {
             }
         }
     }
+
+    fn transmit_gap_poll_if_pending<PHY: ProfibusPhy>(
+        &mut self,
+        now: crate::time::Instant,
+        phy: &mut PHY,
+    ) -> Option<(PollDone, crate::Address)> {
+        if let GapState::DoPoll { current_address } = self.gap_state {
+            debug_assert_ne!(current_address, self.p.address);
+
+            let tx_res = phy
+                .transmit_telegram(now, |tx| {
+                    Some(tx.send_fdl_status_request(current_address, self.p.address))
+                })
+                .unwrap();
+
+            Some((self.mark_tx(now, tx_res.bytes_sent()), current_address))
+        } else {
+            None
+        }
+    }
+
+    fn await_gap_poll_response<PHY: ProfibusPhy>(
+        &mut self,
+        now: crate::time::Instant,
+        phy: &mut PHY,
+        poll_address: crate::Address,
+    ) -> Result<GapPollResponse, PollDone> {
+        debug_assert_ne!(poll_address, self.p.address);
+        debug_assert!(
+            matches!(self.gap_state, GapState::DoPoll { current_address } if current_address == poll_address)
+        );
+
+        // Here we conservatively only receive the first pending telegram because it is very
+        // unlikely that some other station randomly stole our token.  If it did, we will notice in
+        // the next poll cycle.
+        let received = phy.receive_telegram(now, |telegram| {
+            self.mark_rx(now);
+
+            if let crate::fdl::Telegram::Data(telegram) = &telegram {
+                if telegram.h.sa == poll_address && telegram.h.da == self.p.address {
+                    if let crate::fdl::FunctionCode::Response { state, status } = telegram.h.fc {
+                        log::trace!("Address #{poll_address} responded");
+
+                        if status == crate::fdl::ResponseStatus::Ok
+                            && matches!(state, crate::fdl::ResponseState::MasterWithoutToken | crate::fdl::ResponseState::MasterInRing) {
+                            self.token_ring.set_next_station(poll_address);
+                        }
+
+                        return GapPollResponse::StationResponded;
+                    }
+                }
+            }
+
+            // TODO: We should probably differentiate a bit here. If this is a malformed response
+            // from the right station, we don't have to back off from the bus entirely...
+            // Ref: wohp7Aex
+            log::warn!("Received unexpected telegram while waiting for status reply from #{poll_address}: {telegram:?}");
+            return GapPollResponse::UnexpectedTelegram;
+        });
+
+        if let Some(res) = received {
+            return Ok(res);
+        }
+
+        if self.check_slot_expired(now) {
+            log::trace!("No reply from #{poll_address}");
+            Ok(GapPollResponse::NoResponse)
+        } else {
+            Err(PollDone::waiting_for_bus())
+        }
+    }
 }
 
 /// State Machine of the FDL active station
@@ -916,26 +1020,79 @@ impl FdlActiveStation {
     ) -> PollDone {
         debug_assert_state!(self.state, State::ClaimToken { .. });
 
-        // The token is claimed by sending a telegram to ourselves twice.
-        return_if_done!(self.wait_synchronization_pause(now));
-        let tx_res = phy
-            .transmit_telegram(now, |tx| {
-                Some(tx.send_token_telegram(self.p.address, self.p.address))
-            })
-            .unwrap();
+        // TODO: Work on https://github.com/Rahix/profirust/issues/25
+        match *self.state.get_claim_token_step() {
+            s @ ClaimTokenStep::FirstToken | s @ ClaimTokenStep::SecondToken => {
+                // The token is claimed by sending a telegram to ourselves twice.
+                return_if_done!(self.wait_synchronization_pause(now));
+                let tx_res = phy
+                    .transmit_telegram(now, |tx| {
+                        Some(tx.send_token_telegram(self.p.address, self.p.address))
+                    })
+                    .unwrap();
 
-        self.token_ring.claim_token();
+                self.token_ring.claim_token();
 
-        if *self.state.get_claim_token_first() {
-            // This will lead to sending the claim token telegram again
-            *self.state.get_claim_token_first() = false;
-        } else {
-            // Now we have claimed the token and can proceed to use it.
-            self.state
-                .transition_use_token(UseTokenData::with_token_time(now));
-        }
+                *self.state.get_claim_token_step() = match s {
+                    ClaimTokenStep::FirstToken => ClaimTokenStep::SecondToken,
+                    ClaimTokenStep::SecondToken => ClaimTokenStep::Scan,
+                    _ => unreachable!(),
+                };
 
-        self.mark_tx(now, tx_res.bytes_sent())
+                // So we will start scanning at the next address following ourselves.
+                self.gap_state = GapState::DoPoll {
+                    current_address: self.p.address,
+                };
+
+                self.mark_tx(now, tx_res.bytes_sent())
+            }
+            ClaimTokenStep::Scan => {
+                return_if_done!(self.wait_synchronization_pause(now));
+
+                match &mut self.gap_state {
+                    GapState::Waiting { .. } => {
+                        // We are done scanning the gap, let's proceed
+                        log::trace!("Polled full GAP after claiming token, passing on...");
+                        self.state
+                            .transition_pass_token(false, PassTokenAttempt::First);
+                        return PollDone::waiting_for_delay();
+                    }
+                    GapState::DoPoll { current_address } => {
+                        let current_address = *current_address;
+                        self.gap_state = self.next_gap_poll(current_address);
+                    }
+                }
+
+                if let Some((res, address)) = self.transmit_gap_poll_if_pending(now, phy) {
+                    *self.state.get_claim_token_step() =
+                        ClaimTokenStep::ScanAwaitResponse { address };
+                    res
+                } else {
+                    PollDone::waiting_for_delay()
+                }
+            }
+            ClaimTokenStep::ScanAwaitResponse { address } => {
+                match self.await_gap_poll_response(now, phy, address) {
+                    Err(poll_done) => poll_done,
+                    Ok(GapPollResponse::StationResponded) => {
+                        *self.state.get_claim_token_step() = ClaimTokenStep::Scan;
+                        PollDone::waiting_for_delay()
+                    }
+                    Ok(GapPollResponse::NoResponse) => {
+                        *self.state.get_claim_token_step() = ClaimTokenStep::Scan;
+                        // Recursive call to immediately handle the next scan
+                        self.do_claim_token(now, phy)
+                    }
+                    Ok(GapPollResponse::UnexpectedTelegram) => {
+                        // TODO: For now, let's play it safe and back off from the bus entirely if an
+                        // unexpected telegram is received.
+                        // Ref: wohp7Aex
+                        self.state.transition_active_idle();
+                        PollDone::waiting_for_bus()
+                    }
+                }
+            }
+        }
     }
 
     #[must_use = "poll done marker"]
@@ -1130,18 +1287,9 @@ impl FdlActiveStation {
                 }
             }
 
-            if let GapState::DoPoll { current_address } = self.gap_state {
-                debug_assert_ne!(current_address, self.p.address);
-
-                let tx_res = phy
-                    .transmit_telegram(now, |tx| {
-                        Some(tx.send_fdl_status_request(current_address, self.p.address))
-                    })
-                    .unwrap();
-
-                self.state.transition_await_status_response(current_address);
-
-                return self.mark_tx(now, tx_res.bytes_sent());
+            if let Some((res, poll_address)) = self.transmit_gap_poll_if_pending(now, phy) {
+                self.state.transition_await_status_response(poll_address);
+                return res;
             }
         }
 
@@ -1175,45 +1323,27 @@ impl FdlActiveStation {
 
         let address = *self.state.get_await_status_response_address();
 
-        // Here we conservatively only receive the first pending telegram because it is very
-        // unlikely that some other station randomly stole our token.  If it did, we will notice in
-        // the next poll cycle.
-        let received = phy.receive_telegram(now, |telegram| {
-            self.mark_rx(now);
-
-            if let crate::fdl::Telegram::Data(telegram) = &telegram {
-                if telegram.h.sa == address && telegram.h.da == self.p.address {
-                    if let crate::fdl::FunctionCode::Response { state, status } = telegram.h.fc {
-                        log::trace!("Address #{address} responded");
-                        if status == crate::fdl::ResponseStatus::Ok
-                            && matches!(state, crate::fdl::ResponseState::MasterWithoutToken | crate::fdl::ResponseState::MasterInRing) {
-                            self.token_ring.set_next_station(address);
-                        }
-                        self.state.transition_pass_token(false, PassTokenAttempt::First);
-                        return PollDone::waiting_for_delay();
-                    }
-                }
-
+        match self.await_gap_poll_response(now, phy, address) {
+            Err(poll_done) => poll_done,
+            Ok(GapPollResponse::StationResponded) => {
+                self.state
+                    .transition_pass_token(false, PassTokenAttempt::First);
+                PollDone::waiting_for_delay()
+            }
+            Ok(GapPollResponse::NoResponse) => {
+                self.state
+                    .transition_pass_token(false, PassTokenAttempt::First);
+                // Immediately evaluate PassToken state because the bus is free for immediate
+                // transmission
+                self.do_pass_token(now, phy)
+            }
+            Ok(GapPollResponse::UnexpectedTelegram) => {
+                // TODO: For now, let's play it safe and back off from the bus entirely if an
+                // unexpected telegram is received.
+                // Ref: wohp7Aex
+                self.state.transition_active_idle();
+                PollDone::waiting_for_bus()
             }
-
-            log::warn!("Received unexpected telegram while waiting for status reply from #{address}: {telegram:?}");
-            self.state.transition_active_idle();
-            PollDone::waiting_for_bus()
-        });
-
-        if let Some(res) = received {
-            return res;
-        }
-
-        if self.check_slot_expired(now) {
-            log::trace!("No reply from #{address}");
-            self.state
-                .transition_pass_token(false, PassTokenAttempt::First);
-            // Immediately evaluate PassToken state because the bus is free for immediate
-            // transmission
-            self.do_pass_token(now, phy)
-        } else {
-            PollDone::waiting_for_bus()
         }
     }
 

src/fdl/test_active.rs

@@ -584,6 +584,44 @@ fn active_station_discovers_neighbor() {
     fdl_ut.wait_for_matching(|t| t == fdl::Telegram::Token(fdl::TokenTelegram { da: 4, sa: 7 }));
 }
 
+/// Test that an active station discovers another active neighbor station immediately after
+/// claiming the token.
+#[test]
+fn active_station_discovers_neighbor_after_claim() {
+    crate::test_utils::prepare_test_logger();
+    let mut fdl_ut = FdlActiveUnderTest::new(7);
+
+    fdl_ut.assert_next_telegram(fdl::Telegram::Token(fdl::TokenTelegram { da: 7, sa: 7 }));
+    fdl_ut.assert_next_telegram(fdl::Telegram::Token(fdl::TokenTelegram { da: 7, sa: 7 }));
+
+    fdl_ut.wait_for_matching(|t| {
+        // The active station must not do any token passing until the gap poll is completed.
+        assert!(!matches!(t, fdl::Telegram::Token(_)));
+
+        if let fdl::Telegram::Data(data_telegram) = t {
+            assert!(data_telegram.is_fdl_status_request().is_some());
+            data_telegram.h.da == 4 && data_telegram.h.sa == 7
+        } else {
+            // The active station must not do anything but status requests until the gap poll is
+            // completed.
+            panic!("Not a data telegram: {t:?}")
+        }
+    });
+
+    fdl_ut.advance_bus_time_min_tsdr();
+    fdl_ut.transmit_telegram(|tx| {
+        Some(tx.send_fdl_status_response(
+            7,
+            4,
+            fdl::ResponseState::MasterWithoutToken,
+            fdl::ResponseStatus::Ok,
+        ))
+    });
+    fdl_ut.wait_transmission();
+
+    fdl_ut.assert_next_telegram(fdl::Telegram::Token(fdl::TokenTelegram { da: 4, sa: 7 }));
+}
+
 /// Test that an active station discovers a direct (active) neighbor station.
 #[test]
 fn active_station_discovers_direct_neighbor() {
@@ -1010,8 +1048,9 @@ fn slot_time_timing() {
 
     log::debug!("After receiving request...");
 
-    let time =
-        fdl_ut.assert_next_telegram(fdl::Telegram::Token(fdl::TokenTelegram { da: 7, sa: 7 }));
+    let (time, _) = fdl_ut.wait_next_telegram(|t| {
+        assert_eq!(t.source_address(), Some(7));
+    });
 
     // We have to subtract the telegram runtime of the just received token telegram
     let time = time - fdl_ut.bits_to_time(33);