From: Mike Brady <4265913+mikebrady@users.noreply.github.com>
Date: Thu, 14 Jul 2022 16:55:38 +0000 (+0100)
Subject: Make the code to restart a clock a separate function and call it if a clock gets... 
X-Git-Tag: 1.2~1^2~37
X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=bf4d7c3d6a96d7b38f5e05532efe7b3b2705b439;p=thirdparty%2Fnqptp.git

Make the code to restart a clock a separate function and call it if a clock gets stuck. Include the correction field in the calculation of the precise origin timestamp. If a new clock is out by more than -4 seconds, treat it as a discontinuity rather than noise.
---

diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h
index 46740fa..1c81840 100644
--- a/nqptp-clock-sources.h
+++ b/nqptp-clock-sources.h
@@ -37,7 +37,7 @@ typedef struct {
   int follow_up_number;
   int announcements_without_followups; // add 1 for every announce, reset with a followup
   uint64_t clock_id;
-  uint64_t previous_offset, previous_offset_time, previous_offset_grandmaster;
+  uint64_t previous_offset, previous_offset_time, previous_offset_grandmaster, previous_preciseOriginTimestamp;
   uint64_t mastership_start_time; // set to the time of the first sample used as master
 
   // for garbage collection
@@ -57,6 +57,7 @@ typedef struct {
   uint8_t grandmasterPriority2;
   uint64_t grandmasterIdentity;
   uint16_t stepsRemoved;
+  int identical_previous_preciseOriginTimestamp_count;
 
 } clock_source_private_data;
 
diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c
index a5e6514..e2a3980 100644
--- a/nqptp-message-handlers.c
+++ b/nqptp-message-handlers.c
@@ -236,142 +236,197 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source_private_data *clo
   }
 }
 
