From: Mike Brady <4265913+mikebrady@users.noreply.github.com> Date: Mon, 19 Jul 2021 09:16:03 +0000 (+0100) Subject: Candidate full implementation of BCM algorithm. Clang format. X-Git-Tag: 1.2~106 X-Git-Url: http://git.ipfire.org/?a=commitdiff_plain;h=77116ab073e2c5a0f729323cb8495bf5a3d4c9a8;p=thirdparty%2Fnqptp.git Candidate full implementation of BCM algorithm. Clang format. --- diff --git a/nqptp-clock-sources.c b/nqptp-clock-sources.c index b571b5a..bf81283 100644 --- a/nqptp-clock-sources.c +++ b/nqptp-clock-sources.c @@ -22,14 +22,14 @@ #include "general-utilities.h" #include "nqptp-ptp-definitions.h" #include +#include #include #include #include -#include #ifdef CONFIG_FOR_FREEBSD -#include #include +#include #endif #ifndef FIELD_SIZEOF @@ -132,12 +132,12 @@ void update_clock_self_identifications(clock_source_private_data *clocks_private struct sockaddr *my_ifa_addr = ifa->ifa_addr; if (my_ifa_addr) { family = my_ifa_addr->sa_family; - #ifdef AF_INET6 +#ifdef AF_INET6 if (family == AF_INET6) { struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)my_ifa_addr; addr = &(sa6->sin6_addr); } - #endif +#endif if (family == AF_INET) { struct sockaddr_in *sa4 = (struct sockaddr_in *)my_ifa_addr; addr = &(sa4->sin_addr); @@ -156,12 +156,12 @@ void update_clock_self_identifications(clock_source_private_data *clocks_private } } } else { - debug(1,"NULL ifa->ifa_addr. Probably harmless."); + debug(1, "NULL ifa->ifa_addr. Probably harmless."); } } freeifaddrs(ifap); } else { - debug(1,"getifaddrs error - %s.", strerror(errno)); + debug(1, "getifaddrs error - %s.", strerror(errno)); } } @@ -185,7 +185,8 @@ void debug_log_nqptp_status(int level) { if ((clocks_private[i].flags & peer_master_mask) == peer_master_mask) { debug(level, " Peer Master: %" PRIx64 " %s.", clocks_private[i].clock_id, clocks_private[i].ip); - } else if ((clocks_private[i].flags & peer_becoming_master_mask) == peer_becoming_master_mask) { + } else if ((clocks_private[i].flags & peer_becoming_master_mask) == + peer_becoming_master_mask) { debug(level, " Peer Becoming Master: %" PRIx64 " %s.", clocks_private[i].clock_id, clocks_private[i].ip); } else if ((clocks_private[i].flags & peer_clock_mask) == peer_clock_mask) { @@ -207,17 +208,62 @@ void debug_log_nqptp_status(int level) { } } +int uint32_cmp(uint32_t a, uint32_t b, const char *cause) { + // returns -1 if a is less than b, 0 if a = b, +1 if a is greater than b + if (a == b) { + return 0; + } else { + debug(2, "Best Master Clock algorithm deciding factor: %s. Values: %u, %u.", cause, a, b); + if (a < b) + return -1; + else + return 1; + } +} + +int uint64_cmp(uint64_t a, uint64_t b, const char *cause) { + // returns -1 if a is less than b, 0 if a = b, +1 if a is greater than b + if (a == b) { + return 0; + } else { + debug(2, "Best Master Clock algorithm deciding factor: %s. Values: %" PRIx64 ", %" PRIx64 ".", + cause, a, b); + if (a < b) + return -1; + else + return 1; + } +} + void update_master() { - // note -- this is definitely incomplete -- it doesn't do the full - // data set comparison specified by the IEEE 588 standard + + // This implements the IEEE 1588-2008 best master clock algorithm. + + // However, since nqptp is not a ptp clock, some of it doesn't apply. + // Specifically, the Identity of Receiver stuff doesn't apply, since the + // program is merely monitoring Announce message data and isn't a PTP clock itself + // and thus does not have any kind or receiver identity itself. + + // Clock information coming from the same clock over IPv4 and IPv6 should have different + // port numbers. + + // Figure 28 can be therefore be simplified considerably: + + // Since nqptp can not be a receiver, and since nqptp can not originate a clock + // (and anyway nqptp filters out packets coming from self) + // we can do a single comparison of stepsRemoved and pick the shorter, if any. + + // Figure 28 reduces to checking steps removed and then, if necessary, checking identities. + // If we see two identical sets of information, it is an error, + // but we leave things as they are. int old_master = -1; // find the current master clock if there is one and turn off all mastership int i; for (i = 0; i < MAX_CLOCKS; i++) { if ((clocks_private[i].flags & (1 << clock_is_master)) != 0) if (old_master == -1) - old_master = i; // find old master - clocks_private[i].flags &= ~(1 << clock_is_master); // turn them all off + old_master = i; // find old master + clocks_private[i].flags &= ~(1 << clock_is_master); // turn them all off clocks_private[i].flags &= ~(1 << clock_is_becoming_master); // turn them all off } @@ -229,32 +275,58 @@ void update_master() { if ((clocks_private[i].flags & acceptance_mask) == acceptance_mask) { // found a possible clock candidate timing_peer_count++; + int outcome; if (best_so_far == -1) { best_so_far = i; } else { - // do the data set comparison detailed in Figure 27 and Figure 28 on pp89-90 + // Do the data set comparison detailed in Figure 27 and Figure 28 on pp89-90 if (clocks_private[i].grandmasterIdentity == clocks_private[best_so_far].grandmasterIdentity) { - // should implement Figure 28 here - } else if (clocks_private[i].grandmasterPriority1 < - clocks_private[best_so_far].grandmasterPriority1) { - best_so_far = i; - } else if (clocks_private[i].grandmasterClass < - clocks_private[best_so_far].grandmasterClass) { - best_so_far = i; - } else if (clocks_private[i].grandmasterAccuracy < - clocks_private[best_so_far].grandmasterAccuracy) { - best_so_far = i; - } else if (clocks_private[i].grandmasterVariance < - clocks_private[best_so_far].grandmasterVariance) { - best_so_far = i; - } else if (clocks_private[i].grandmasterPriority2 < - clocks_private[best_so_far].grandmasterPriority2) { - best_so_far = i; - } else if (clocks_private[i].grandmasterIdentity < - clocks_private[best_so_far].grandmasterIdentity) { - best_so_far = i; + // Do the relevant part of Figure 28: + outcome = uint32_cmp(clocks_private[i].stepsRemoved, + clocks_private[best_so_far].stepsRemoved, "steps removed"); + // we need to check the portIdentify, which is the clock_id and the clock_port_number + if (outcome == 0) + outcome = uint64_cmp(clocks_private[i].clock_id, clocks_private[best_so_far].clock_id, + "clock id"); + if (outcome == 0) + outcome = + uint32_cmp(clocks_private[i].clock_port_number, + clocks_private[best_so_far].clock_port_number, "clock port number"); + if (outcome == 0) { + debug(1, + "Best Master Clock algorithm: two separate but identical potential clock " + "masters: %" PRIx64 ".", + clocks_private[best_so_far].clock_id); + } + + } else { + outcome = + uint32_cmp(clocks_private[i].grandmasterPriority1, + clocks_private[best_so_far].grandmasterPriority1, "grandmasterPriority1"); + if (outcome == 0) + outcome = uint32_cmp(clocks_private[i].grandmasterClass, + clocks_private[best_so_far].grandmasterClass, "grandmasterClass"); + if (outcome == 0) + outcome = + uint32_cmp(clocks_private[i].grandmasterAccuracy, + clocks_private[best_so_far].grandmasterAccuracy, "grandmasterAccuracy"); + if (outcome == 0) + outcome = + uint32_cmp(clocks_private[i].grandmasterVariance, + clocks_private[best_so_far].grandmasterVariance, "grandmasterVariance"); + if (outcome == 0) + outcome = uint32_cmp(clocks_private[i].grandmasterPriority2, + clocks_private[best_so_far].grandmasterPriority2, + "grandmasterPriority2"); + if (outcome == 0) + // this can't fail, as it's a condition of entering this section that they are different + outcome = + uint64_cmp(clocks_private[i].grandmasterIdentity, + clocks_private[best_so_far].grandmasterIdentity, "grandmasterIdentity"); } + if (outcome == -1) + best_so_far = i; } } } @@ -262,7 +334,7 @@ void update_master() { // no master clock if (old_master != -1) { // but there was a master clock, so remove it - debug(1, "shm interface -- remove master clock designation"); + debug(2, "Remove master clock."); update_master_clock_info(0, NULL, 0, 0, 0); } if (timing_peer_count == 0) @@ -272,7 +344,6 @@ void update_master() { } else { // we found a master clock - if (old_master != best_so_far) { // if the naster is a new one clocks_private[best_so_far].flags |= (1 << clock_is_becoming_master); diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h index c0674d6..28b5b09 100644 --- a/nqptp-clock-sources.h +++ b/nqptp-clock-sources.h @@ -66,7 +66,7 @@ typedef struct { // these are for finding the best clock to use // See Figure 27 and 27 pp 89 -- 90 for the Data set comparison algorithm - + uint16_t clock_port_number; // used along with the clock_id as the portIdentity uint8_t grandmasterPriority1; uint32_t grandmasterQuality; // class/accuracy/variance -- lower is better uint8_t grandmasterClass; diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c index 7e7f7dc..3e9d39e 100644 --- a/nqptp-message-handlers.c +++ b/nqptp-message-handlers.c @@ -16,7 +16,8 @@ * * Commercial licensing is also available. */ -#include //strsep +#include // ntohl and ntohs +#include //strsep #include "debug.h" #include "general-utilities.h" @@ -121,7 +122,7 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source_private_data *clo 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; - uint32_t clockQuality = msg->announce.grandmasterClockQuality; + uint32_t clockQuality = ntohl(msg->announce.grandmasterClockQuality); uint8_t clockClass = (clockQuality >> 24) & 0xff; uint8_t clockAccuracy = (clockQuality >> 16) & 0xff; uint16_t offsetScaledLogVariance = clockQuality & 0xffff; @@ -136,7 +137,8 @@ void handle_announce(char *buf, ssize_t recv_len, clock_source_private_data *clo clock_private_info->grandmasterAccuracy = clockAccuracy; clock_private_info->grandmasterVariance = offsetScaledLogVariance; clock_private_info->grandmasterPriority2 = msg->announce.grandmasterPriority2; - clock_private_info->stepsRemoved = msg->announce.stepsRemoved; + clock_private_info->stepsRemoved = ntohs(msg->announce.stepsRemoved); + clock_private_info->clock_port_number = ntohs(msg->header.sourcePortID); best_clock_update_needed = 1; } else { // otherwise, something in it might have changed, I guess, that @@ -245,105 +247,103 @@ void handle_follow_up(char *buf, __attribute__((unused)) ssize_t recv_len, debug(2, "FOLLOWUP from %" PRIx64 ", %s.", clock_private_info->clock_id, &clock_private_info->ip); uint64_t offset = preciseOriginTimestamp - reception_time; - + clock_private_info->local_time = reception_time; clock_private_info->source_time = preciseOriginTimestamp; clock_private_info->local_to_source_time_offset = offset; - + int64_t jitter = 0; - if ((clock_private_info->flags & (1 << clock_is_becoming_master)) != 0) { - // we definitely have at least one sample since the request was made to - // designate it a master, so we assume it is legitimate. That is, we assume - // that the clock originator knows that it a clock master by now. - uint64_t oldest_acceptable_master_clock_time = - clock_private_info->source_time + 1150000000; // ns. - - // we will try to improve on this present, definitive, local_to_source_time_offset we have - int changes_made = 0; - - uint64_t best_offset_so_far = clock_private_info->local_to_source_time_offset; - uint64_t age_of_oldest_legitimate_sample = clock_private_info->local_time; - - int number_of_samples = MAX_TIMING_SAMPLES - clock_private_info->vacant_samples; - int samples_checked = 0; - if (number_of_samples > 0) { - debug(3, "Number of samples: %d.", number_of_samples); - - // Now we use the last few samples to calculate the best offset for the - // new master clock. - - // The time of the oldest sample we use will become the time of the start of the - // mastership. - - // We will accept samples that would make the local-to-clock offset greatest, - // provided they are not too old and that they don't push the current clock time - // more than, say, 1000 ms plus one sample interval (i.e about 1.125 seconds) in the future. - - // This present sample is the only time estimate we have when the clock is definitely a - // master, so we use it to eliminate any previous time estimates, made when the clock wasn't - // designated a master, that would put it more than, say, a 1.15 seconds further into the - // future. - - // Allow the samples to give a valid master clock time up to this much later than the - // present, definitive, sample: - - uint64_t oldest_acceptable_time = reception_time - 10000000000; // only go back this far (ns) - - int64_t cko = age_of_oldest_legitimate_sample - oldest_acceptable_time; - if (cko < 0) - debug(1,"starting sample is too old: %" PRId64 " ns.", cko); - - int i; - for (i = 0; i < number_of_samples; i++) { - int64_t age = reception_time - clock_private_info->samples[i].local_time; - int64_t age_relative_to_oldest_acceptable_time = - clock_private_info->samples[i].local_time - oldest_acceptable_time; - if (age_relative_to_oldest_acceptable_time > 0) { - debug(3, "sample accepted at %f seconds old.", 0.000000001 * age); - if (clock_private_info->samples[i].local_time < - age_of_oldest_legitimate_sample) { - age_of_oldest_legitimate_sample = clock_private_info->samples[i].local_time; - } - uint64_t possible_offset = clock_private_info->samples[i].clock_time - - clock_private_info->samples[i].local_time; - uint64_t possible_master_clock_time = - clock_private_info->local_time + possible_offset; - int64_t age_relative_to_oldest_acceptable_master_clock_time = - possible_master_clock_time - oldest_acceptable_master_clock_time; - if (age_relative_to_oldest_acceptable_master_clock_time <= 0) { - samples_checked++; - // so, the sample was not obtained too far in the past - // and it would not push the estimated master clock_time too far into the future - // so, if it is greater than the best_offset_so_far, then make it the new one - if (possible_offset > best_offset_so_far) { - debug(3, "new best offset"); - best_offset_so_far = possible_offset; - changes_made++; - } - } else { - debug(3, "sample too far into the future"); + if ((clock_private_info->flags & (1 << clock_is_becoming_master)) != 0) { + // we definitely have at least one sample since the request was made to + // designate it a master, so we assume it is legitimate. That is, we assume + // that the clock originator knows that it a clock master by now. + uint64_t oldest_acceptable_master_clock_time = + clock_private_info->source_time + 1150000000; // ns. + + // we will try to improve on this present, definitive, local_to_source_time_offset we have + int changes_made = 0; + + uint64_t best_offset_so_far = clock_private_info->local_to_source_time_offset; + uint64_t age_of_oldest_legitimate_sample = clock_private_info->local_time; + + int number_of_samples = MAX_TIMING_SAMPLES - clock_private_info->vacant_samples; + int samples_checked = 0; + if (number_of_samples > 0) { + debug(3, "Number of samples: %d.", number_of_samples); + + // Now we use the last few samples to calculate the best offset for the + // new master clock. + + // The time of the oldest sample we use will become the time of the start of the + // mastership. + + // We will accept samples that would make the local-to-clock offset greatest, + // provided they are not too old and that they don't push the current clock time + // more than, say, 1000 ms plus one sample interval (i.e about 1.125 seconds) in the future. + + // This present sample is the only time estimate we have when the clock is definitely a + // master, so we use it to eliminate any previous time estimates, made when the clock wasn't + // designated a master, that would put it more than, say, a 1.15 seconds further into the + // future. + + // Allow the samples to give a valid master clock time up to this much later than the + // present, definitive, sample: + + uint64_t oldest_acceptable_time = reception_time - 10000000000; // only go back this far (ns) + + int64_t cko = age_of_oldest_legitimate_sample - oldest_acceptable_time; + if (cko < 0) + debug(1, "starting sample is too old: %" PRId64 " ns.", cko); + + int i; + for (i = 0; i < number_of_samples; i++) { + int64_t age = reception_time - clock_private_info->samples[i].local_time; + int64_t age_relative_to_oldest_acceptable_time = + clock_private_info->samples[i].local_time - oldest_acceptable_time; + if (age_relative_to_oldest_acceptable_time > 0) { + debug(3, "sample accepted at %f seconds old.", 0.000000001 * age); + if (clock_private_info->samples[i].local_time < age_of_oldest_legitimate_sample) { + age_of_oldest_legitimate_sample = clock_private_info->samples[i].local_time; + } + uint64_t possible_offset = + clock_private_info->samples[i].clock_time - clock_private_info->samples[i].local_time; + uint64_t possible_master_clock_time = clock_private_info->local_time + possible_offset; + int64_t age_relative_to_oldest_acceptable_master_clock_time = + possible_master_clock_time - oldest_acceptable_master_clock_time; + if (age_relative_to_oldest_acceptable_master_clock_time <= 0) { + samples_checked++; + // so, the sample was not obtained too far in the past + // and it would not push the estimated master clock_time too far into the future + // so, if it is greater than the best_offset_so_far, then make it the new one + if (possible_offset > best_offset_so_far) { + debug(3, "new best offset"); + best_offset_so_far = possible_offset; + changes_made++; } } else { - debug(3, "sample too old at %f seconds old.", 0.000000001 * age); + debug(3, "sample too far into the future"); } + } else { + debug(3, "sample too old at %f seconds old.", 0.000000001 * age); } } - clock_private_info->mastership_start_time = age_of_oldest_legitimate_sample; - int64_t offset_difference = - best_offset_so_far - clock_private_info->local_to_source_time_offset; - - debug(2, "Lookback difference: %f ms with %d samples checked of %d samples total.", - 0.000001 * offset_difference, samples_checked, number_of_samples); - clock_private_info->local_to_source_time_offset = best_offset_so_far; - - debug(2, "Master sampling started %f ms before becoming master.", - 0.000001 * (reception_time - age_of_oldest_legitimate_sample)); - clock_private_info->flags &= ~(1 << clock_is_becoming_master); - clock_private_info->flags |= 1 << clock_is_master; - clock_private_info->previous_offset_time = 0; - } else if (clock_private_info->previous_offset_time != 0) { - // i.e. if it's not becoming a master and there has been a previous follow_up + } + clock_private_info->mastership_start_time = age_of_oldest_legitimate_sample; + int64_t offset_difference = + best_offset_so_far - clock_private_info->local_to_source_time_offset; + + debug(2, "Lookback difference: %f ms with %d samples checked of %d samples total.", + 0.000001 * offset_difference, samples_checked, number_of_samples); + clock_private_info->local_to_source_time_offset = best_offset_so_far; + + debug(2, "Master sampling started %f ms before becoming master.", + 0.000001 * (reception_time - age_of_oldest_legitimate_sample)); + clock_private_info->flags &= ~(1 << clock_is_becoming_master); + clock_private_info->flags |= 1 << clock_is_master; + clock_private_info->previous_offset_time = 0; + } else if (clock_private_info->previous_offset_time != 0) { + // i.e. if it's not becoming a master and there has been a previous follow_up int64_t time_since_last_sync = reception_time - clock_private_info->last_sync_time; int64_t sync_timeout = 60000000000; // nanoseconds debug(2, "Sync interval: %f seconds.", 0.000000001 * time_since_last_sync); @@ -379,8 +379,8 @@ void handle_follow_up(char *buf, __attribute__((unused)) ssize_t recv_len, offset = clock_private_info->previous_offset + jitter; } } else if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { - debug(1, "Lost sync with clock %" PRIx64 " at %s. Resynchronising.", clock_private_info->clock_id, - clock_private_info->ip); + debug(1, "Lost sync with clock %" PRIx64 " at %s. Resynchronising.", + clock_private_info->clock_id, clock_private_info->ip); // leave the offset as it was coming in and take it as a sync time clock_private_info->last_sync_time = reception_time; } diff --git a/nqptp-utilities.c b/nqptp-utilities.c index 07367ec..b444e1e 100644 --- a/nqptp-utilities.c +++ b/nqptp-utilities.c @@ -19,25 +19,25 @@ #include "nqptp-utilities.h" #include -#include // fcntl etc. -#include // getifaddrs +#include // fcntl etc. +#include // getifaddrs #ifdef CONFIG_FOR_LINUX #include // sockaddr_ll #endif #ifdef CONFIG_FOR_FREEBSD -#include -#include -#include -#include #include +#include +#include +#include +#include #endif -#include // getaddrinfo etc. -#include // snprintf -#include // malloc, free -#include // memset strcpy, etc. +#include // getaddrinfo etc. +#include // snprintf +#include // malloc, free +#include // memset strcpy, etc. #include "debug.h" @@ -153,7 +153,6 @@ void debug_print_buffer(int level, char *buf, size_t buf_len) { } } - uint64_t get_self_clock_id() { // make up a clock ID based on an interfaces' MAC char local_clock_id[8]; @@ -178,15 +177,15 @@ uint64_t get_self_clock_id() { #else // This AF_LINK stuff hasn't been tested! #ifdef AF_LINK - struct sockaddr_dl * sdl = (struct sockaddr_dl *) ifa->ifa_addr; - if ((sdl) && (sdl->sdl_family == AF_LINK)) { + struct sockaddr_dl *sdl = (struct sockaddr_dl *)ifa->ifa_addr; + if ((sdl) && (sdl->sdl_family == AF_LINK)) { if (sdl->sdl_type == IFT_ETHER) { char *s = LLADDR(sdl); int i; for (i = 0; i < sdl->sdl_alen; i++) { - debug(1,"char %d: \"%c\".", i, *s); - // *t++ = (uint8_t)*s++; - } + debug(1, "char %d: \"%c\".", i, *s); + // *t++ = (uint8_t)*s++; + } found = 1; } } diff --git a/nqptp.c b/nqptp.c index 2fbe0d5..e067e99 100644 --- a/nqptp.c +++ b/nqptp.c @@ -37,17 +37,17 @@ #include #include // close -#include /* For O_* constants */ +#include /* For O_* constants */ +#include // for shared memory stuff #include // for fd_set -#include // for timeval -#include // for shared memory stuff -#include // umask +#include // umask +#include // for timeval #include // SIGTERM and stuff like that #ifdef CONFIG_FOR_FREEBSD -#include #include +#include #endif #ifndef FIELD_SIZEOF @@ -204,9 +204,8 @@ int main(int argc, char **argv) { } #ifdef CONFIG_FOR_FREEBSD - shared_memory = - (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), PROT_READ | PROT_WRITE, - MAP_SHARED, shm_fd, 0); + shared_memory = (struct shm_structure *)mmap(NULL, sizeof(struct shm_structure), + PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); #endif #ifdef CONFIG_FOR_LINUX @@ -314,7 +313,7 @@ int main(int argc, char **argv) { (clock_source_private_data *)&clocks_private); } else if (recv_len >= (ssize_t)sizeof(struct ptp_common_message_header)) { debug_print_buffer(2, buf, recv_len); - + // check its credentials // the sending and receiving ports must be the same (i.e. 319 -> 319 or 320 -> 320)