Try to "Qualify" Announce messages coming in from clock masters. Decode them on debug...

diff --git a/Makefile.am b/Makefile.am
index 87e67b1ebe43510b9abba12be2498b9c5bf657da..b36e961119d572b7f5f812c931c11d1cf1d3910d 100644 (file)
--- a/Makefile.am
+++ b/Makefile.am
@@ -1,5 +1,5 @@
 bin_PROGRAMS = nqptp
-nqptp_SOURCES = nqptp.c nqptp-clock-sources.c nqptp-utilities.c general-utilities.c debug.c
+nqptp_SOURCES = nqptp.c nqptp-clock-sources.c nqptp-message-handlers.c nqptp-utilities.c general-utilities.c debug.c
 
 AM_CFLAGS = -fno-common -Wno-multichar -Wall -Wextra -Wno-clobbered -Wno-psabi -pthread
 

diff --git a/nqptp-clock-sources.c b/nqptp-clock-sources.c
index 5eda7dd395fe6478b05908520cd9a325f1cfa756..13c31a627c78441d397ab23f1515c421ac0afc98 100644 (file)
--- a/nqptp-clock-sources.c
+++ b/nqptp-clock-sources.c
@@ -77,7 +77,7 @@ int create_clock_source_record(char *sender_string, uint64_t packet_clock_id,
     clocks_private_info[i].in_use = 1;
     clocks_private_info[i].t2 = 0;
     clocks_private_info[i].current_stage = waiting_for_sync;
-    debug(1, "activated source %d with clock_id %" PRIx64 " on ip: %s.", i,
+    debug(2, "activated source %d with clock_id %" PRIx64 " on ip: %s.", i,
           clocks_shared_info[i].clock_id, &clocks_shared_info[i].ip);
   } else {
     die("Clock tables full!");
@@ -100,7 +100,7 @@ void manage_clock_sources(uint64_t reception_time, clock_source *clocks_shared_i
       // seconds to nanoseconds
       syncTimeout = syncTimeout * 1000000000;
       if (time_since_last_sync > syncTimeout) {
-        debug(1, "deactivating source %d with clock_id %" PRIx64 " on ip: %s.", i,
+        debug(2, "deactivated source %d with clock_id %" PRIx64 " on ip: %s.", i,
               clocks_shared_info[i].clock_id, &clocks_shared_info[i].ip);
         int rc = pthread_mutex_lock(&shared_memory->shm_mutex);
         if (rc != 0)

diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h
index 448929f6381f824c8b9fa366459d1bee226c2ec0..07f398c935c796df240e17cf3d783d1ebb88644b 100644 (file)
--- a/nqptp-clock-sources.h
+++ b/nqptp-clock-sources.h
@@ -36,7 +36,6 @@ typedef struct {
   uint64_t t2;
   // for Announce Qualification
   uint64_t announce_times[4];  // we'll check qualification and currency using these
-  int announce_times_valid_count;
   int announce_is_valid; // this may mean it's a master clock_source
 } clock_source_private_data;
 

diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c
new file mode 100644 (file)
index 0000000..a942246
--- /dev/null
+++ b/nqptp-message-handlers.c
@@ -0,0 +1,90 @@
+/*
+ * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
+ * Copyright (c) 2021 Mike Brady.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 2.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Commercial licensing is also available.
+ */
+
+#include "nqptp-ptp-definitions.h"
+#include "nqptp-message-handlers.h"
+#include "nqptp-utilities.h"
+
+#include "general-utilities.h"
+#include "debug.h"
+
+void handle_announce(char *buf, ssize_t recv_len, clock_source* clock_info, clock_source_private_data *clock_private_info, uint64_t reception_time) {
+  // debug_print_buffer(1, buf, (size_t) recv_len);
+  // make way for the new time
+  if ((size_t)recv_len >= sizeof(struct ptp_announce_message)) {
+  struct ptp_announce_message *msg = (struct ptp_announce_message *)buf;
+  int i;
+  // number of elements in the array is 4, hence the 4-1 stuff
+  for (i = 4-1; i > 1-1; i--) {
+    clock_private_info->announce_times[i] = clock_private_info->announce_times[i - 1];
+  };
+  clock_private_info->announce_times[0] = reception_time;
+
+  // so, we have added a new element and assumed that
+  // now we need to walk down the array checking that non of the elements are too old
+  i = 0;
+  int valid_count = 0;
+  int finished = 0;
+
+  // see 9.3.2.4.4 and 9.3.2.5
+  uint64_t foreign_master_time_window = 1;
+  foreign_master_time_window = foreign_master_time_window  << (32 + aPTPinitialLogAnnounceInterval);
+  foreign_master_time_window = foreign_master_time_window * 4;
+  foreign_master_time_window = foreign_master_time_window >> 32; // should be 4 seconds
+
+  uint64_t cutoff_time = reception_time + foreign_master_time_window;
+  int foreign_master_threshold = 2;
+  while ((i < 4) && (finished == 0)) {
+    int64_t delta = cutoff_time - clock_private_info->announce_times[i];
+    if (delta > 0)
+      valid_count++;
+    else
+      finished = 1;
+    i++;
+  }
+  if (valid_count >= foreign_master_threshold) {
+    if (clock_private_info->announce_is_valid == 0) {
+      uint64_t grandmaster_clock_id = nctohl(&msg->announce.grandmasterIdentity[0]);
+      uint64_t grandmaster_clock_id_low = nctohl(&msg->announce.grandmasterIdentity[4]);
+      grandmaster_clock_id = grandmaster_clock_id << 32;
+      grandmaster_clock_id = grandmaster_clock_id + grandmaster_clock_id_low;
+
+      debug(1, "clock_id %" PRIx64 " on ip: %s, \"Announce\" message is Qualified -- See 9.3.2.5.",
+          clock_info->clock_id, clock_info->ip);
+      uint32_t clockQuality = msg->announce.grandmasterClockQuality;
+      uint8_t clockClass = (clockQuality >> 24) & 0xff;
+      uint8_t clockAccuracy = (clockQuality >> 16) & 0xff;
+      uint16_t offsetScaledLogVariance = clockQuality & 0xffff;
+      debug(1, "    grandmasterIdentity:     %" PRIx64 ".", grandmaster_clock_id);
+      debug(1, "    grandmasterPriority1:    %u.", msg->announce.grandmasterPriority1);
+      debug(1, "    grandmasterClockQuality,: 0x%x.", msg->announce.grandmasterClockQuality);
+      debug(1, "        clockClass:              %u.", clockClass); // See 7.6.2.4 clockClass
+      debug(1, "        clockAccuracy:           0x%x.", clockAccuracy); // See 7.6.2.5 clockAccuracy
+      debug(1, "        offsetScaledLogVariance: %x.", offsetScaledLogVariance); // See 7.6.3 PTP variance
+      debug(1, "    grandmasterPriority2:    %u.", msg->announce.grandmasterPriority2);
+    }
+    clock_private_info->announce_is_valid = 1;
+  } else {
+    if (clock_private_info->announce_is_valid != 0)
+      debug(1, "clock_id %" PRIx64 " on ip: %s \"Announce\" message is not Qualified -- See 9.3.2.5.",
+          clock_info->clock_id, clock_info->ip);
+    clock_private_info->announce_is_valid = 0;
+  }
+  }
+}

diff --git a/nqptp-message-handlers.h b/nqptp-message-handlers.h
new file mode 100644 (file)
index 0000000..3101992
--- /dev/null
+++ b/nqptp-message-handlers.h
@@ -0,0 +1,28 @@
+/*
+ * This file is part of the nqptp distribution (https://github.com/mikebrady/nqptp).
+ * Copyright (c) 2021 Mike Brady.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 2.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Commercial licensing is also available.
+ */
+
+#ifndef NQPTP_MESSAGE_HANDLERS_H
+#define NQPTP_MESSAGE_HANDLERS_H
+
+#include "nqptp-shm-structures.h"
+#include "nqptp-clock-sources.h"
+
+void handle_announce(char *buf, ssize_t recv_len, clock_source* clock_info, clock_source_private_data *clock_private_info, uint64_t reception_time);
+
+#endif
\ No newline at end of file

diff --git a/nqptp-ptp-definitions.h b/nqptp-ptp-definitions.h
index 21f84596ae2e24a355648be7c17a9a3818c541d2..03aad0a1f3b95202715f2a928074eee507b40388 100644 (file)
--- a/nqptp-ptp-definitions.h
+++ b/nqptp-ptp-definitions.h
@@ -20,6 +20,8 @@
 #ifndef NQPTP_PTP_DEFINITIONS_H
 #define NQPTP_PTP_DEFINITIONS_H
 
+#include <inttypes.h>
+
 // This is for definitions and stuff that flows more or less directly
 // from external sources.
 

diff --git a/nqptp.c b/nqptp.c
index 9ae42e2ce4ddf5092209062b1ee329a5432e05db..656b9eb07974fe409b6cbf45f0a75ae960aa8602 100644 (file)
--- a/nqptp.c
+++ b/nqptp.c
@@ -24,6 +24,7 @@
 #include "nqptp.h"
 #include "nqptp-ptp-definitions.h"
 #include "nqptp-clock-sources.h"
+#include "nqptp-message-handlers.h"
 #include "nqptp-utilities.h"
 #include "general-utilities.h"
 #include "debug.h"
@@ -347,6 +348,9 @@ int main(void) {
               }
               if (the_clock != -1) {
                 switch (buf[0] & 0xF) {
+                case Announce:
+                  handle_announce(buf, recv_len, &shared_memory->clocks[the_clock],&clocks_private[the_clock], reception_time);
+                  break;
                 case Sync: { // if it's a sync
                   struct ptp_sync_message *msg = (struct ptp_sync_message *)buf;
                   // this is just to see if anything interesting comes in the SYNC package