From: Mike Brady <4265913+mikebrady@users.noreply.github.com> Date: Mon, 21 Jun 2021 19:20:24 +0000 (+0100) Subject: Drop epoll in favour of select. Stop using Linux-specific ethernet timing constructs... X-Git-Tag: 1.2~119 X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=00bf88d665aec7689e379d4844a887dcdea6ce40;p=thirdparty%2Fnqptp.git Drop epoll in favour of select. Stop using Linux-specific ethernet timing constructs. Remove redundant and commented-out code. --- diff --git a/nqptp-clock-sources.c b/nqptp-clock-sources.c index b119aa7..3d05020 100644 --- a/nqptp-clock-sources.c +++ b/nqptp-clock-sources.c @@ -216,26 +216,10 @@ void update_master() { clocks_private[best_so_far].flags |= (1 << clock_is_master); if (old_master != best_so_far) { - // clang-format off - // Now we use the last few samples to calculate the best offset for the - // new master clock. + uint64_t oldest_acceptable_master_clock_time = + clocks_private[best_so_far].source_time + 1150000000; // ns. - // 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 in the future. - - // This 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 second further into the future. - - // Allow the samples to give a valid master clock time up to this much later than the single definitive sample we have: - - uint64_t oldest_acceptable_master_clock_time = clocks_private[best_so_far].source_time + 1150000000; - - // we will try to improve on this single definitive local_to_source_time_offset we have + // 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 = clocks_private[best_so_far].local_to_source_time_offset; @@ -243,62 +227,78 @@ void update_master() { int number_of_samples = MAX_TIMING_SAMPLES - clocks_private[best_so_far].vacant_samples; int samples_checked = 0; -// if (0) { if (number_of_samples > 0) { - debug(3,"Number of samples: %d.", number_of_samples); + debug(3, "Number of samples: %d.", number_of_samples); uint64_t time_now = get_time_now(); + + // 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 = time_now - 10000000000; // only go back this far (ns) int i; for (i = 0; i < number_of_samples; i++) { - int64_t age_relative_to_oldest_acceptable_time = clocks_private[best_so_far].samples[i].local_time - oldest_acceptable_time; + int64_t age_relative_to_oldest_acceptable_time = + clocks_private[best_so_far].samples[i].local_time - oldest_acceptable_time; if (age_relative_to_oldest_acceptable_time > 0) { - if (clocks_private[best_so_far].samples[i].local_time < age_of_oldest_legitimate_sample) { + if (clocks_private[best_so_far].samples[i].local_time < + age_of_oldest_legitimate_sample) { age_of_oldest_legitimate_sample = clocks_private[best_so_far].samples[i].local_time; } - uint64_t possible_offset = clocks_private[best_so_far].samples[i].clock_time - clocks_private[best_so_far].samples[i].local_time; - uint64_t possible_master_clock_time = clocks_private[best_so_far].local_time + possible_offset; - int64_t age_relative_to_oldest_acceptable_master_clock_time = possible_master_clock_time - oldest_acceptable_master_clock_time; + uint64_t possible_offset = clocks_private[best_so_far].samples[i].clock_time - + clocks_private[best_so_far].samples[i].local_time; + uint64_t possible_master_clock_time = + clocks_private[best_so_far].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"); + debug(3, "new best offset"); best_offset_so_far = possible_offset; changes_made++; } } else { - debug(3,"sample too far into the future"); + debug(3, "sample too far into the future"); } } else { - debug(3,"sample too old"); + debug(3, "sample too old"); } } - } -// if (changes_made == 0) { -// clocks_private[best_so_far].previous_offset_time = 0; // if you have no previous samples (how?), then resync -// clocks_private[best_so_far].mastership_start_time = clocks_private[best_so_far].local_time; -// } else { - clocks_private[best_so_far].mastership_start_time = age_of_oldest_legitimate_sample; - int64_t offset_difference = best_offset_so_far - clocks_private[best_so_far].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); - clocks_private[best_so_far].local_to_source_time_offset = best_offset_so_far; + clocks_private[best_so_far].mastership_start_time = age_of_oldest_legitimate_sample; + int64_t offset_difference = + best_offset_so_far - clocks_private[best_so_far].local_to_source_time_offset; -// clocks_private[best_so_far].previous_offset_time = clocks_private[best_so_far].local_time; -// clocks_private[best_so_far].previous_offset = clocks_private[best_so_far].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); + clocks_private[best_so_far].local_to_source_time_offset = best_offset_so_far; - debug(2,"Master sampling started %f ms before becoming master.", 0.000001 * (clocks_private[best_so_far].local_time - age_of_oldest_legitimate_sample)); + debug(2, "Master sampling started %f ms before becoming master.", + 0.000001 * (clocks_private[best_so_far].local_time - age_of_oldest_legitimate_sample)); update_master_clock_info(clocks_private[best_so_far].clock_id, (const char *)&clocks_private[best_so_far].ip, clocks_private[best_so_far].local_time, clocks_private[best_so_far].local_to_source_time_offset, clocks_private[best_so_far].mastership_start_time); - // clang-format on clocks_private[best_so_far].previous_offset_time = 0; // resync } diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h index 146b5d8..68de9e2 100644 --- a/nqptp-clock-sources.h +++ b/nqptp-clock-sources.h @@ -53,7 +53,7 @@ typedef struct { // for garbage collection uint64_t time_of_last_use; // will be taken out of use if not used for a while and not in the // timing peer group - // (A member of the timing peer group could appear and disappear) + // (A member of the timing peer group could appear and disappear so will not be gc'ed.) // for Announce Qualification uint64_t announce_times[4]; // we'll check qualification and currency using these int is_one_of_ours; // true if it is one of our own clocks diff --git a/nqptp-message-handlers.c b/nqptp-message-handlers.c index 1fcebd6..39ee835 100644 --- a/nqptp-message-handlers.c +++ b/nqptp-message-handlers.c @@ -243,86 +243,73 @@ void handle_follow_up(char *buf, __attribute__((unused)) ssize_t recv_len, if (clock_private_info->next_sample_goes_here == MAX_TIMING_SAMPLES) clock_private_info->next_sample_goes_here = 0; - // if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { - if (1) { - debug(2, "FOLLOWUP from %" PRIx64 ", %s.", clock_private_info->clock_id, - &clock_private_info->ip); - - // check to see the difference between the previous preciseOriginTimestamp - - // update the shared clock information - uint64_t offset = preciseOriginTimestamp - reception_time; - - int64_t jitter = 0; - // if there has never been a previous follow_up or if it was long ago (more than 15 seconds), - // don't use it - if (clock_private_info->previous_offset_time != 0) { - 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); - if (time_since_last_sync < sync_timeout) { - // do acceptance checking - // if the new offset is greater, by any amount, than the old offset - // accept it - // if it is less than the new offset by up to what a reasonable drift divergence would allow - // accept it - // otherwise, reject it - // drift divergence of 1000 ppm (which is huge) would give 125 us per 125 ms. - - jitter = offset - clock_private_info->previous_offset; - - uint64_t jitter_timing_interval = reception_time - clock_private_info->previous_offset_time; - long double jitterppm = 0.0; - if (jitter_timing_interval != 0) { - jitterppm = (0.001 * (jitter * 1000000000)) / jitter_timing_interval; - debug(2, "jitter: %" PRId64 " in: %" PRId64 " ns, %+f ppm ", jitter, - jitter_timing_interval, jitterppm); - } - if (jitterppm >= -1000) { - // we take a positive or small negative jitter as a sync event - // as we have a new figure for the difference between the local clock and the - // remote clock which is almost the same or greater than our previous estimate - clock_private_info->last_sync_time = reception_time; - } else { - // let our previous estimate drop by some parts-per-million - // jitter = (-100 * jitter_timing_interval) / 1000000; - jitter = -10 * 1000; // this is nanoseconds in, supposedly, 125 milliseconds. 12.5 us / - // 125 ms is 100 ppm. - offset = clock_private_info->previous_offset + jitter; - } - } else if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { - warn("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 + 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; + // if there has never been a previous follow_up or if it was long ago (more than 15 seconds), + // don't use it + if (clock_private_info->previous_offset_time != 0) { + 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); + if (time_since_last_sync < sync_timeout) { + + // Do acceptance checking. + // If the new offset is greater, by any amount, than the old offset, + // accept it. + // If it is less than the new offset by up to what a reasonable drift divergence would allow, + // accept it. + // Otherwise, reject it + + // A drift divergence of 100 ppm would give 12.5 us per 125 ms. + + jitter = offset - clock_private_info->previous_offset; + + uint64_t jitter_timing_interval = reception_time - clock_private_info->previous_offset_time; + long double jitterppm = 0.0; + if (jitter_timing_interval != 0) { + jitterppm = (0.001 * (jitter * 1000000000)) / jitter_timing_interval; + debug(2, "jitter: %" PRId64 " in: %" PRId64 " ns, %+f ppm ", jitter, jitter_timing_interval, + jitterppm); + } + if (jitterppm >= -1000) { + // we take a positive or small negative jitter as a sync event + // as we have a new figure for the difference between the local clock and the + // remote clock which is almost the same or greater than our previous estimate clock_private_info->last_sync_time = reception_time; + } else { + // let our previous estimate drop fall back by this many nanoseconds + jitter = -10 * 1000; // this is nanoseconds in, supposedly, 125 milliseconds. 12.5 us / + // 125 ms is 100 ppm. + offset = clock_private_info->previous_offset + jitter; } - } else { + } else if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { + warn("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; } + } else { + clock_private_info->last_sync_time = reception_time; + } - // uint64_t estimated_offset = offset; - - uint32_t old_flags = clock_private_info->flags; - - // if ((clock_private_info->flags & (1 << clock_is_valid)) == 0) { - // debug(1, "clock %" PRIx64 " is now valid at: %s", packet_clock_id, clock_private_info->ip); - //} - - // clock_private_info->clock_id = packet_clock_id; - clock_private_info->local_time = reception_time; - clock_private_info->source_time = preciseOriginTimestamp; - clock_private_info->local_to_source_time_offset = offset; + uint32_t old_flags = clock_private_info->flags; - if (old_flags != clock_private_info->flags) { - update_master(); - } else if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { - update_master_clock_info(clock_private_info->clock_id, (const char *)&clock_private_info->ip, - reception_time, offset, clock_private_info->mastership_start_time); - debug(3, "clock: %" PRIx64 ", time: %" PRIu64 ", offset: %" PRId64 ", jitter: %+f ms.", - clock_private_info->clock_id, reception_time, offset, 0.000001 * jitter); - } + clock_private_info->local_time = reception_time; + clock_private_info->source_time = preciseOriginTimestamp; + clock_private_info->local_to_source_time_offset = offset; - clock_private_info->previous_offset = offset; - clock_private_info->previous_offset_time = reception_time; + if (old_flags != clock_private_info->flags) { + update_master(); + } else if ((clock_private_info->flags & (1 << clock_is_master)) != 0) { + update_master_clock_info(clock_private_info->clock_id, (const char *)&clock_private_info->ip, + reception_time, offset, clock_private_info->mastership_start_time); + debug(3, "clock: %" PRIx64 ", time: %" PRIu64 ", offset: %" PRId64 ", jitter: %+f ms.", + clock_private_info->clock_id, reception_time, offset, 0.000001 * jitter); } + + clock_private_info->previous_offset = offset; + clock_private_info->previous_offset_time = reception_time; } diff --git a/nqptp-utilities.c b/nqptp-utilities.c index dec439a..72f8e0c 100644 --- a/nqptp-utilities.c +++ b/nqptp-utilities.c @@ -19,31 +19,16 @@ #include "nqptp-utilities.h" #include -#include // fcntl etc. -#include // getifaddrs -#include // sockaddr_ll -#include // SOF_TIMESTAMPING_TX_HARDWARE and friends -#include // getaddrinfo etc. -#include // snprintf -#include // malloc, free -#include // memset strcpy, etc. +#include // fcntl etc. +#include // getifaddrs +#include // sockaddr_ll +#include // getaddrinfo etc. +#include // snprintf +#include // malloc, free +#include // memset strcpy, etc. #include "debug.h" -#ifndef SO_TIMESTAMPING -#define SO_TIMESTAMPING 37 -#define SCM_TIMESTAMPING SO_TIMESTAMPING -#endif -#ifndef SO_TIMESTAMPNS -#define SO_TIMESTAMPNS 35 -#endif -#ifndef SIOCGSTAMPNS -#define SIOCGSTAMPNS 0x8907 -#endif -#ifndef SIOCSHWTSTAMP -#define SIOCSHWTSTAMP 0x89b0 -#endif - void open_sockets_at_port(uint16_t port, sockets_open_bundle *sockets_open_stuff) { // open up sockets for UDP ports 319 and 320 @@ -82,15 +67,6 @@ void open_sockets_at_port(uint16_t port, sockets_open_bundle *sockets_open_stuff if (!ret) ret = bind(fd, p->ai_addr, p->ai_addrlen); - int so_timestamping_flags = SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_TX_SOFTWARE | - SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_RX_SOFTWARE | - SOF_TIMESTAMPING_SOFTWARE | SOF_TIMESTAMPING_RAW_HARDWARE; - // int so_timestamping_flags = SOF_TIMESTAMPING_RX_SOFTWARE ; - - if (ret == 0) - ret = setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_flags, - sizeof(so_timestamping_flags)); - int flags = fcntl(fd, F_GETFL); fcntl(fd, F_SETFL, flags | O_NONBLOCK); @@ -201,39 +177,5 @@ uint64_t get_self_clock_id() { // it's in Network Byte Order! uint64_t result; memcpy(&result, local_clock_id, sizeof(result)); - // debug(1,"local_clock_id: %" PRIx64 ".", result); return result; -} - -/* -void send_delay_req_message(int socket_number, SOCKADDR *from_sock_addr, uint16_t seqno) { - struct ptp_delay_req_message m; - memset(&m, 0, sizeof(m)); - m.header.transportSpecificAndMessageID = 0x11; // Table 19, pp 125, 1 byte field - m.header.reservedAndVersionPTP = 0x02; // 1 byte field - m.header.messageLength = htons(44); - m.header.flags = htons(0x608); - m.header.sourcePortID = htons(1); - m.header.controlOtherMessage = 5; // 1 byte field - m.header.sequenceId = htons(seqno); - m.header.logMessagePeriod = 0x7f; // Table 24, pp 128 - uint64_t sid = get_self_clock_id(); - memcpy(&m.header.clockIdentity, &sid, sizeof(uint64_t)); - struct msghdr header; - struct iovec io; - memset(&header, 0, sizeof(header)); - memset(&io, 0, sizeof(io)); - header.msg_name = from_sock_addr; - header.msg_namelen = sizeof(SOCKADDR); - header.msg_iov = &io; - header.msg_iov->iov_base = &m; - header.msg_iov->iov_len = sizeof(m); - header.msg_iovlen = 1; - uint64_t transmission_time = get_time_now(); // in case nothing better works - if ((sendmsg(socket_number, &header, 0)) == -1) { - debug(1, "Error in sendmsg [errno = %d]", errno); - } else { - debug_print_buffer(1, (char *)&m, sizeof(m)); - } -} -*/ +} \ No newline at end of file diff --git a/nqptp.c b/nqptp.c index a1d3f48..09eb274 100644 --- a/nqptp.c +++ b/nqptp.c @@ -30,7 +30,7 @@ #endif #include // inet_ntop -#include // printf +#include // fprint #include // malloc; #include // memset @@ -41,10 +41,7 @@ #include // for shared memory stuff #include // umask -#include // group stuff - #include // SIGTERM and stuff like that -#include #ifndef FIELD_SIZEOF #define FIELD_SIZEOF(t, f) (sizeof(((t *)0)->f)) @@ -52,8 +49,7 @@ // 8 samples per second -#define BUFLEN 4096 // Max length of buffer -#define MAX_EVENTS 128 // For epoll +#define BUFLEN 4096 // Max length of buffer sockets_open_bundle sockets_open_stuff; @@ -159,8 +155,6 @@ int main(int argc, char **argv) { epoll_fd = -1; shared_memory = NULL; - // memset(sources,0,sizeof(sources)); - // level 0 is no messages, level 3 is most messages -- see debug.h // control-c (SIGINT) cleanly struct sigaction act; @@ -192,20 +186,12 @@ int main(int argc, char **argv) { int shm_fd = -1; mode_t oldumask = umask(0); - struct group *grp = getgrnam("nqptp"); - if (grp == NULL) { - inform("the group \"nqptp\" was not found, will try \"root\" group instead."); - } shm_fd = shm_open(STORAGE_ID, O_RDWR | O_CREAT, 0666); if (shm_fd == -1) { die("cannot open shared memory \"%s\".", STORAGE_ID); } (void)umask(oldumask); - if (fchown(shm_fd, -1, grp != NULL ? grp->gr_gid : 0) < 0) { - warn("failed to set ownership of shared memory \"%s\" to group \"nqptp\".", STORAGE_ID); - } - if (ftruncate(shm_fd, sizeof(struct shm_structure)) == -1) { die("failed to set size of shared memory \"%s\".", STORAGE_ID); } @@ -239,176 +225,142 @@ int main(int argc, char **argv) { // now, get down to business if (sockets_open_stuff.sockets_open > 0) { - struct epoll_event event; - int epoll_fd = epoll_create(32); - - if (epoll_fd == -1) - die("Failed to create epoll file descriptor\n"); - - unsigned int ep; - for (ep = 0; ep < sockets_open_stuff.sockets_open; ep++) { - // if (sockets[s].number > smax) - // smax = sockets[s].number; - event.events = EPOLLIN; - event.data.fd = sockets_open_stuff.sockets[ep].number; - if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sockets_open_stuff.sockets[ep].number, &event) != 0) - die("failed to add socket %d to epoll", sockets_open_stuff.sockets[ep].number); - else - debug(3, "add socket %d to epoll", sockets_open_stuff.sockets[ep].number); - } - while (1) { + fd_set readSockSet; + struct timeval timeout; + FD_ZERO(&readSockSet); + int smax = -1; + unsigned int s; + for (s = 0; s < sockets_open_stuff.sockets_open; s++) { + if (sockets_open_stuff.sockets[s].number > smax) + smax = sockets_open_stuff.sockets[s].number; + FD_SET(sockets_open_stuff.sockets[s].number, &readSockSet); + } - struct epoll_event events[MAX_EVENTS]; - // the timeout is in milliseconds - int event_count = epoll_wait(epoll_fd, events, MAX_EVENTS, 1000); + timeout.tv_sec = 0; + timeout.tv_usec = 10000; // timeout after ten milliseconds + int retval = select(smax + 1, &readSockSet, NULL, NULL, &timeout); uint64_t reception_time = get_time_now(); // use this if other methods fail - - int t; - for (t = 0; t < event_count; t++) { - int socket_number = events[t].data.fd; - { - - SOCKADDR from_sock_addr; - memset(&from_sock_addr, 0, sizeof(SOCKADDR)); - - struct { - struct cmsghdr cm; - char control[512]; - } control; - - struct msghdr msg; - struct iovec iov[1]; - memset(iov, 0, sizeof(iov)); - memset(&msg, 0, sizeof(msg)); - memset(&control, 0, sizeof(control)); - - iov[0].iov_base = buf; - iov[0].iov_len = BUFLEN; - - msg.msg_iov = iov; - msg.msg_iovlen = 1; - - msg.msg_name = &from_sock_addr; - msg.msg_namelen = sizeof(from_sock_addr); - msg.msg_control = &control; - msg.msg_controllen = sizeof(control); - - uint16_t receiver_port = 0; - // int msgsize = recv(udpsocket_fd, &msg_buffer, 4, 0); - recv_len = recvmsg(socket_number, &msg, MSG_DONTWAIT); - - if (recv_len != -1) { - // get the receiver port - unsigned int jp; - for (jp = 0; jp < sockets_open_stuff.sockets_open; jp++) { - if (socket_number == sockets_open_stuff.sockets[jp].number) - receiver_port = sockets_open_stuff.sockets[jp].port; - } - } - if (recv_len == -1) { - if (errno == EAGAIN) { - usleep(1000); // this can happen, it seems... - } else { - debug(1, "recvmsg() error %d", errno); + if (retval > 0) { + unsigned t; + for (t = 0; t < sockets_open_stuff.sockets_open; t++) { + int socket_number = sockets_open_stuff.sockets[t].number; + if (FD_ISSET(socket_number, &readSockSet)) { + + SOCKADDR from_sock_addr; + memset(&from_sock_addr, 0, sizeof(SOCKADDR)); + + struct { + struct cmsghdr cm; + char control[512]; + } control; + + struct msghdr msg; + struct iovec iov[1]; + memset(iov, 0, sizeof(iov)); + memset(&msg, 0, sizeof(msg)); + memset(&control, 0, sizeof(control)); + + iov[0].iov_base = buf; + iov[0].iov_len = BUFLEN; + + msg.msg_iov = iov; + msg.msg_iovlen = 1; + + msg.msg_name = &from_sock_addr; + msg.msg_namelen = sizeof(from_sock_addr); + msg.msg_control = &control; + msg.msg_controllen = sizeof(control); + + uint16_t receiver_port = 0; + // int msgsize = recv(udpsocket_fd, &msg_buffer, 4, 0); + recv_len = recvmsg(socket_number, &msg, MSG_DONTWAIT); + + if (recv_len != -1) { + // get the receiver port + unsigned int jp; + for (jp = 0; jp < sockets_open_stuff.sockets_open; jp++) { + if (socket_number == sockets_open_stuff.sockets[jp].number) + receiver_port = sockets_open_stuff.sockets[jp].port; + } } - // check if it's a control port message before checking for the length of the message. - } else if (receiver_port == NQPTP_CONTROL_PORT) { - handle_control_port_messages(buf, recv_len, - (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); - debug(3, "Received %d bytes control message on reception.", msg.msg_controllen); - // get the time - int level, type; - struct cmsghdr *cm; - struct timespec *ts = NULL; - for (cm = CMSG_FIRSTHDR(&msg); cm != NULL; cm = CMSG_NXTHDR(&msg, cm)) { - level = cm->cmsg_level; - type = cm->cmsg_type; - if (SOL_SOCKET == level && SO_TIMESTAMPING == type) { - /* - struct timespec *stamp = (struct timespec *)CMSG_DATA(cm); - fprintf(stderr, "SO_TIMESTAMPING Rx: "); - fprintf(stderr, "SW %ld.%09ld\n", (long)stamp->tv_sec, - (long)stamp->tv_nsec); stamp++; - // skip deprecated HW transformed - stamp++; - fprintf(stderr, "SO_TIMESTAMPING Rx: "); - fprintf(stderr, "HW raw %ld.%09ld\n", (long)stamp->tv_sec, - (long)stamp->tv_nsec); - */ - ts = (struct timespec *)CMSG_DATA(cm); - reception_time = ts->tv_sec; - reception_time = reception_time * 1000000000; - reception_time = reception_time + ts->tv_nsec; + if (recv_len == -1) { + if (errno == EAGAIN) { + usleep(1000); // this can happen, it seems... } else { - debug(3, "Can't establish a reception time -- falling back on get_time_now()."); + debug(1, "recvmsg() error %d", errno); } - } + // check if it's a control port message before checking for the length of the + // message. + } else if (receiver_port == NQPTP_CONTROL_PORT) { + handle_control_port_messages(buf, recv_len, + (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); + debug(3, "Received %d bytes control message on reception.", msg.msg_controllen); - // check its credentials - // the sending and receiving ports must be the same (i.e. 319 -> 319 or 320 -> 320) + // check its credentials + // the sending and receiving ports must be the same (i.e. 319 -> 319 or 320 -> 320) - // initialise the connection info - void *sender_addr = NULL; - uint16_t sender_port = 0; + // initialise the connection info + void *sender_addr = NULL; + uint16_t sender_port = 0; - sa_family_t connection_ip_family = from_sock_addr.SAFAMILY; + sa_family_t connection_ip_family = from_sock_addr.SAFAMILY; #ifdef AF_INET6 - if (connection_ip_family == AF_INET6) { - struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)&from_sock_addr; - sender_addr = &(sa6->sin6_addr); - sender_port = ntohs(sa6->sin6_port); - } + if (connection_ip_family == AF_INET6) { + struct sockaddr_in6 *sa6 = (struct sockaddr_in6 *)&from_sock_addr; + sender_addr = &(sa6->sin6_addr); + sender_port = ntohs(sa6->sin6_port); + } #endif - if (connection_ip_family == AF_INET) { - struct sockaddr_in *sa4 = (struct sockaddr_in *)&from_sock_addr; - sender_addr = &(sa4->sin_addr); - sender_port = ntohs(sa4->sin_port); - } + if (connection_ip_family == AF_INET) { + struct sockaddr_in *sa4 = (struct sockaddr_in *)&from_sock_addr; + sender_addr = &(sa4->sin_addr); + sender_port = ntohs(sa4->sin_port); + } - // if ((sender_port == receiver_port) && (connection_ip_family == AF_INET)) { - if (sender_port == receiver_port) { - - char sender_string[256]; - memset(sender_string, 0, sizeof(sender_string)); - inet_ntop(connection_ip_family, sender_addr, sender_string, sizeof(sender_string)); - // now, find or create a record for this ip - int the_clock = find_clock_source_record( - sender_string, (clock_source_private_data *)&clocks_private); - // not sure about requiring a Sync before creating it... - if ((the_clock == -1) && ((buf[0] & 0xF) == Sync)) { - the_clock = create_clock_source_record( + if (sender_port == receiver_port) { + + char sender_string[256]; + memset(sender_string, 0, sizeof(sender_string)); + inet_ntop(connection_ip_family, sender_addr, sender_string, sizeof(sender_string)); + // now, find or create a record for this ip + int the_clock = find_clock_source_record( sender_string, (clock_source_private_data *)&clocks_private); - } - if (the_clock != -1) { - clocks_private[the_clock].time_of_last_use = - reception_time; // for garbage collection - switch (buf[0] & 0xF) { - case Announce: - // needed to reject messages coming from self - update_clock_self_identifications((clock_source_private_data *)&clocks_private); - handle_announce(buf, recv_len, &clocks_private[the_clock], reception_time); - break; - case Follow_Up: { - handle_follow_up(buf, recv_len, &clocks_private[the_clock], reception_time); - } break; - default: - debug_print_buffer(2, buf, recv_len); // unusual messages will have debug level 1. - break; + // not sure about requiring a Sync before creating it... + if ((the_clock == -1) && ((buf[0] & 0xF) == Sync)) { + the_clock = create_clock_source_record( + sender_string, (clock_source_private_data *)&clocks_private); + } + if (the_clock != -1) { + clocks_private[the_clock].time_of_last_use = + reception_time; // for garbage collection + switch (buf[0] & 0xF) { + case Announce: + // needed to reject messages coming from self + update_clock_self_identifications((clock_source_private_data *)&clocks_private); + handle_announce(buf, recv_len, &clocks_private[the_clock], reception_time); + break; + case Follow_Up: { + handle_follow_up(buf, recv_len, &clocks_private[the_clock], reception_time); + } break; + default: + debug_print_buffer(2, buf, + recv_len); // unusual messages will have debug level 1. + break; + } } } } } } } - manage_clock_sources(reception_time, (clock_source_private_data *)&clocks_private); + if (retval >= 0) + manage_clock_sources(reception_time, (clock_source_private_data *)&clocks_private); } } - - // here, close all the sockets... - + // should never get to here, unless no sockets were ever opened. return 0; }