try to add compensation for short timing disturbances, clamp the slew rate

diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h
index a35d3e1f49d1226ddafe5618118155ae7b31bd02..793b86c64c777bca26e8aac4948665c56a70ee40 100644 (file)
--- a/nqptp-clock-sources.h
+++ b/nqptp-clock-sources.h
@@ -28,8 +28,9 @@ enum stage {
   sync_seen,
 };
 
-#define MAX_TIMING_SAMPLES 3
+#define MAX_TIMING_SAMPLES 11
 typedef struct {
+  uint16_t sequence_number;
   uint64_t local, local_to_remote_offset;
 } timing_samples;
 
@@ -50,7 +51,6 @@ typedef struct {
   int vacant_samples; // the number of elements in the timing_samples array that are not yet used
   int next_sample_goes_here; // point to where in the timing samples array the next entries should
                              // go
-  uint32_t sample_number;
 } clock_source_private_data;
 
 int find_clock_source_record(char *sender_string, clock_source *clocks_shared_info,

diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c
index ab112932036c54dabfbf7ecca740797a5eb9c297..ad6a6d00bfe6034822a8c95b3a58256c76877232 100644 (file)
--- a/nqptp-message-handlers.c
+++ b/nqptp-message-handlers.c
@@ -47,10 +47,11 @@ void handle_control_port_messages(char *buf, ssize_t recv_len, clock_source *clo
           if (t == -1)
             t = create_clock_source_record(new_ip, clock_info, clock_private_info,
                                            0); // don't use the mutex
+
           // if it is just about to become a timing peer, reset its sample count
           clock_private_info[t].vacant_samples = MAX_TIMING_SAMPLES;
           clock_private_info[t].next_sample_goes_here = 0;
-          clock_private_info[t].sample_number = 0;
+
           clock_info[t].flags |= (1 << clock_is_a_timing_peer);
         }
       }
@@ -119,22 +120,22 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source *clock_info,
           grandmaster_clock_id = grandmaster_clock_id << 32;
           grandmaster_clock_id = grandmaster_clock_id + grandmaster_clock_id_low;
 
