From 8dbf26016a630456ba2e914fb20c4805458dc650 Mon Sep 17 00:00:00 2001
From: Petteri Aimonen <jpa@git.mail.kapsi.fi>
Date: Mon, 13 Nov 2023 14:49:32 +0200
Subject: [PATCH] ptpd: Re-join multicast group after timeout

If multicast PTP packets are not being received, rejoin the multicast group.
This automatically recovers from situations such as rebooting a network switch.
---
 netutils/ptpd/Kconfig |  9 ++++++++
 netutils/ptpd/ptpd.c  | 51 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/netutils/ptpd/Kconfig b/netutils/ptpd/Kconfig
index 6c4099709..c662735aa 100644
--- a/netutils/ptpd/Kconfig
+++ b/netutils/ptpd/Kconfig
@@ -162,6 +162,15 @@ config NETUTILS_PTPD_SETTIME_THRESHOLD_MS
 		time is reset with settimeofday() instead of changing the rate with
 		adjtime().
 
+config NETUTILS_PTPD_MULTICAST_TIMEOUT_MS
+	int "PTP client timeout to rejoin multicast group (ms)"
+	default 30000
+	---help---
+		If no PTP multicast packets are being received, attempt to rejoin the
+		multicast group. This can be necessary if network topology changes, or
+		depending on hardware, after some error recovery events.
+		Set to 0 to disable.
+
 endif # NETUTILS_PTPD_CLIENT
 
 endif # NETUTILS_PTPD
diff --git a/netutils/ptpd/ptpd.c b/netutils/ptpd/ptpd.c
index 178d4185f..61ea06b72 100644
--- a/netutils/ptpd/ptpd.c
+++ b/netutils/ptpd/ptpd.c
@@ -93,6 +93,7 @@ struct ptp_state_s
 
   struct ptp_announce_s selected_source;
   struct timespec last_received_sync;
+  struct timespec last_received_multicast;
 
   /* Last transmitted sync & announcement packets */
 
@@ -399,6 +400,8 @@ static int ptp_initialize_state(struct ptp_state_s *state,
   bind_addr.sin_family = AF_INET;
   bind_addr.sin_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
 
+  clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);
+
   ret = ipmsfilter(&state->interface_addr.sin_addr,
                    &bind_addr.sin_addr,
                    MCAST_INCLUDE);
@@ -476,6 +479,45 @@ static int ptp_destroy_state(struct ptp_state_s *state)
   return OK;
 }
 
+/* Re-subscribe multicast address.
+ * This can become necessary if Ethernet interface gets reset or if external
+ * IGMP-compliant Ethernet switch gets plugged in.
+ */
+
+static int ptp_check_multicast_status(struct ptp_state_s *state)
+{
+#if CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS > 0
+  struct in_addr mcast_addr;
+  struct timespec time_now;
+  struct timespec delta;
+
+  clock_gettime(CLOCK_MONOTONIC, &time_now);
+  clock_timespec_subtract(&time_now, &state->last_received_multicast,
+                          &delta);
+
+  if (timespec_to_ms(&delta) > CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS)
+    {
+      /* Remove and re-add the multicast group */
+
+      state->last_received_multicast = time_now;
+
+      mcast_addr.s_addr = HTONL(PTP_MULTICAST_ADDR);
+      ipmsfilter(&state->interface_addr.sin_addr,
+                  &mcast_addr,
+                  MCAST_EXCLUDE);
+
+      return ipmsfilter(&state->interface_addr.sin_addr,
+                        &mcast_addr,
+                        MCAST_INCLUDE);
+    }
+
+#else
+  UNUSED(state);
+#endif /* CONFIG_NETUTILS_PTPD_MULTICAST_TIMEOUT_MS */
+
+  return OK;
+}
+
 /* Send PTP server announcement packet */
 
 static int ptp_send_announce(struct ptp_state_s *state)
@@ -779,6 +821,8 @@ static int ptp_process_rx_packet(struct ptp_state_s *state, ssize_t length)
       return OK;
     }
 
+  clock_gettime(CLOCK_MONOTONIC, &state->last_received_multicast);
+
   switch (state->rxbuf.header.messagetype & PTP_MSGTYPE_MASK)
   {
 #ifdef CONFIG_NETUTILS_PTPD_CLIENT
@@ -877,6 +921,13 @@ static int ptp_daemon(int argc, FAR char** argv)
             }
         }
 
+      if (pollfds[0].revents == 0 && pollfds[1].revents == 0)
+        {
+          /* No packets received, check for multicast timeout */
+
+          ptp_check_multicast_status(state);
+        }
+
       ptp_periodic_send(state);
     }