move follow-up code to a separate function.

diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c
index b8287dc3111e5ed5cccd6fa33050fb81d20acace..81d2b89d6cbf8379e6accaed7698cde2d586bb54 100644 (file)
--- a/nqptp-message-handlers.c
+++ b/nqptp-message-handlers.c
@@ -150,3 +150,57 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source *clock_info,
     }
   }
 }
+
+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) {
+  struct ptp_follow_up_message *msg = (struct ptp_follow_up_message *)buf;
+
+                  if ((clock_private_info->current_stage == sync_seen) &&
+                      (clock_private_info->sequence_number ==
+                       ntohs(msg->header.sequenceId))) {
+
+                    uint64_t packet_clock_id = nctohl(&msg->header.clockIdentity[0]);
+                    uint64_t packet_clock_id_low = nctohl(&msg->header.clockIdentity[4]);
+                    packet_clock_id = packet_clock_id << 32;
+                    packet_clock_id = packet_clock_id + packet_clock_id_low;
+
+                    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;
+                    // this result is called "t1" in the IEEE spec.
+                    // we already have "t2" and it seems as if we can't generate "t3"
+                    // and "t4", so use t1 - t2 as the clock-to-local offsets
+
+                    clock_private_info->current_stage = waiting_for_sync;
+
+                    // update the shared clock information
+                    uint64_t offset = preciseOriginTimestamp - clock_private_info->t2;
+
+                    int rc = pthread_mutex_lock(shm_mutex);
+                    if (rc != 0)
+                      warn("Can't acquire mutex to update a clock!");
+                    // update/set the clock_id
+
+                    clock_info->clock_id = packet_clock_id;
+                    clock_info->flags |= (1<<clock_is_valid);
+                    clock_info->local_time = clock_private_info->t2;
+                    clock_info->local_to_source_time_offset = offset;
+                    rc = pthread_mutex_unlock(shm_mutex);
+                    if (rc != 0)
+                      warn("Can't release mutex after updating a clock!");
+
+                  } else {
+                    debug(3,
+                          "Follow_Up %u expecting to be in state sync_seen (%u). Stage error -- "
+                          "current state is %u, sequence %u. Ignoring it. %s",
+                          ntohs(msg->header.sequenceId), sync_seen,
+                          clock_private_info->current_stage,
+                          clock_private_info->sequence_number,
+                          clock_info->ip);
+                  }
+}
\ No newline at end of file

diff --git a/nqptp-message-handlers.h b/nqptp-message-handlers.h
index a882dde00ef5870100b3c966b7131c92bbdff7e2..1b6931180b7cb1d3783541f46e91e1eb4311276d 100644 (file)
--- a/nqptp-message-handlers.h
+++ b/nqptp-message-handlers.h
@@ -26,6 +26,9 @@
 void handle_announce(char *buf, ssize_t recv_len, clock_source *clock_info,
                      clock_source_private_data *clock_private_info, uint64_t reception_time);
 
+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);
+
 void handle_control_port_messages(char *buf, ssize_t recv_len, clock_source *clock_info,
                                   clock_source_private_data *clock_private_info);
 

diff --git a/nqptp.c b/nqptp.c
index 4d1c44f78e981bb74bf8364a70564ecedabfa50b..eb4df1476d805ae87ebd51db0b2abfde5cdafd39 100644 (file)
--- a/nqptp.c
+++ b/nqptp.c
@@ -404,56 +404,8 @@ int main(void) {
                 } break;
 
                 case Follow_Up: {
-                  struct ptp_follow_up_message *msg = (struct ptp_follow_up_message *)buf;
-
-                  if ((clocks_private[the_clock].current_stage == sync_seen) &&
-                      (clocks_private[the_clock].sequence_number ==
-                       ntohs(msg->header.sequenceId))) {
-
-                    uint64_t packet_clock_id = nctohl(&msg->header.clockIdentity[0]);
-                    uint64_t packet_clock_id_low = nctohl(&msg->header.clockIdentity[4]);
-                    packet_clock_id = packet_clock_id << 32;
-                    packet_clock_id = packet_clock_id + packet_clock_id_low;
-
-                    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;
-                    // this result is called "t1" in the IEEE spec.
-                    // we already have "t2" and it seems as if we can't generate "t3"
-                    // and "t4", so use t1 - t2 as the clock-to-local offsets
-
-                    clocks_private[the_clock].current_stage = waiting_for_sync;
-
-                    // update the shared clock information
-                    uint64_t offset = preciseOriginTimestamp - clocks_private[the_clock].t2;
-
-                    int rc = pthread_mutex_lock(&shared_memory->shm_mutex);
-                    if (rc != 0)
-                      warn("Can't acquire mutex to update a clock!");
-                    // update/set the clock_id
-
-                    shared_memory->clocks[the_clock].clock_id = packet_clock_id;
-                    shared_memory->clocks[the_clock].flags |= (1 << clock_is_valid);
-                    shared_memory->clocks[the_clock].local_time = clocks_private[the_clock].t2;
-                    shared_memory->clocks[the_clock].local_to_source_time_offset = offset;
-                    rc = pthread_mutex_unlock(&shared_memory->shm_mutex);
-                    if (rc != 0)
-                      warn("Can't release mutex after updating a clock!");
-
-                  } else {
-                    debug(3,
-                          "Follow_Up %u expecting to be in state sync_seen (%u). Stage error -- "
-                          "current state is %u, sequence %u. Ignoring it. %s",
-                          ntohs(msg->header.sequenceId), sync_seen,
-                          clocks_private[the_clock].current_stage,
-                          clocks_private[the_clock].sequence_number,
-                          &shared_memory->clocks[the_clock].ip);
-                  }
+                  handle_follow_up(buf, recv_len, &shared_memory->clocks[the_clock],
+                    &clocks_private[the_clock], reception_time, &shared_memory->shm_mutex);
                 } break;
                 default:
                   break;