-void handle_follow_up(char *buf, __attribute__((unused)) ssize_t recv_len,
-                      clock_source_private_data *clock_private_info, uint64_t reception_time) {
-
-  struct ptp_follow_up_message *msg = (struct ptp_follow_up_message *)buf;
-
-  uint16_t seconds_hi = nctohs(&msg->follow_up.preciseOriginTimestamp[0]);
-  uint32_t seconds_low = nctohl(&msg->follow_up.preciseOriginTimestamp[2]);
-  uint32_t nanoseconds = nctohl(&msg->follow_up.preciseOriginTimestamp[6]);
-  uint64_t preciseOriginTimestamp = seconds_hi;
-  preciseOriginTimestamp = preciseOriginTimestamp << 32;
-  preciseOriginTimestamp = preciseOriginTimestamp + seconds_low;
-  preciseOriginTimestamp = preciseOriginTimestamp * 1000000000L;
-  preciseOriginTimestamp = preciseOriginTimestamp + nanoseconds;
-  
-  // update our sample information
-
-  if (clock_private_info->follow_up_number < 100)
-    clock_private_info->follow_up_number++;
-
-  if (clock_private_info->announcements_without_followups < 4) // if we haven't signalled already
-    clock_private_info->announcements_without_followups = 0;   // we've seen a followup
-
-  debug(2, "FOLLOWUP from %" PRIx64 ", %s.", clock_private_info->clock_id, &clock_private_info->ip);
-  uint64_t offset = preciseOriginTimestamp - reception_time;
-
-  int64_t jitter = 0;
-
-  int64_t time_since_previous_offset = 0;
-  uint64_t smoothed_offset = offset;
-  
-  // This is a bit hacky.
-  // Basically, the idea is that if the grandmaster has changed, then acceptance checking and smoothing
-  // should start as it it's a new clock. This is because the preciseOriginTimestamp, which is part of
-  // the data that is being smoothed, refers to the grandmaster, so when the grandmaster changes
-  // any previous calculations are no longer valid.
-  // The hacky bit is to signal this condition by zeroing the previous_offset_time.
-  if (clock_private_info->previous_offset_grandmaster != clock_private_info->grandmasterIdentity)
-    clock_private_info->previous_offset_time = 0; // the preciseOriginTimestamp always (?) refers to the grandmaster
-
-  if (clock_private_info->previous_offset_time != 0) {
-    time_since_previous_offset = reception_time - clock_private_info->previous_offset_time;
-  }
-
-  // Do acceptance checking and smoothing.
-
-  // Positive changes in the offset are much more likely to be
-  // legitimate, since they could only occur due to a shorter
-  // propagation time or less of a delay sending or receiving the packet.
-  // (Actually, this is not quite true --
-  // it is possible that the remote clock could be adjusted forward
-  // and this would increase the offset too.)
-  // Anyway, when the clock is new, we give extra preferential weighting to
-  // positive changes in the offset.
-
-  // If the new offset is greater, by any amount, than the old offset,
-  // or if it is less by up to 10 mS, accept it.
-  // Otherwise, drop it if the last sample was fairly recent
-  // If the last sample was long ago, take this as a discontinuity and
-  // accept it as the start of a new period of mastership.
-
-  // This seems to be quite stable
-
-  if (clock_private_info->previous_offset_time != 0)
-    jitter = offset - clock_private_info->previous_offset;
-
-  // We take any positive or a limited negative jitter as a sync event in
-  // a continuous synchronisation sequence.
-  // This works well with PTP sources that sleep, as when they sleep
-  // their clock stops. When they awaken, the offset from
-  // the local clock to them must be smaller than before, triggering the
-  // timing discontinuity below and allowing an immediate readjustment.
-
-  // The full value of a positive offset jitter is accepted for a
-  // number of follow_ups at the start.
-  // After that, the weight of the jitter is reduced.
-  // Follow-ups don't always come in at 125 ms intervals, especially after a discontinuity
-  // Delays makes the offsets smaller than they should be, which is quickly
-  // allowed for.
-
-  if ((clock_private_info->previous_offset_time != 0) && (jitter > -10000000)) {
-
-    if (jitter < 0) {
-      if (clock_private_info->follow_up_number < (5 * 8)) // at the beginning (8 samples per second)
-        smoothed_offset = clock_private_info->previous_offset + jitter / 16;
-      else
-        smoothed_offset = clock_private_info->previous_offset + jitter / 64;
-    } else if (clock_private_info->follow_up_number <
-               (5 * 8)) // at the beginning (8 samples per second)
-      smoothed_offset =
-          clock_private_info->previous_offset + jitter / 1; // accept positive changes quickly
-    else
-      smoothed_offset = clock_private_info->previous_offset + jitter / 64;
+void handle_follow_up(char *buf, ssize_t recv_len, clock_source_private_data *clock_private_info,
+                      uint64_t reception_time) {
+  if (clock_private_info->clock_id == 0) {
+    debug(2,"Follow_Up received before announcement -- discarded.");
   } else {
-    // allow samples to disappear for up to a second
-    if ((time_since_previous_offset != 0) && (time_since_previous_offset < 1000000000)) {
-      smoothed_offset =
-          clock_private_info
-              ->previous_offset + 1; // if we have recent samples, forget the present sample...
-    } else {
-      if (clock_private_info->previous_offset_time == 0)
-        debug(2, "Clock %" PRIx64 " record (re)starting at %s.", clock_private_info->clock_id,
-              clock_private_info->ip);
-      else
-        debug(2,
-              "Timing discontinuity on clock %" PRIx64
-              " at %s: time_since_previous_offset: %.3f seconds.",
-              clock_private_info->clock_id, clock_private_info->ip,
-              0.000000001 * time_since_previous_offset);
-      smoothed_offset = offset;
-      clock_private_info->follow_up_number = 0;
-      clock_private_info->mastership_start_time =
-          reception_time; // mastership is reset to this time...
-    }
-  }
+    if ((recv_len >= 0) && ((size_t)recv_len >= sizeof(struct ptp_follow_up_message))) {
+      // debug_print_buffer(1, buf, recv_len);
+      struct ptp_follow_up_message *msg = (struct ptp_follow_up_message *)buf;
+      uint16_t seconds_hi = nctohs(&msg->follow_up.preciseOriginTimestamp[0]);
+      uint32_t seconds_low = nctohl(&msg->follow_up.preciseOriginTimestamp[2]);
+      uint32_t nanoseconds = nctohl(&msg->follow_up.preciseOriginTimestamp[6]);
+      uint64_t preciseOriginTimestamp = seconds_hi;
+      preciseOriginTimestamp = preciseOriginTimestamp << 32;
+      preciseOriginTimestamp = preciseOriginTimestamp + seconds_low;
+      preciseOriginTimestamp = preciseOriginTimestamp * 1000000000L;
+      preciseOriginTimestamp = preciseOriginTimestamp + nanoseconds;
+
+      // update our sample information
+
+
+      if (clock_private_info->previous_preciseOriginTimestamp == preciseOriginTimestamp) {
+        clock_private_info->identical_previous_preciseOriginTimestamp_count++;
+        
+        if ((clock_private_info->identical_previous_preciseOriginTimestamp_count == 5) && (clock_private_info->follow_up_number < 100)){
+          debug(1,"Clock %" PRIx64 "'s grandmaster clock has stopped or may not have been read.", clock_private_info->clock_id);
+
+        
+          debug(1, "Attempt to start a stopped clock %" PRIx64 ", at follow_up_number %u at IP %s.",
+            clock_private_info->clock_id, clock_private_info->follow_up_number,
+            clock_private_info->ip);
+          send_awakening_announcement_sequence(clock_private_info->clock_id, clock_private_info->ip,
+                                            clock_private_info->family, clock_private_info->grandmasterPriority1,
+                                            clock_private_info->grandmasterPriority2);
+        }
+      } else {
+        if (clock_private_info->identical_previous_preciseOriginTimestamp_count >= 20) {
+          debug(1,"Clock %" PRIx64 "'s grandmaster clock has started again...", clock_private_info->clock_id);
+          clock_private_info->identical_previous_preciseOriginTimestamp_count = 0;
+        }
+      }
 
-  clock_private_info->previous_offset_grandmaster = clock_private_info->grandmasterIdentity;
-  clock_private_info->previous_offset = smoothed_offset;
-  clock_private_info->previous_offset_time = reception_time;
-    
-/*
-  debug(1,
-      "Clock %" PRIx64 ", grandmaster %" PRIx64 " at %s. Offset: %" PRIx64 ", smoothed offset: %" PRIx64
-      ". Precise Origin Timestamp: %" PRIx64
-      ". Time since previous offset: %.3f milliseconds.",
-      clock_private_info->clock_id, clock_private_info->grandmasterIdentity, clock_private_info->ip, offset, smoothed_offset,
-      preciseOriginTimestamp, 0.000001 * time_since_previous_offset);
-*/
-
-  int temp_client_id;
-  for (temp_client_id = 0; temp_client_id < MAX_CLIENTS; temp_client_id++) {
-    if ((clock_private_info->client_flags[temp_client_id] & (1 << clock_is_master)) != 0) {
-      debug(2, "clock_is_master -- updating master clock info for client \"%s\"",
-            get_client_name(temp_client_id));
-      update_master_clock_info(temp_client_id, clock_private_info->clock_id,
-                               (const char *)&clock_private_info->ip, reception_time,
-                               smoothed_offset, clock_private_info->mastership_start_time);
+
+      clock_private_info->previous_preciseOriginTimestamp = preciseOriginTimestamp;
+      
+      // clang-format off
+      
+      // actually the precision timestamp needs to be corrected by the Follow_Up Correction_Field contents.
+      // According to IEEE Std 802.1AS-2020, paragraph 11.4.4.2.1:
+      /*
+      The value of the preciseOriginTimestamp field is the sourceTime of the ClockMaster entity of the Grandmaster PTP Instance,
+      when the associated Sync message was sent by that Grandmaster PTP Instance, with any fractional nanoseconds truncated (see 10.2.9).
+      The sum of the correctionFields in the Follow_Up and associated Sync messages, added to the preciseOriginTimestamp field of the Follow_Up message,
+      is the value of the synchronized time corresponding to the syncEventEgressTimestamp at the PTP Instance that sent the associated Sync message,
+      including any fractional nanoseconds.
+      */
+      
+      // clang-format on
+      
+      int64_t correction_field = ntoh64(msg->header.correctionField);
+      
+      // debug(1," Check ntoh64: in: %" PRIx64 ", out: %" PRIx64 ".", msg->header.correctionField, correction_field);
+      
+      correction_field = correction_field / 65536; //might be signed
+      uint64_t correctedPreciseOriginTimestamp = preciseOriginTimestamp + correction_field;
+      
+
+      if (clock_private_info->follow_up_number < 100)
+        clock_private_info->follow_up_number++;
+
+      // if (clock_private_info->announcements_without_followups < 4) // if we haven't signalled already
+        clock_private_info->announcements_without_followups = 0;   // we've seen a followup
+
+      debug(2, "FOLLOWUP from %" PRIx64 ", %s.", clock_private_info->clock_id,
+            &clock_private_info->ip);
+      uint64_t offset = correctedPreciseOriginTimestamp - reception_time;
+
+      int64_t jitter = 0;
+
+      int64_t time_since_previous_offset = 0;
+      uint64_t smoothed_offset = offset;
+
+      // This is a bit hacky.
+      // Basically, the idea is that if the grandmaster has changed, then acceptance checking and
+      // smoothing should start as it it's a new clock. This is because the correctedPreciseOriginTimestamp,
+      // which is part of the data that is being smoothed, refers to the grandmaster, so when the
+      // grandmaster changes any previous calculations are no longer valid. The hacky bit is to signal
+      // this condition by zeroing the previous_offset_time.
+      if (clock_private_info->previous_offset_grandmaster != clock_private_info->grandmasterIdentity)
+        clock_private_info->previous_offset_time =
+            0;
+
+      if (clock_private_info->previous_offset_time != 0) {
+        time_since_previous_offset = reception_time - clock_private_info->previous_offset_time;
+      }
+
+      // Do acceptance checking and smoothing.
+
+      // Positive changes in the offset are much more likely to be
+      // legitimate, since they could only occur due to a shorter
+      // propagation time or less of a delay sending or receiving the packet.
+      // (Actually, this is not quite true --
+      // it is possible that the remote clock could be adjusted forward
+      // and this would increase the offset too.)
+      // Anyway, when the clock is new, we give extra preferential weighting to
+      // positive changes in the offset.
+
+      // If the new offset is greater, by any amount, than the old offset,
+      // or if it is less by up to 10 mS, accept it.
+      // Otherwise, drop it if the last sample was fairly recent
+      // If the last sample was long ago, take this as a discontinuity and
+      // accept it as the start of a new period of mastership.
+
+      // This seems to be quite stable
+
+      if (clock_private_info->previous_offset_time != 0)
+        jitter = offset - clock_private_info->previous_offset;
+
+      // We take any positive or a limited negative jitter as a sync event in
+      // a continuous synchronisation sequence.
+      // This works well with PTP sources that sleep, as when they sleep
+      // their clock stops. When they awaken, the offset from
+      // the local clock to them must be smaller than before, triggering the
+      // timing discontinuity below and allowing an immediate readjustment.
+
+      // The full value of a positive offset jitter is accepted for a
+      // number of follow_ups at the start.
+      // After that, the weight of the jitter is reduced.
+      // Follow-ups don't always come in at 125 ms intervals, especially after a discontinuity
+      // Delays makes the offsets smaller than they should be, which is quickly
+      // allowed for.
+
+      if ((clock_private_info->previous_offset_time != 0) && (jitter > -10000000)) {
+
+        if (jitter < 0) {
+          if (clock_private_info->follow_up_number <
+              (5 * 8)) // at the beginning (8 samples per second)
+            smoothed_offset = clock_private_info->previous_offset + jitter / 16;
+          else
+            smoothed_offset = clock_private_info->previous_offset + jitter / 64;
+        } else if (clock_private_info->follow_up_number <
+                   (5 * 8)) // at the beginning (8 samples per second)
+          smoothed_offset =
+              clock_private_info->previous_offset + jitter / 1; // accept positive changes quickly
+        else
+          smoothed_offset = clock_private_info->previous_offset + jitter / 64;
+      } else {
+        // allow samples to disappear for up to a second
+        if ((time_since_previous_offset != 0) && (time_since_previous_offset < 1000000000) && (jitter > -4000000000L)) {
+          smoothed_offset = clock_private_info->previous_offset +
+                            1; // if we have recent samples, forget the present sample...
+        } else {
+          if (clock_private_info->previous_offset_time == 0)
+            debug(1, "Clock %" PRIx64 " record (re)starting at %s.", clock_private_info->clock_id,
+                  clock_private_info->ip);
+          else
+            debug(1,
+                  "Timing discontinuity on clock %" PRIx64
+                  " at %s: time_since_previous_offset: %.3f seconds.",
+                  clock_private_info->clock_id, clock_private_info->ip,
+                  0.000000001 * time_since_previous_offset);
+          smoothed_offset = offset;
+          clock_private_info->follow_up_number = 0;
+          clock_private_info->mastership_start_time =
+              reception_time; // mastership is reset to this time...
+        }
+      }
+
+      clock_private_info->previous_offset_grandmaster = clock_private_info->grandmasterIdentity;
+      clock_private_info->previous_offset = smoothed_offset;
+      clock_private_info->previous_offset_time = reception_time;
+
+      int temp_client_id;
+      for (temp_client_id = 0; temp_client_id < MAX_CLIENTS; temp_client_id++) {
+        if ((clock_private_info->client_flags[temp_client_id] & (1 << clock_is_master)) != 0) {
+        debug(2,
+              "Clock %" PRIx64 ", grandmaster %" PRIx64 ". Offset: %" PRIx64
+              ", smoothed offset: %" PRIx64 ". Raw Precise Origin Timestamp: %" PRIx64
+              ". Time since previous offset: %8.3f milliseconds. ID: %5u, Follow_Up Number: %u. Source: %s",
+              clock_private_info->clock_id, clock_private_info->grandmasterIdentity, offset,
+              smoothed_offset, preciseOriginTimestamp, 0.000001 * time_since_previous_offset,
+              ntohs(msg->header.sequenceId), clock_private_info->follow_up_number, clock_private_info->ip);
+
+          debug(2, "clock_is_master -- updating master clock info for client \"%s\"",
+                get_client_name(temp_client_id));
+          update_master_clock_info(temp_client_id, clock_private_info->clock_id,
+                                   (const char *)&clock_private_info->ip, reception_time,
+                                   smoothed_offset, clock_private_info->mastership_start_time);
+        }
+      }
+    } else {
+      debug(1, "Follow_Up message is too small to be valid.");
     }
   }
 }
diff --git a/nqptp.c b/nqptp.c
index 153864f..e8112a0 100644
--- a/nqptp.c
+++ b/nqptp.c
@@ -338,112 +338,117 @@ int main(int argc, char **argv) {
   return 0;
 }
 
+void send_awakening_announcement_sequence(const uint64_t clock_id, const char *clock_ip,
+                                          const int ip_family, const uint8_t priority1,
+                                          const uint8_t priority2) {
+  struct ptp_announce_message *msg;
+  size_t msg_length = sizeof(struct ptp_announce_message);
+  msg = malloc(msg_length);
+  memset((void *)msg, 0, msg_length);
+
+  uint64_t my_clock_id = get_self_clock_id();
+  msg->header.transportSpecificAndMessageID = 0x10 + Announce;
+  msg->header.reservedAndVersionPTP = 0x02;
+  msg->header.messageLength = htons(sizeof(struct ptp_announce_message));
+  msg->header.flags = htons(0x0408);
+  hcton64(my_clock_id, &msg->header.clockIdentity[0]);
+  msg->header.sourcePortID = htons(32776);
+  msg->header.controlOtherMessage = 0x05;
+  msg->header.logMessagePeriod = 0xFE;
+  msg->announce.currentUtcOffset = htons(37);
+  hcton64(my_clock_id, &msg->announce.grandmasterIdentity[0]);
+  uint32_t my_clock_quality = 0xf8fe436a;
+  msg->announce.grandmasterClockQuality = htonl(my_clock_quality);
+  if (priority1 > 2) {
+    msg->announce.grandmasterPriority1 =
+        priority1 - 1; // make this announcement seem better than the clock we are about to ping
+    msg->announce.grandmasterPriority2 = priority2;
+  } else {
+    warn("Cannot select a suitable priority for pinging clock %" PRIx64 " at %s.", clock_id,
+         clock_ip);
+    msg->announce.grandmasterPriority1 = 248;
+    msg->announce.grandmasterPriority2 = 248;
+  }
+  msg->announce.timeSource = 160; // Internal Oscillator
+
+  // get the socket for the correct port -- 320 -- and family -- IPv4 or IPv6 -- to send it
+  // from.
+
+  int s = 0;
+  unsigned t;
+  for (t = 0; t < sockets_open_stuff.sockets_open; t++) {
+    if ((sockets_open_stuff.sockets[t].port == 320) &&
+        (sockets_open_stuff.sockets[t].family == ip_family))
+      s = sockets_open_stuff.sockets[t].number;
+  }
+  if (s == 0) {
+    debug(1, "sending socket not found for clock %" PRIx64 " at %s, family %s.", clock_id, clock_ip,
+          ip_family == AF_INET    ? "IPv4"
+          : ip_family == AF_INET6 ? "IPv6"
+                                  : "Unknown");
+  } else {
+    // debug(1, "Send message from socket %d.", s);
+
+    const char *portname = "320";
+    struct addrinfo hints;
+    memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+    hints.ai_socktype = SOCK_DGRAM;
+    hints.ai_protocol = 0;
+    hints.ai_flags = AI_ADDRCONFIG;
+    struct addrinfo *res = NULL;
+    int err = getaddrinfo(clock_ip, portname, &hints, &res);
+    if (err != 0) {
+      debug(1, "failed to resolve remote socket address (err=%d)", err);
+    } else {
+      // here, we have the destination, so send it
+
+      // debug_print_buffer(1, (char *)msg, msg_length);
+      int ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
+      if (ret == -1)
+        debug(1, "result of sendto is %d.", ret);
+      debug(2, "Send awaken Announce message to clock \"%" PRIx64 "\" at %s on %s.", clock_id, clock_ip,
+            ip_family == AF_INET6 ? "IPv6" : "IPv4");
+
+      if (priority1 < 254) {
+        msg->announce.grandmasterPriority1 =
+            priority1 + 1; // make this announcement seem worse than the clock we about to ping
+      } else {
+        warn("Cannot select a suitable priority for second ping of clock %" PRIx64 " at %s.",
+             clock_id, clock_ip);
+        msg->announce.grandmasterPriority1 = 250;
+      }
+
+      msg->announce.grandmasterPriority2 = priority2;
+      usleep(500000);
+      ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
+      if (ret == -1)
+        debug(1, "result of second sendto is %d.", ret);
+      freeaddrinfo(res);
+    }
+  }
+  free(msg);
+}
+
 uint64_t broadcasting_task(uint64_t call_time, __attribute__((unused)) void *private_data) {
   clock_source_private_data *clocks_private = (clock_source_private_data *)private_data;
   int i;
   for (i = 0; i < MAX_CLOCKS; i++) {
     if ((clocks_private[i].announcements_without_followups == 3) &&
-        (clocks_private[i].follow_up_number == 0) && // only check at the start
+        // (clocks_private[i].follow_up_number == 0) && // only check at the start
         ((clocks_private[i].flags & (1 << clock_is_one_of_ours)) == 0)) {
-      debug(1, "Found a silent clock %" PRIx64 " at %s.", clocks_private[i].clock_id,
+      debug(2, "Attempt to awaken a silent clock %" PRIx64 ", index %u, at follow_up_number %u at IP %s.",
+            clocks_private[i].clock_id, i, clocks_private[i].follow_up_number,
             clocks_private[i].ip);
+
       // send an Announce message to attempt to waken this silent PTP clock by
       // getting it to negotiate with an apparently better clock
       // that then immediately sends another Announce message indicating that it's inferior
 
       clocks_private[i].announcements_without_followups++; // set to 4 to indicate done/parked
-
-      struct ptp_announce_message *msg;
-      size_t msg_length = sizeof(struct ptp_announce_message);
-      msg = malloc(msg_length);
-      memset((void *)msg, 0, msg_length);
-
-      uint64_t my_clock_id = get_self_clock_id();
-      msg->header.transportSpecificAndMessageID = 0x10 + Announce;
-      msg->header.reservedAndVersionPTP = 0x02;
-      msg->header.messageLength = htons(sizeof(struct ptp_announce_message));
-      msg->header.flags = htons(0x0408);
-      hcton64(my_clock_id, &msg->header.clockIdentity[0]);
-      msg->header.sourcePortID = htons(32776);
-      msg->header.controlOtherMessage = 0x05;
-      msg->header.logMessagePeriod = 0xFE;
-      msg->announce.currentUtcOffset = htons(37);
-      hcton64(my_clock_id, &msg->announce.grandmasterIdentity[0]);
-      uint32_t my_clock_quality = 0xf8fe436a;
-      msg->announce.grandmasterClockQuality = htonl(my_clock_quality);
-      if (clocks_private[i].grandmasterPriority1 > 2) {
-        msg->announce.grandmasterPriority1 =
-            clocks_private[i].grandmasterPriority1 -
-            1; // make this announcement seem better than the clock we are about to ping
-        msg->announce.grandmasterPriority2 = clocks_private[i].grandmasterPriority2;
-      } else {
-        warn("Cannot select a suitable priority for pinging clock %" PRIx64 " at %s.",
-             clocks_private[i].clock_id, clocks_private[i].ip);
-        msg->announce.grandmasterPriority1 = 248;
-        msg->announce.grandmasterPriority2 = 248;
-      }
-      msg->announce.timeSource = 160; // Internal Oscillator
-
-      // get the socket for the correct port -- 320 -- and family -- IPv4 or IPv6 -- to send it
-      // from.
-
-      int s = 0;
-      unsigned t;
-      for (t = 0; t < sockets_open_stuff.sockets_open; t++) {
-        if ((sockets_open_stuff.sockets[t].port == 320) &&
-            (sockets_open_stuff.sockets[t].family == clocks_private[i].family))
-          s = sockets_open_stuff.sockets[t].number;
-      }
-      if (s == 0) {
-        debug(1, "sending socket not found for clock %" PRIx64 " at %s, family %s.",
-              clocks_private[i].clock_id, clocks_private[i].ip,
-              clocks_private[i].family == AF_INET    ? "IPv4"
-              : clocks_private[i].family == AF_INET6 ? "IPv6"
-                                                     : "Unknown");
-      } else {
-        // debug(1, "Send message from socket %d.", s);
-
-        const char *portname = "320";
-        struct addrinfo hints;
-        memset(&hints, 0, sizeof(hints));
-        hints.ai_family = AF_UNSPEC;
-        hints.ai_socktype = SOCK_DGRAM;
-        hints.ai_protocol = 0;
-        hints.ai_flags = AI_ADDRCONFIG;
-        struct addrinfo *res = NULL;
-        int err = getaddrinfo(clocks_private[i].ip, portname, &hints, &res);
-        if (err != 0) {
-          debug(1, "failed to resolve remote socket address (err=%d)", err);
-        } else {
-          // here, we have the destination, so send it
-
-          // if (clocks_private[i].family == AF_INET6) {
-          // debug_print_buffer(1, (char *)msg, msg_length);
-          int ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
-          if (ret == -1)
-            debug(1, "result of sendto is %d.", ret);
-          debug(2, "message clock \"%" PRIx64 "\" at %s on %s.", clocks_private[i].clock_id,
-                clocks_private[i].ip, clocks_private[i].family == AF_INET6 ? "IPv6" : "IPv4");
-
-          if (clocks_private[i].grandmasterPriority1 < 254) {
-            msg->announce.grandmasterPriority1 =
-                clocks_private[i].grandmasterPriority1 +
-                1; // make this announcement seem worse than the clock we about to ping
-          } else {
-            warn("Cannot select a suitable priority for second ping of clock %" PRIx64 " at %s.",
-                 clocks_private[i].clock_id, clocks_private[i].ip);
-            msg->announce.grandmasterPriority1 = 250;
-          }
-
-          msg->announce.grandmasterPriority2 = clocks_private[i].grandmasterPriority2;
-          ret = sendto(s, msg, msg_length, 0, res->ai_addr, res->ai_addrlen);
-          if (ret == -1)
-            debug(1, "result of second sendto is %d.", ret);
-          // }
-          freeaddrinfo(res);
-        }
-      }
-      free(msg);
+      send_awakening_announcement_sequence(
+          clocks_private[i].clock_id, clocks_private[i].ip, clocks_private[i].family,
+          clocks_private[i].grandmasterPriority1, clocks_private[i].grandmasterPriority2);
     }
   }
 
diff --git a/nqptp.h b/nqptp.h
index b4c1af3..3f04e1d 100644
--- a/nqptp.h
+++ b/nqptp.h
@@ -39,4 +39,10 @@
 // Instances" -- of a "PTP Network" it wishes to monitor. This is a "timing group" in AirPlay 2
 // parlance, it seems.
 
+
+void send_awakening_announcement_sequence(const uint64_t clock_id, const char *clock_ip,
+                                          const int ip_family, const uint8_t priority1,
+                                          const uint8_t priority2);
+
+
 #endif