From: Mike Brady <4265913+mikebrady@users.noreply.github.com> Date: Sun, 4 Apr 2021 14:33:54 +0000 (+0100) Subject: Move the port opening stuff to the utilities file and turn it inot a function. X-Git-Tag: 1.1-dev~48 X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=4d6995817ca34cd985d5616934bd6ab95b967817;p=thirdparty%2Fnqptp.git Move the port opening stuff to the utilities file and turn it inot a function. --- diff --git a/nqptp-clock-sources.c b/nqptp-clock-sources.c index 1001127..5eda7dd 100644 --- a/nqptp-clock-sources.c +++ b/nqptp-clock-sources.c @@ -19,6 +19,7 @@ #include #include "nqptp-clock-sources.h" +#include "nqptp-ptp-definitions.h" #include "debug.h" #ifndef FIELD_SIZEOF @@ -91,7 +92,14 @@ void manage_clock_sources(uint64_t reception_time, clock_source *clocks_shared_i for (i = 0; i < MAX_CLOCKS; i++) { if (clocks_private_info[i].in_use != 0) { int64_t time_since_last_sync = reception_time - clocks_private_info[i].t2; - if (time_since_last_sync > 60000000000) { + // the following give the sync receipt time in whole seconds + // depending on the aPTPinitialLogSyncInterval and the aPTPsyncReceiptTimeout + int64_t syncTimeout = (1 << (32 + aPTPinitialLogSyncInterval)); + syncTimeout = syncTimeout * aPTPsyncReceiptTimeout; + syncTimeout = syncTimeout >> 32; + // seconds to nanoseconds + syncTimeout = syncTimeout * 1000000000; + if (time_since_last_sync > syncTimeout) { debug(1, "deactivating source %d with clock_id %" PRIx64 " on ip: %s.", i, clocks_shared_info[i].clock_id, &clocks_shared_info[i].ip); int rc = pthread_mutex_lock(&shared_memory->shm_mutex); diff --git a/nqptp-clock-sources.h b/nqptp-clock-sources.h index 84aad55..448929f 100644 --- a/nqptp-clock-sources.h +++ b/nqptp-clock-sources.h @@ -34,6 +34,10 @@ typedef struct { uint16_t in_use; enum stage current_stage; uint64_t t2; + // for Announce Qualification + uint64_t announce_times[4]; // we'll check qualification and currency using these + int announce_times_valid_count; + int announce_is_valid; // this may mean it's a master clock_source } clock_source_private_data; int find_clock_source_record(char *sender_string, uint64_t packet_clock_id, diff --git a/nqptp-ptp-definitions.h b/nqptp-ptp-definitions.h index bb6a3db..21f8459 100644 --- a/nqptp-ptp-definitions.h +++ b/nqptp-ptp-definitions.h @@ -20,10 +20,59 @@ #ifndef NQPTP_PTP_DEFINITIONS_H #define NQPTP_PTP_DEFINITIONS_H +// This is for definitions and stuff that flows more or less directly +// from external sources. + +// They may not be used. Yet. + +// Derived from https://github.com/rroussel/OpenAvnu/blob/ArtAndLogic-aPTP-changes/daemons/gptp/gptp_cfg.ini: + +#define aPTPpriority1 248 +#define aPTPpriority2 248 +#define aPTPaccuracy 254 + +// "Per the Apple Vendor PTP profile" +// these seem to be log2 of seconds, thus 0 is 2^0 or 1 sec, -3 to 2^-3 or 0.125 sec +// see 7.7.7.2 +#define aPTPinitialLogAnnounceInterval 0 + +// see 7.7.2.3 +#define aPTPinitialLogSyncInterval -3 + +// This doesn't seem to be used in OpenAvnu +// but see 7.7.3.1, so it looks like they are units of the announceInterval, so seconds here +#define aPTPannounceReceiptTimeout 120 + +// "Per the Apple Vendor PTP profile (8*announceReceiptTimeout)" +// This doesn't seem to be used in OpenAvnu +// Guess it's the same idea, but based on aPTPinitialLogSyncInterval +// but it could be based on aPTPinitialLogAnnounceInterval, of course. + +#define aPTPsyncReceiptTimeout 960 + +// "Neighbor propagation delay threshold in nanoseconds" +#define aPTPneighborPropDelayThresh 800 + +// "Sync Receipt Threshold +// This value defines the number of syncs with wrong seqID that will trigger +// the ptp slave to become master (it will start announcing) +// Normally sync messages are sent every 125ms, so setting it to 8 will allow +// up to 1 second of wrong messages before switching" + +#define aPTPsyncReceiptThresh 8 + // References from the IEEE Document ISBN 978-0-7381-5400-8 STD95773. // "IEEE Standard for a Precision Clock Synchronization Protocol for Networked Measurement and // Control Systems" The IEEE Std 1588-2008 (Revision of IEEE Std 1588-2002) + +// See 9.3.2.4.4 FOREIGN_MASTER_TIME_WINDOW and FOREIGN_MASTER_THRESHOLD +// units are the announceInterval +#define FOREIGN_MASTER_TIME_WINDOW 4 +#define FOREIGN_MASTER_THRESHOLD 2 + +// See also 9.3.2.5 Qualification of Announce messages + // Table 19 enum messageType { Sync, diff --git a/nqptp-utilities.c b/nqptp-utilities.c index 1151a02..8ceb125 100644 --- a/nqptp-utilities.c +++ b/nqptp-utilities.c @@ -19,9 +19,101 @@ #include #include +#include +#include +#include +#include +#include +#include +#include #include "nqptp-utilities.h" #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 + + struct addrinfo hints, *info, *p; + int ret; + + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + hints.ai_flags = AI_PASSIVE; + + char portstr[20]; + snprintf(portstr, 20, "%d", port); + + ret = getaddrinfo(NULL, portstr, &hints, &info); + if (ret) { + die("getifaddrs: %s", gai_strerror(ret)); + } + + for (p = info; p; p = p->ai_next) { + ret = 0; + int fd = socket(p->ai_family, p->ai_socktype, IPPROTO_UDP); + int yes = 1; + + // Handle socket open failures if protocol unavailable (or IPV6 not handled) + if (fd != -1) { +#ifdef IPV6_V6ONLY + // some systems don't support v4 access on v6 sockets, but some do. + // since we need to account for two sockets we might as well + // always. + if (p->ai_family == AF_INET6) { + ret |= setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &yes, sizeof(yes)); + } +#endif + + 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); + + // one of the address families will fail on some systems that + // report its availability. do not complain. + + if (ret) { + die("unable to listen on %s port %d. The error is: \"%s\". Daemon must run as root. Or is " + "a " + "separate PTP daemon running?", + p->ai_family == AF_INET6 ? "IPv6" : "IPv4", port, strerror(errno)); + } else { + + debug(2, "listening on %s port %d.", p->ai_family == AF_INET6 ? "IPv6" : "IPv4", port); + sockets_open_stuff->sockets[sockets_open_stuff->sockets_open].number = fd; + sockets_open_stuff->sockets[sockets_open_stuff->sockets_open].port = port; + sockets_open_stuff->sockets_open++; + } + } + } + + freeaddrinfo(info); + } + void debug_print_buffer(int level, char *buf, size_t buf_len) { // printf("Received %u bytes in a packet from %s:%d\n", buf_len, inet_ntoa(si_other.sin_addr), // ntohs(si_other.sin_port)); diff --git a/nqptp-utilities.h b/nqptp-utilities.h index 28e9922..5f5098b 100644 --- a/nqptp-utilities.h +++ b/nqptp-utilities.h @@ -20,9 +20,21 @@ #ifndef NQPTP_UTILITIES_H #define NQPTP_UTILITIES_H #include +#include "nqptp.h" // functions that are specific to NQPTP // general stuff should go in the general-utilities +typedef struct { + int number; + uint16_t port; +} socket_info; + +typedef struct { + unsigned int sockets_open; // also doubles as where to put next one, as sockets are never closed. + socket_info sockets[MAX_OPEN_SOCKETS]; +} sockets_open_bundle; + +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); #endif \ No newline at end of file diff --git a/nqptp.c b/nqptp.c index 4d71b5f..9ae42e2 100644 --- a/nqptp.c +++ b/nqptp.c @@ -62,35 +62,13 @@ #include #include -#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 - // 8 samples per second #define BUFLEN 4096 // Max length of buffer -#define MAX_OPEN_SOCKETS 32 // up to 32 sockets open on ports 319 and 320 - -struct socket_info { - int number; - uint16_t port; -}; +#define MAX_EVENTS 128 // For epoll +sockets_open_bundle sockets_open_stuff; clock_source_private_data clocks_private[MAX_CLOCKS]; - -struct socket_info sockets[MAX_OPEN_SOCKETS]; -unsigned int sockets_open = - 0; // also doubles as where to put next one, as sockets are never closed. struct shm_structure *shared_memory = NULL; // this is where public clock info is available int epoll_fd; @@ -103,13 +81,11 @@ int epoll_fd; #define SAFAMILY sa_family #endif -uint64_t time_then = 0; - void goodbye(void) { // close any open sockets unsigned int i; - for (i = 0; i < sockets_open; i++) - close(sockets[i].number); + for (i = 0; i < sockets_open_stuff.sockets_open; i++) + close(sockets_open_stuff.sockets[i].number); if (shared_memory != NULL) { // mmap cleanup if (munmap(shared_memory, sizeof(struct shm_structure)) != 0) @@ -139,6 +115,8 @@ int main(void) { debug(1, "startup"); atexit(goodbye); + sockets_open_stuff.sockets_open = 0; + epoll_fd = -1; shared_memory = NULL; // memset(sources,0,sizeof(sources)); @@ -163,131 +141,10 @@ int main(void) { pthread_mutexattr_t shared; int err; - 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 ; - - // open up sockets for UDP ports 319 and 320 - - struct addrinfo hints, *info, *p; - int ret; - - // replicating nearly the same code for 319 and 320. Ugh! - - // 319... - memset(&hints, 0, sizeof(hints)); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - ret = getaddrinfo(NULL, "319", &hints, &info); - if (ret) { - die("getifaddrs: %s", gai_strerror(ret)); - } - - for (p = info; p; p = p->ai_next) { - ret = 0; - int fd = socket(p->ai_family, p->ai_socktype, IPPROTO_UDP); - int yes = 1; - - // Handle socket open failures if protocol unavailable (or IPV6 not handled) - if (fd != -1) { -#ifdef IPV6_V6ONLY - // some systems don't support v4 access on v6 sockets, but some do. - // since we need to account for two sockets we might as well - // always. - if (p->ai_family == AF_INET6) { - ret |= setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &yes, sizeof(yes)); - } -#endif - - if (!ret) - ret = bind(fd, p->ai_addr, p->ai_addrlen); - - 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); - - // one of the address families will fail on some systems that - // report its availability. do not complain. - - if (ret) { - die("unable to listen on %s port %d. The error is: \"%s\". Daemon must run as root. Or is " - "a " - "separate PTP daemon running?", - p->ai_family == AF_INET6 ? "IPv6" : "IPv4", 320, strerror(errno)); - } else { - - debug(2, "listening on %s port %d.", p->ai_family == AF_INET6 ? "IPv6" : "IPv4", 319); - sockets[sockets_open].number = fd; - sockets[sockets_open].port = 319; - sockets_open++; - } - } - } - - freeaddrinfo(info); - - // 320... - memset(&hints, 0, sizeof(hints)); - hints.ai_family = AF_UNSPEC; - hints.ai_socktype = SOCK_DGRAM; - hints.ai_flags = AI_PASSIVE; - - ret = getaddrinfo(NULL, "320", &hints, &info); - if (ret) { - die("getifaddrs: %s", gai_strerror(ret)); - } - - for (p = info; p; p = p->ai_next) { - ret = 0; - int fd = socket(p->ai_family, p->ai_socktype, IPPROTO_UDP); - int yes = 1; - - // Handle socket open failures if protocol unavailable (or IPV6 not handled) - if (fd != -1) { -#ifdef IPV6_V6ONLY - // some systems don't support v4 access on v6 sockets, but some do. - // since we need to account for two sockets we might as well - // always. - if (p->ai_family == AF_INET6) { - ret |= setsockopt(fd, IPPROTO_IPV6, IPV6_V6ONLY, &yes, sizeof(yes)); - } -#endif - - if (!ret) - ret = bind(fd, p->ai_addr, p->ai_addrlen); - - if (ret == 0) - 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); - - // one of the address families will fail on some systems that - // report its availability. do not complain. - - if (ret) { - die("unable to listen on %s port %d. The error is: \"%s\". Daemon must run as root. Or is " - "a " - "separate PTP daemon running?", - p->ai_family == AF_INET6 ? "IPv6" : "IPv4", 320, strerror(errno)); - exit(1); - } else { - debug(2, "listening on %s port %d.", p->ai_family == AF_INET6 ? "IPv6" : "IPv4", 320); - sockets[sockets_open].number = fd; - sockets[sockets_open].port = 320; - sockets_open++; - } - } - } + // open sockets 319 and 320 - freeaddrinfo(info); + open_sockets_at_port(319,&sockets_open_stuff); + open_sockets_at_port(320,&sockets_open_stuff); // open a shared memory interface. int shm_fd = -1; @@ -338,9 +195,9 @@ int main(void) { die("mutex initialization failed - %s.", strerror(errno)); } - if (sockets_open > 0) { + // now, get down to business + if (sockets_open_stuff.sockets_open > 0) { -#define MAX_EVENTS 128 struct epoll_event event; int epoll_fd = epoll_create(32); @@ -348,15 +205,15 @@ int main(void) { die("Failed to create epoll file descriptor\n"); unsigned int ep; - for (ep = 0; ep < sockets_open; 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[ep].number; - if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sockets[ep].number, &event) != 0) - die("failed to add socket %d to epoll", sockets[ep].number); + 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[ep].number); + debug(3, "add socket %d to epoll", sockets_open_stuff.sockets[ep].number); } while (1) { @@ -463,9 +320,9 @@ int main(void) { // find the socket in the socket list uint16_t receiver_port = 0; unsigned int jp; - for (jp = 0; jp < sockets_open; jp++) { - if (socket_number == sockets[jp].number) - receiver_port = sockets[jp].port; + 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 (sender_port == receiver_port) { diff --git a/nqptp.h b/nqptp.h index 9f1ade3..93c2a10 100644 --- a/nqptp.h +++ b/nqptp.h @@ -22,6 +22,8 @@ #include "nqptp-shm-structures.h" +#define MAX_OPEN_SOCKETS 16 + extern struct shm_structure *shared_memory; #endif \ No newline at end of file