From: Mike Brady <4265913+mikebrady@users.noreply.github.com> Date: Thu, 8 Apr 2021 16:33:19 +0000 (+0100) Subject: Move the Sync hander into the message-handlers, add a function to generate a self... X-Git-Tag: 1.1-dev~39 X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=03de7ac0f353235e625e68ddd2fa440e42595303;p=thirdparty%2Fnqptp.git Move the Sync hander into the message-handlers, add a function to generate a self clock id --- diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h index 6f46172..a35d3e1 100644 --- a/nqptp-clock-sources.h +++ b/nqptp-clock-sources.h @@ -28,7 +28,7 @@ enum stage { sync_seen, }; -#define MAX_TIMING_SAMPLES 19 +#define MAX_TIMING_SAMPLES 3 typedef struct { uint64_t local, local_to_remote_offset; } timing_samples; @@ -50,6 +50,7 @@ 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 add262f..ab11293 100644 --- a/nqptp-message-handlers.c +++ b/nqptp-message-handlers.c @@ -50,6 +50,7 @@ void handle_control_port_messages(char *buf, ssize_t recv_len, clock_source *clo // 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); } } @@ -190,7 +191,8 @@ 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 - if ((clock_info->flags & (1 << clock_is_valid)) && + // don't clamp for the first two seconds? + if ((clock_info->flags & (1 << clock_is_valid)) && (clock_private_info->sample_number > 17) && (clock_private_info->vacant_samples != MAX_TIMING_SAMPLES)) { const int64_t clamp = 1 * 1000 * 1000; @@ -212,6 +214,7 @@ void handle_follow_up(char *buf, ssize_t recv_len, clock_source *clock_info, 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; if (clock_private_info->vacant_samples == MAX_TIMING_SAMPLES) { @@ -269,4 +272,61 @@ 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); } -} \ No newline at end of file +} + +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; + } +} diff --git a/nqptp-message-handlers.h b/nqptp-message-handlers.h index 056e754..4460c95 100644 --- 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_sync(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); diff --git a/nqptp-utilities.c b/nqptp-utilities.c index 201bc7d..3b98b45 100644 --- a/nqptp-utilities.c +++ b/nqptp-utilities.c @@ -21,6 +21,8 @@ #include "general-utilities.h" #include #include +#include +#include #include #include #include @@ -165,3 +167,41 @@ void debug_print_buffer(int level, char *buf, size_t buf_len) { free(obf); } } + +uint64_t get_self_clock_id() { + // make up a clock ID based on an interfaces' MAC + char local_clock_id[8]; + int len = 0; + struct ifaddrs *ifaddr = NULL; + struct ifaddrs *ifa = NULL; + int status; + if ((status = getifaddrs(&ifaddr) == -1)) { + die("getifaddrs: %s", gai_strerror(status)); + } else { + int found = 0; + for (ifa = ifaddr; ifa != NULL; ifa = ifa->ifa_next) { + if ((ifa->ifa_addr) && (ifa->ifa_addr->sa_family == AF_PACKET)) { + struct sockaddr_ll *s = (struct sockaddr_ll *)ifa->ifa_addr; + if ((strcmp(ifa->ifa_name, "lo") != 0) && (found == 0)) { + len = s->sll_halen; + memcpy(local_clock_id, &s->sll_addr, len); + found = 1; + } + } + } + freeifaddrs(ifaddr); + } + // if the length of the MAC address is 6 we need to doctor it a little + // See Section 7.5.2.2.2 IEEE EUI-64 clockIdentity values, NOTE 2 + + if (len == 6) { // i.e. an EUI-48 MAC Address + local_clock_id[7] = local_clock_id[5]; + local_clock_id[6] = local_clock_id[4]; + local_clock_id[5] = local_clock_id[3]; + local_clock_id[3] = 0xFF; + local_clock_id[4] = 0xFE; + } + uint64_t result; + memcpy(&result, local_clock_id, sizeof(result)); + return result; +} diff --git a/nqptp-utilities.h b/nqptp-utilities.h index 9e07c61..f821f28 100644 --- a/nqptp-utilities.h +++ b/nqptp-utilities.h @@ -36,5 +36,6 @@ typedef struct { void open_sockets_at_port(uint16_t port, sockets_open_bundle *sockets_open_stuff); void debug_print_buffer(int level, char *buf, size_t buf_len); +uint64_t get_self_clock_id(); // a clock ID based on a MAC address #endif \ No newline at end of file diff --git a/nqptp.c b/nqptp.c index 6a2e247..0828436 100644 --- a/nqptp.c +++ b/nqptp.c @@ -349,58 +349,8 @@ int main(void) { &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 - // 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 (clocks_private[the_clock].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 = - (clocks_private[the_clock].sequence_number - new_sync_sequence_number); - - if ((sequence_number_difference > 0) && (sequence_number_difference < 40)) - discard_sync = 1; - } - - if (discard_sync == 0) { - - clocks_private[the_clock].sequence_number = ntohs(msg->header.sequenceId); - clocks_private[the_clock].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 is the Follow_Up - - clocks_private[the_clock].current_stage = sync_seen; - } + handle_sync(buf, recv_len, &shared_memory->clocks[the_clock], + &clocks_private[the_clock], reception_time); } break; case Follow_Up: {