-          debug(2,
+          debug(1,
                 "clock_id %" PRIx64 " at:    %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(2, "    grandmasterIdentity:         %" PRIx64 ".", grandmaster_clock_id);
-          debug(2, "    grandmasterPriority1:        %u.", msg->announce.grandmasterPriority1);
-          debug(2, "    grandmasterClockQuality:     0x%x.", msg->announce.grandmasterClockQuality);
-          debug(2, "        clockClass:              %u.", clockClass); // See 7.6.2.4 clockClass
-          debug(2, "        clockAccuracy:           0x%x.",
+          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(2, "        offsetScaledLogVariance: 0x%x.",
+          debug(1, "        offsetScaledLogVariance: 0x%x.",
                 offsetScaledLogVariance); // See 7.6.3 PTP variance
-          debug(2, "    grandmasterPriority2:        %u.", msg->announce.grandmasterPriority2);
+          debug(1, "    grandmasterPriority2:        %u.", msg->announce.grandmasterPriority2);
         }
         if (pthread_mutex_lock(&shared_memory->shm_mutex) != 0)
           warn("Can't acquire mutex to set_timing_peers!");
@@ -158,6 +159,75 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source *clock_info,
   }
 }
 
+void handle_sync(char *buf, ssize_t recv_len, clock_source *clock_info,
+                 clock_source_private_data *clock_private_info, uint64_t reception_time) {
+
+  struct ptp_sync_message *msg = (struct ptp_sync_message *)buf;
+  // this is just to see if anything interesting comes in the SYNC package
+  // a non-zero origin timestamp
+  // or correction field would be interesting....
+  int ck;
+  int non_empty_origin_timestamp = 0;
+  for (ck = 0; ck < 10; ck++) {
+    if (msg->sync.originTimestamp[ck] != 0) {
+      non_empty_origin_timestamp = (non_empty_origin_timestamp | 1);
+    }
+  }
+  if (non_empty_origin_timestamp != 0)
+    debug(2, "Sync Origin Timestamp!");
+  if (msg->header.correctionField != 0)
+    debug(3, "correctionField: %" PRIx64 ".", msg->header.correctionField);
+
+  int discard_sync = 0;
+
+  // check if we should discard this SYNC
+  if (clock_private_info->current_stage != waiting_for_sync) {
+
+    // here, we have an unexpected SYNC. It could be because the
+    // previous transaction sequence failed for some reason
+    // But, if that is so, the SYNC will have a newer sequence number
+    // so, ignore it if it's a little older.
+
+    // If it seems a lot older in sequence number terms, then it might
+    // be the start of a completely new sequence, so if the
+    // difference is more than 40 (WAG), accept it
+
+    uint16_t new_sync_sequence_number = ntohs(msg->header.sequenceId);
+    int16_t sequence_number_difference =
+        (clock_private_info->sequence_number - new_sync_sequence_number);
+
+    if ((sequence_number_difference > 0) && (sequence_number_difference < 40))
+      discard_sync = 1;
+  }
+
+  if (discard_sync == 0) {
+    /*
+        // just check how long since the last sync, if there was one
+        clock_private_info->reception_interval = 0;
+        if (clock_private_info->t2 != 0) {
+          int16_t seq_diff = ntohs(msg->header.sequenceId) - clock_private_info->sequence_number;
+          if (seq_diff == 1) {
+            uint64_t delta = reception_time - clock_private_info->t2;
+            clock_private_info->reception_interval = delta;
+            debug(1," reception interval: %f", delta * 0.000001);
+
+          }
+        }
+        */
+    clock_private_info->sequence_number = ntohs(msg->header.sequenceId);
+    clock_private_info->t2 = reception_time;
+
+    // it turns out that we don't really need to send a Delay_Req
+    // as a Follow_Up message always comes through
+
+    // If we had hardware assisted network timing, then maybe
+    // Even then, AP2 devices don't seem to send an accurate
+    // Delay_Resp time -- it contains the same information as the Follow_Up
+
+    clock_private_info->current_stage = sync_seen;
+  }
+}
+
 void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info,
                       clock_source_private_data *clock_private_info, uint64_t reception_time,
                       pthread_mutex_t *shm_mutex) {
@@ -191,32 +261,54 @@ void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info,
     // now, if there was a valid offset previously,
     // check if the offset should be clamped
 
-    // don't clamp for the first two seconds?
-    if ((clock_info->flags & (1 << clock_is_valid)) && (clock_private_info->sample_number > 17) &&
+    if ((clock_info->flags & (1 << clock_is_valid)) &&
         (clock_private_info->vacant_samples != MAX_TIMING_SAMPLES)) {
 
-      const int64_t clamp = 1 * 1000 * 1000;
-      int64_t jitter = offset - clock_private_info->previous_estimated_offset;
-      if (jitter > clamp)
-        offset = clock_private_info->previous_estimated_offset + clamp;
-      else if (jitter < (-clamp))
-        offset = clock_private_info->previous_estimated_offset - clamp;
-      int64_t clamped_jitter = offset - clock_private_info->previous_estimated_offset;
-      debug(1, "clock: %" PRIx64 ", jitter: %+f ms, clamped_jitter: %+f ms.", clock_info->clock_id,
-            jitter * 0.000001, clamped_jitter * 0.000001);
+      /*
+      if (clock_private_info->previous_estimated_offset != 0) {
+          int64_t jitter = offset - clock_private_info->previous_estimated_offset;
+          int64_t skew = 0;
+              debug(1," reception interval is: %f", clock_private_info->reception_interval *
+      0.000001); if (clock_private_info->reception_interval != 0) { uint64_t nominal_interval =
+      125000000; // 125 ms skew = clock_private_info->reception_interval - nominal_interval; skew =
+      skew / 2; offset = offset + skew; // try to compensate is a packet arrives too early or too
+      late
+          }
+
+          int64_t compensated_jitter = offset - clock_private_info->previous_estimated_offset;
+          debug(1," jitter: %f, skew: %f, compensated_jitter: %f.", jitter * 0.000001, skew *
+      0.000001, compensated_jitter * 0.000001);
+      }
+      */
+
+      /*
+      // only clamp if in the timing peer list
+        if ((clock_info->flags & (1 << clock_is_a_timing_peer)) != 0) {
+            const int64_t clamp = 1 * 1000 * 1000; //
+            int64_t jitter = offset - clock_private_info->previous_estimated_offset;
+            if (jitter > clamp)
+              offset = clock_private_info->previous_estimated_offset + clamp;
+            else if (jitter < (-clamp))
+              offset = clock_private_info->previous_estimated_offset - clamp;
+            int64_t clamped_jitter = offset - clock_private_info->previous_estimated_offset;
+            debug(1, "clock: %" PRIx64 ", jitter: %+f ms, clamped_jitter: %+f ms.",
+      clock_info->clock_id, jitter * 0.000001, clamped_jitter * 0.000001);
+        }
+          // okay, so the offset may now have been clamped to be close to the estimated previous
+      offset
+      */
     }
 
-    // okay, so the offset may now have been clamped to be close to the estimated previous offset
-    // track our samples
+    // update our sample information
 
     clock_private_info->samples[clock_private_info->next_sample_goes_here].local =
-        clock_private_info->t2;
+        clock_private_info->t2; // this is when the Sync message arrived.
     clock_private_info->samples[clock_private_info->next_sample_goes_here].local_to_remote_offset =
         offset;
-    clock_private_info->next_sample_goes_here++;
-    clock_private_info->sample_number++;
-    if (clock_private_info->next_sample_goes_here == MAX_TIMING_SAMPLES)
-      clock_private_info->next_sample_goes_here = 0;
+    clock_private_info->samples[clock_private_info->next_sample_goes_here].sequence_number =
+        clock_private_info->sequence_number;
+
+    // if this is the very first...
     if (clock_private_info->vacant_samples == MAX_TIMING_SAMPLES) {
       clock_private_info->previous_offset = offset;
       clock_private_info->previous_estimated_offset = offset;
@@ -225,30 +317,76 @@ void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info,
     if (clock_private_info->vacant_samples > 0)
       clock_private_info->vacant_samples--;
 
+    // okay, so now we have our samples, including the current one.
+
+    // let's try to estimate when this Sync message _should_ have arrived.
+    // this might allow us to detect a sample that is anomalously late or early
+    // skewing the offset calculation.
+
+    int sample_count = MAX_TIMING_SAMPLES - clock_private_info->vacant_samples;
+    int64_t divergence;
+    if (sample_count > 1) {
+      int f;
+      uint64_t ts = 0;
+      for (f = 0; f < sample_count; f++) {
+        uint64_t ti = clock_private_info->samples[f].local >> 8;
+        uint16_t sequence_gap =
+            clock_private_info->sequence_number - clock_private_info->samples[f].sequence_number;
+        ti += (125000000 * (sequence_gap)) >> 8;
+        // debug(1, "ti: %f, sequence_gap: %u, sample count: %u", ti, sequence_gap, sample_count);
+        ts += ti;
+      }
+      ts = ts / sample_count;
+      ts = ts << 8;
+      // uint64_t estimated_t2 = (uint64_t)ts;
+      divergence = clock_private_info->t2 - ts;
+      // debug(1, "divergence is: %f ms. %d samples", divergence * 0.000001, sample_count);
+    }
+
     // calculate averages
 
+    // here we will correct the offset by adding the divergence to it
+
+    offset = offset + divergence;
+
+    const int64_t clamp = 1000 * 1000; // WAG
+    int64_t jitter = offset - clock_private_info->previous_estimated_offset;
+    if (jitter > clamp)
+      offset = clock_private_info->previous_estimated_offset + clamp;
+    else if (jitter < (-clamp))
+      offset = clock_private_info->previous_estimated_offset - clamp;
+    //    int64_t clamped_jitter = offset - clock_private_info->previous_estimated_offset;
+    //     debug(1, "clock: %" PRIx64 ", jitter: %+f ms, clamped_jitter: %+f ms.",
+    //     clock_info->clock_id,
+    //           jitter * 0.000001, clamped_jitter * 0.000001);
+
+    // okay, so the offset may now have been clamped to be close to the estimated previous offset
+
     uint64_t estimated_offset = offset;
 
-    // here, calculate the average offset
+    /*
+        // here, calculate the average offset
 
-    int sample_count = MAX_TIMING_SAMPLES - clock_private_info->vacant_samples;
+        // int sample_count = MAX_TIMING_SAMPLES - clock_private_info->vacant_samples;
 
-    if (sample_count > 1) {
-      int e;
-      long double offsets = 0;
-      for (e = 0; e < sample_count; e++) {
-        uint64_t ho = clock_private_info->samples[e].local_to_remote_offset;
+        if (sample_count > 1) {
+          int e;
+          long double offsets = 0;
+          for (e = 0; e < sample_count; e++) {
+            uint64_t ho = clock_private_info->samples[e].local_to_remote_offset;
 
-        offsets = offsets + 1.0 * ho;
-      }
+            offsets = offsets + 1.0 * ho;
+          }
 
-      offsets = offsets / sample_count;
-      estimated_offset = (uint64_t)offsets;
-    }
+          offsets = offsets / sample_count;
+          estimated_offset = (uint64_t)offsets;
+        }
+    */
 
     int64_t estimated_variation = estimated_offset - clock_private_info->previous_estimated_offset;
-    debug(2, "clock: %" PRIx64 ", estimated_jitter: %+f ms.", clock_info->clock_id,
-          estimated_variation * 0.000001);
+    // debug(1, "clock: %" PRIx64 ", estimated_jitter: %+f ms, divergence: %+f.",
+    // clock_info->clock_id,
+    //      estimated_variation * 0.000001, divergence * 0.000001);
 
     clock_private_info->previous_estimated_offset = estimated_offset;
 
@@ -265,6 +403,12 @@ void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info,
     if (rc != 0)
       warn("Can't release mutex after updating a clock!");
 
+    clock_private_info->next_sample_goes_here++;
+
+    // if we have need to wrap.
+    if (clock_private_info->next_sample_goes_here == MAX_TIMING_SAMPLES)
+      clock_private_info->next_sample_goes_here = 0;
+
   } else {
     debug(3,
           "Follow_Up %u expecting to be in state sync_seen (%u). Stage error -- "
@@ -272,61 +416,4 @@ void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info,
           ntohs(msg->header.sequenceId), sync_seen, clock_private_info->current_stage,
           clock_private_info->sequence_number, clock_info->ip);
   }
-}
-
-void handle_sync(char *buf, ssize_t recv_len, clock_source *clock_info,
-                 clock_source_private_data *clock_private_info, uint64_t reception_time) {
-
-  struct ptp_sync_message *msg = (struct ptp_sync_message *)buf;
-  // this is just to see if anything interesting comes in the SYNC package
-  // a non-zero origin timestamp
-  // or correction field would be interesting....
-  int ck;
-  int non_empty_origin_timestamp = 0;
-  for (ck = 0; ck < 10; ck++) {
-    if (msg->sync.originTimestamp[ck] != 0) {
-      non_empty_origin_timestamp = (non_empty_origin_timestamp | 1);
-    }
-  }
-  if (non_empty_origin_timestamp != 0)
-    debug(2, "Sync Origin Timestamp!");
-  if (msg->header.correctionField != 0)
-    debug(3, "correctionField: %" PRIx64 ".", msg->header.correctionField);
-
-  int discard_sync = 0;
-
-  // check if we should discard this SYNC
-  if (clock_private_info->current_stage != waiting_for_sync) {
-
-    // here, we have an unexpected SYNC. It could be because the
-    // previous transaction sequence failed for some reason
-    // But, if that is so, the SYNC will have a newer sequence number
-    // so, ignore it if it's a little older.
-
-    // If it seems a lot older in sequence number terms, then it might
-    // be the start of a completely new sequence, so if the
-    // difference is more than 40 (WAG), accept it
-
-    uint16_t new_sync_sequence_number = ntohs(msg->header.sequenceId);
-    int16_t sequence_number_difference =
-        (clock_private_info->sequence_number - new_sync_sequence_number);
-
-    if ((sequence_number_difference > 0) && (sequence_number_difference < 40))
-      discard_sync = 1;
-  }
-
-  if (discard_sync == 0) {
-
-    clock_private_info->sequence_number = ntohs(msg->header.sequenceId);
-    clock_private_info->t2 = reception_time;
-
-    // it turns out that we don't really need to send a Delay_Req
-    // as a Follow_Up message always comes through
-
-    // If we had hardware assisted network timing, then maybe
-    // Even then, AP2 devices don't seem to send an accurate
-    // Delay_Resp time -- it contains the same information as the Follow_Up
-
-    clock_private_info->current_stage = sync_seen;
-  }
-}
+}
\ No newline at end of file

diff --git a/nqptp.c b/nqptp.c
index 0828436738dbde7ac908fc4dd97902af68008a36..351c8a4e2d9668e7e74474befd27bbe8ab968172 100644 (file)
--- a/nqptp.c
+++ b/nqptp.c
@@ -322,7 +322,7 @@ int main(void) {
               sender_port = ntohs(sa4->sin_port);
             }
 
-            if (sender_port == receiver_port) {
+            if ((sender_port == receiver_port) && (connection_ip_family == AF_INET)) {
 
               char sender_string[256];
               memset(sender_string, 0, sizeof(sender_string));