From: Mike Brady <4265913+mikebrady@users.noreply.github.com> Date: Sun, 14 Mar 2021 22:09:49 +0000 (+0000) Subject: Initial commit from the "listener" experimental app. X-Git-Tag: 1.1-dev~116 X-Git-Url: http://git.ipfire.org/cgi-bin/gitweb.cgi?a=commitdiff_plain;h=c605dceba38c555b738fbbed48c0cbbf33ec9d60;p=thirdparty%2Fnqptp.git Initial commit from the "listener" experimental app. --- diff --git a/.gitignore b/.gitignore index 3523288..fa420fb 100644 --- a/.gitignore +++ b/.gitignore @@ -14,17 +14,13 @@ autom4te.cache /autoscan-*.log /aclocal.m4 /compile -/config.guess -/config.h.in -/config.log -/config.status -/config.sub +/config.* /configure -/configure.scan /depcomp /install-sh /missing /stamp-h1 +/.deps/ # https://www.gnu.org/software/libtool/ @@ -47,3 +43,9 @@ m4/lt~obsolete.m4 # can automatically generate from config.status script # (which is called by configure script)) Makefile + +# Object files +/*.o + +# Executable +nqptp diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..e69de29 diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..6ee6ec4 --- /dev/null +++ b/COPYING @@ -0,0 +1 @@ +For copying terms, refer to the LICENSE file in this directory. \ No newline at end of file diff --git a/ChangeLog b/ChangeLog new file mode 100644 index 0000000..e69de29 diff --git a/INSTALL b/INSTALL new file mode 100644 index 0000000..8865734 --- /dev/null +++ b/INSTALL @@ -0,0 +1,368 @@ +Installation Instructions +************************* + + Copyright (C) 1994-1996, 1999-2002, 2004-2016 Free Software +Foundation, Inc. + + Copying and distribution of this file, with or without modification, +are permitted in any medium without royalty provided the copyright +notice and this notice are preserved. This file is offered as-is, +without warranty of any kind. + +Basic Installation +================== + + Briefly, the shell command './configure && make && make install' +should configure, build, and install this package. The following +more-detailed instructions are generic; see the 'README' file for +instructions specific to this package. Some packages provide this +'INSTALL' file but do not implement all of the features documented +below. The lack of an optional feature in a given package is not +necessarily a bug. More recommendations for GNU packages can be found +in *note Makefile Conventions: (standards)Makefile Conventions. + + The 'configure' shell script attempts to guess correct values for +various system-dependent variables used during compilation. It uses +those values to create a 'Makefile' in each directory of the package. +It may also create one or more '.h' files containing system-dependent +definitions. Finally, it creates a shell script 'config.status' that +you can run in the future to recreate the current configuration, and a +file 'config.log' containing compiler output (useful mainly for +debugging 'configure'). + + It can also use an optional file (typically called 'config.cache' and +enabled with '--cache-file=config.cache' or simply '-C') that saves the +results of its tests to speed up reconfiguring. Caching is disabled by +default to prevent problems with accidental use of stale cache files. + + If you need to do unusual things to compile the package, please try +to figure out how 'configure' could check whether to do them, and mail +diffs or instructions to the address given in the 'README' so they can +be considered for the next release. If you are using the cache, and at +some point 'config.cache' contains results you don't want to keep, you +may remove or edit it. + + The file 'configure.ac' (or 'configure.in') is used to create +'configure' by a program called 'autoconf'. You need 'configure.ac' if +you want to change it or regenerate 'configure' using a newer version of +'autoconf'. + + The simplest way to compile this package is: + + 1. 'cd' to the directory containing the package's source code and type + './configure' to configure the package for your system. + + Running 'configure' might take a while. While running, it prints + some messages telling which features it is checking for. + + 2. Type 'make' to compile the package. + + 3. Optionally, type 'make check' to run any self-tests that come with + the package, generally using the just-built uninstalled binaries. + + 4. Type 'make install' to install the programs and any data files and + documentation. When installing into a prefix owned by root, it is + recommended that the package be configured and built as a regular + user, and only the 'make install' phase executed with root + privileges. + + 5. Optionally, type 'make installcheck' to repeat any self-tests, but + this time using the binaries in their final installed location. + This target does not install anything. Running this target as a + regular user, particularly if the prior 'make install' required + root privileges, verifies that the installation completed + correctly. + + 6. You can remove the program binaries and object files from the + source code directory by typing 'make clean'. To also remove the + files that 'configure' created (so you can compile the package for + a different kind of computer), type 'make distclean'. There is + also a 'make maintainer-clean' target, but that is intended mainly + for the package's developers. If you use it, you may have to get + all sorts of other programs in order to regenerate files that came + with the distribution. + + 7. Often, you can also type 'make uninstall' to remove the installed + files again. In practice, not all packages have tested that + uninstallation works correctly, even though it is required by the + GNU Coding Standards. + + 8. Some packages, particularly those that use Automake, provide 'make + distcheck', which can by used by developers to test that all other + targets like 'make install' and 'make uninstall' work correctly. + This target is generally not run by end users. + +Compilers and Options +===================== + + Some systems require unusual options for compilation or linking that +the 'configure' script does not know about. Run './configure --help' +for details on some of the pertinent environment variables. + + You can give 'configure' initial values for configuration parameters +by setting variables in the command line or in the environment. Here is +an example: + + ./configure CC=c99 CFLAGS=-g LIBS=-lposix + + *Note Defining Variables::, for more details. + +Compiling For Multiple Architectures +==================================== + + You can compile the package for more than one kind of computer at the +same time, by placing the object files for each architecture in their +own directory. To do this, you can use GNU 'make'. 'cd' to the +directory where you want the object files and executables to go and run +the 'configure' script. 'configure' automatically checks for the source +code in the directory that 'configure' is in and in '..'. This is known +as a "VPATH" build. + + With a non-GNU 'make', it is safer to compile the package for one +architecture at a time in the source code directory. After you have +installed the package for one architecture, use 'make distclean' before +reconfiguring for another architecture. + + On MacOS X 10.5 and later systems, you can create libraries and +executables that work on multiple system types--known as "fat" or +"universal" binaries--by specifying multiple '-arch' options to the +compiler but only a single '-arch' option to the preprocessor. Like +this: + + ./configure CC="gcc -arch i386 -arch x86_64 -arch ppc -arch ppc64" \ + CXX="g++ -arch i386 -arch x86_64 -arch ppc -arch ppc64" \ + CPP="gcc -E" CXXCPP="g++ -E" + + This is not guaranteed to produce working output in all cases, you +may have to build one architecture at a time and combine the results +using the 'lipo' tool if you have problems. + +Installation Names +================== + + By default, 'make install' installs the package's commands under +'/usr/local/bin', include files under '/usr/local/include', etc. You +can specify an installation prefix other than '/usr/local' by giving +'configure' the option '--prefix=PREFIX', where PREFIX must be an +absolute file name. + + You can specify separate installation prefixes for +architecture-specific files and architecture-independent files. If you +pass the option '--exec-prefix=PREFIX' to 'configure', the package uses +PREFIX as the prefix for installing programs and libraries. +Documentation and other data files still use the regular prefix. + + In addition, if you use an unusual directory layout you can give +options like '--bindir=DIR' to specify different values for particular +kinds of files. Run 'configure --help' for a list of the directories +you can set and what kinds of files go in them. In general, the default +for these options is expressed in terms of '${prefix}', so that +specifying just '--prefix' will affect all of the other directory +specifications that were not explicitly provided. + + The most portable way to affect installation locations is to pass the +correct locations to 'configure'; however, many packages provide one or +both of the following shortcuts of passing variable assignments to the +'make install' command line to change installation locations without +having to reconfigure or recompile. + + The first method involves providing an override variable for each +affected directory. For example, 'make install +prefix=/alternate/directory' will choose an alternate location for all +directory configuration variables that were expressed in terms of +'${prefix}'. Any directories that were specified during 'configure', +but not in terms of '${prefix}', must each be overridden at install time +for the entire installation to be relocated. The approach of makefile +variable overrides for each directory variable is required by the GNU +Coding Standards, and ideally causes no recompilation. However, some +platforms have known limitations with the semantics of shared libraries +that end up requiring recompilation when using this method, particularly +noticeable in packages that use GNU Libtool. + + The second method involves providing the 'DESTDIR' variable. For +example, 'make install DESTDIR=/alternate/directory' will prepend +'/alternate/directory' before all installation names. The approach of +'DESTDIR' overrides is not required by the GNU Coding Standards, and +does not work on platforms that have drive letters. On the other hand, +it does better at avoiding recompilation issues, and works well even +when some directory options were not specified in terms of '${prefix}' +at 'configure' time. + +Optional Features +================= + + If the package supports it, you can cause programs to be installed +with an extra prefix or suffix on their names by giving 'configure' the +option '--program-prefix=PREFIX' or '--program-suffix=SUFFIX'. + + Some packages pay attention to '--enable-FEATURE' options to +'configure', where FEATURE indicates an optional part of the package. +They may also pay attention to '--with-PACKAGE' options, where PACKAGE +is something like 'gnu-as' or 'x' (for the X Window System). The +'README' should mention any '--enable-' and '--with-' options that the +package recognizes. + + For packages that use the X Window System, 'configure' can usually +find the X include and library files automatically, but if it doesn't, +you can use the 'configure' options '--x-includes=DIR' and +'--x-libraries=DIR' to specify their locations. + + Some packages offer the ability to configure how verbose the +execution of 'make' will be. For these packages, running './configure +--enable-silent-rules' sets the default to minimal output, which can be +overridden with 'make V=1'; while running './configure +--disable-silent-rules' sets the default to verbose, which can be +overridden with 'make V=0'. + +Particular systems +================== + + On HP-UX, the default C compiler is not ANSI C compatible. If GNU CC +is not installed, it is recommended to use the following options in +order to use an ANSI C compiler: + + ./configure CC="cc -Ae -D_XOPEN_SOURCE=500" + +and if that doesn't work, install pre-built binaries of GCC for HP-UX. + + HP-UX 'make' updates targets which have the same time stamps as their +prerequisites, which makes it generally unusable when shipped generated +files such as 'configure' are involved. Use GNU 'make' instead. + + On OSF/1 a.k.a. Tru64, some versions of the default C compiler cannot +parse its '' header file. The option '-nodtk' can be used as a +workaround. If GNU CC is not installed, it is therefore recommended to +try + + ./configure CC="cc" + +and if that doesn't work, try + + ./configure CC="cc -nodtk" + + On Solaris, don't put '/usr/ucb' early in your 'PATH'. This +directory contains several dysfunctional programs; working variants of +these programs are available in '/usr/bin'. So, if you need '/usr/ucb' +in your 'PATH', put it _after_ '/usr/bin'. + + On Haiku, software installed for all users goes in '/boot/common', +not '/usr/local'. It is recommended to use the following options: + + ./configure --prefix=/boot/common + +Specifying the System Type +========================== + + There may be some features 'configure' cannot figure out +automatically, but needs to determine by the type of machine the package +will run on. Usually, assuming the package is built to be run on the +_same_ architectures, 'configure' can figure that out, but if it prints +a message saying it cannot guess the machine type, give it the +'--build=TYPE' option. TYPE can either be a short name for the system +type, such as 'sun4', or a canonical name which has the form: + + CPU-COMPANY-SYSTEM + +where SYSTEM can have one of these forms: + + OS + KERNEL-OS + + See the file 'config.sub' for the possible values of each field. If +'config.sub' isn't included in this package, then this package doesn't +need to know the machine type. + + If you are _building_ compiler tools for cross-compiling, you should +use the option '--target=TYPE' to select the type of system they will +produce code for. + + If you want to _use_ a cross compiler, that generates code for a +platform different from the build platform, you should specify the +"host" platform (i.e., that on which the generated programs will +eventually be run) with '--host=TYPE'. + +Sharing Defaults +================ + + If you want to set default values for 'configure' scripts to share, +you can create a site shell script called 'config.site' that gives +default values for variables like 'CC', 'cache_file', and 'prefix'. +'configure' looks for 'PREFIX/share/config.site' if it exists, then +'PREFIX/etc/config.site' if it exists. Or, you can set the +'CONFIG_SITE' environment variable to the location of the site script. +A warning: not all 'configure' scripts look for a site script. + +Defining Variables +================== + + Variables not defined in a site shell script can be set in the +environment passed to 'configure'. However, some packages may run +configure again during the build, and the customized values of these +variables may be lost. In order to avoid this problem, you should set +them in the 'configure' command line, using 'VAR=value'. For example: + + ./configure CC=/usr/local2/bin/gcc + +causes the specified 'gcc' to be used as the C compiler (unless it is +overridden in the site shell script). + +Unfortunately, this technique does not work for 'CONFIG_SHELL' due to an +Autoconf limitation. Until the limitation is lifted, you can use this +workaround: + + CONFIG_SHELL=/bin/bash ./configure CONFIG_SHELL=/bin/bash + +'configure' Invocation +====================== + + 'configure' recognizes the following options to control how it +operates. + +'--help' +'-h' + Print a summary of all of the options to 'configure', and exit. + +'--help=short' +'--help=recursive' + Print a summary of the options unique to this package's + 'configure', and exit. The 'short' variant lists options used only + in the top level, while the 'recursive' variant lists options also + present in any nested packages. + +'--version' +'-V' + Print the version of Autoconf used to generate the 'configure' + script, and exit. + +'--cache-file=FILE' + Enable the cache: use and save the results of the tests in FILE, + traditionally 'config.cache'. FILE defaults to '/dev/null' to + disable caching. + +'--config-cache' +'-C' + Alias for '--cache-file=config.cache'. + +'--quiet' +'--silent' +'-q' + Do not print messages saying which checks are being made. To + suppress all normal output, redirect it to '/dev/null' (any error + messages will still be shown). + +'--srcdir=DIR' + Look for the package's source code in directory DIR. Usually + 'configure' can determine that directory automatically. + +'--prefix=DIR' + Use DIR as the installation prefix. *note Installation Names:: for + more details, including other options available for fine-tuning the + installation locations. + +'--no-create' +'-n' + Run the configure checks, but stop before creating any output + files. + +'configure' also accepts some other, not widely useful, options. Run +'configure --help' for more details. diff --git a/Makefile.am b/Makefile.am new file mode 100644 index 0000000..0fceada --- /dev/null +++ b/Makefile.am @@ -0,0 +1,4 @@ +bin_PROGRAMS = nqptp +nqptp_SOURCES = main.c + +AM_CFLAGS = -fno-common -Wno-multichar -Wall -Wextra -Wno-clobbered -Wno-psabi -pthread diff --git a/NEWS b/NEWS new file mode 100644 index 0000000..e69de29 diff --git a/README b/README new file mode 100644 index 0000000..e69de29 diff --git a/configure.ac b/configure.ac new file mode 100644 index 0000000..e80c21e --- /dev/null +++ b/configure.ac @@ -0,0 +1,28 @@ +# -*- Autoconf -*- +# Process this file with autoconf to produce a configure script. + +AC_PREREQ([2.68]) +AC_INIT([nqptp], [1.0d0], [4265913+mikebrady@users.noreply.github.com]) +AM_INIT_AUTOMAKE +AC_CONFIG_SRCDIR([main.c]) +AC_CONFIG_HEADERS([config.h]) + +# Checks for programs. +AC_PROG_CC +AC_PROG_INSTALL + +# Checks for libraries. + +# Checks for header files. +AC_CHECK_HEADERS([arpa/inet.h inttypes.h netdb.h stdlib.h string.h sys/socket.h unistd.h]) + +# Checks for typedefs, structures, and compiler characteristics. +AC_TYPE_SIZE_T +AC_TYPE_UINT32_T + +# Checks for library functions. +AC_FUNC_MALLOC +AC_CHECK_FUNCS([clock_gettime inet_ntoa memset select socket strerror]) + +AC_CONFIG_FILES([Makefile]) +AC_OUTPUT \ No newline at end of file diff --git a/main.c b/main.c new file mode 100644 index 0000000..9d8899b --- /dev/null +++ b/main.c @@ -0,0 +1,394 @@ +/* + * This file is part of the nqPTP distribution (https://github.com/mikebrady/nqPTP). + * Copyright (c) 2021 Mike Brady. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, version 3. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Commercial licensing is also available. + */ + +#include +#include //printf +#include //exit(0); +#include //memset +#include +#include // close + +#include +#include + +#include +#include +#include + +#include +#include /* the L2 protocols */ + +#include + +// 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) + +// Table 19 +enum messageType { + Sync, + Delay_Req, + Pdelay_Req, + Pdelay_Resp, + Reserved_4, + Reserved_5, + Reserved_6, + Reserved_7, + Follow_Up, + Delay_Resp, + Pdelay_Resp_Follow_Up, + Announce, + Signaling, + Management, + Reserved_E, + Reserved_F +}; + +#define BUFLEN 4096 // Max length of buffer +#define PORT 320 // The port on which to listen for incoming data + +struct sockaddr_in si_me_319, si_me_320, si_other; + +uint64_t time_then = 0; + +void die(char *s) { + perror(s); + exit(1); +} + +uint32_t nctohl(const uint8_t *p) { // read 4 characters from *p and do ntohl on them + // this is to avoid possible aliasing violations + uint32_t holder; + memcpy(&holder, p, sizeof(holder)); + return ntohl(holder); +} + +uint16_t nctohs(const uint8_t *p) { // read 2 characters from *p and do ntohs on them + // this is to avoid possible aliasing violations + uint16_t holder; + memcpy(&holder, p, sizeof(holder)); + return ntohs(holder); +} + +uint64_t timespec_to_ns(struct timespec *tn) { + uint64_t tnfpsec = tn->tv_sec; + uint64_t tnfpnsec = tn->tv_nsec; + tnfpsec = tnfpsec * 1000000000; + return tnfpsec + tnfpnsec; +} + +uint64_t get_time_now() { + struct timespec tn; + clock_gettime(CLOCK_MONOTONIC, &tn); // this should be optionally CLOCK_MONOTONIC etc. + return timespec_to_ns(&tn); +} + +void print_buffer(char *buf, size_t buf_len) { + uint64_t time_now = get_time_now(); + if (time_then == 0) { + printf(" "); + } else { + printf("%f ", (time_now - time_then) * 0.000000001); + } + time_then = time_now; + // printf("Received %u bytes in a packet from %s:%d\n", buf_len, inet_ntoa(si_other.sin_addr), + // ntohs(si_other.sin_port)); + char obf[BUFLEN * 2 + BUFLEN / 4 + 1 + 1]; + char *obfp = obf; + unsigned int obfc; + for (obfc = 0; obfc < buf_len; obfc++) { + snprintf(obfp, 3, "%02X", buf[obfc]); + obfp += 2; + if (obfc != buf_len - 1) { + if (obfc % 32 == 31) { + snprintf(obfp, 5, " || "); + obfp += 4; + } else if (obfc % 16 == 15) { + snprintf(obfp, 4, " | "); + obfp += 3; + } else if (obfc % 4 == 3) { + snprintf(obfp, 2, " "); + obfp += 1; + } + } + }; + *obfp = 0; + switch (buf[0]) { + + case 0x10: + printf("SYNC: \"%s\".\n", obf); + break; + case 0x18: + printf("FLUP: \"%s\".\n", obf); + break; + case 0x19: + printf("DRSP: \"%s\".\n", obf); + break; + case 0x1B: + printf("ANNC: \"%s\".\n", obf); + break; + case 0x1C: + printf("SGNL: \"%s\".\n", obf); + break; + default: + printf(" \"%s\".\n", obf); + break; + } +} + +int main(void) { + int s319, s320; + unsigned int slen = sizeof(si_other); + ssize_t recv_len; + + char buf[BUFLEN]; + + int status; + + uint64_t previous_offset = 0; + + struct __attribute__((__packed__)) ptp_common_message_header { + uint8_t transportSpecificAndMessageID; // 0x11 + uint8_t reservedAndVersionPTP; // 0x02 + uint16_t messageLength; + uint8_t domainNumber; // 0 + uint8_t reserved_b; // 0 + uint16_t flags; // 0x0608 + uint8_t correctionNs[6]; // 0 + uint8_t correctionSubNs[2]; // 0 + uint32_t reserved_l; // 0 + uint8_t clockIdentity[8]; // MAC + uint16_t sourcePortID; // 1 + uint16_t sequenceId; // increments + uint8_t controlOtherMessage; // 5 + uint8_t logMessagePeriod; // 0 + }; + + // this is the extra part for a Sync or Delay_Req message + struct __attribute__((__packed__)) ptp_sync { + uint8_t originTimestamp[10]; + }; + + // this is the extra part for a Sync or Delay_Req message + struct __attribute__((__packed__)) ptp_delay_req { + uint8_t originTimestamp[10]; + }; + + // this is the extra part for a Follow_Up message + struct __attribute__((__packed__)) ptp_follow_up { + uint8_t preciseOriginTimestamp[10]; + }; + + // this is the extra part for a Delay_Resp message + struct __attribute__((__packed__)) ptp_delay_resp { + uint8_t receiveTimestamp[10]; + uint8_t requestingPortIdentity[10]; + }; + + struct __attribute__((__packed__)) ptp_sync_message { + struct ptp_common_message_header header; + struct ptp_sync sync; + }; + + struct __attribute__((__packed__)) ptp_delay_req_message { + struct ptp_common_message_header header; + struct ptp_delay_req delay_req; + } ; + + struct __attribute__((__packed__)) ptp_follow_up_message { + struct ptp_common_message_header header; + struct ptp_follow_up follow_up; + } ; + + struct __attribute__((__packed__)) ptp_delay_resp_message { + struct ptp_common_message_header header; + struct ptp_delay_resp delay_resp; + }; + + struct ptp_delay_req_message m; + + // create a UDP socket + if ((s319 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + die("socket319"); + } + + // zero out the structure + memset((char *)&si_me_319, 0, sizeof(si_me_319)); + + si_me_319.sin_family = AF_INET; + si_me_319.sin_port = htons(319); + si_me_319.sin_addr.s_addr = htonl(INADDR_ANY); + + // bind socket to port + if (bind(s319, (struct sockaddr *)&si_me_319, sizeof(si_me_319)) == -1) { + die("bind 319"); + } + + // create a UDP socket + if ((s320 = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) { + die("socket320"); + } + + // zero out the structure + memset((char *)&si_me_320, 0, sizeof(si_me_320)); + + si_me_320.sin_family = AF_INET; + si_me_320.sin_port = htons(320); + si_me_320.sin_addr.s_addr = htonl(INADDR_ANY); + + // bind socket to port + if (bind(s320, (struct sockaddr *)&si_me_320, sizeof(si_me_320)) == -1) { + die("bind 320"); + } + + fd_set readSockSet; + struct timeval timeout; + int smax = s319; + if (smax < s320) + smax = s320; + + while (1) { + FD_ZERO(&readSockSet); + FD_SET(s319, &readSockSet); + FD_SET(s320, &readSockSet); + // add connected TCP clients, if needed... + + timeout.tv_sec = 10; + timeout.tv_usec = 0; + int retval = select(smax + 1, &readSockSet, NULL, NULL, &timeout); + uint64_t t1, t2, t3, t4, t5; + uint64_t reception_time = get_time_now(); + + if (retval > 0) { + // note time of arrival + if (FD_ISSET(s319, &readSockSet)) { + // printf("S319 Client query incoming...\n"); + // try to receive some data, this is a blocking call + if ((recv_len = recvfrom(s319, buf, BUFLEN, 0, (struct sockaddr *)&si_other, &slen)) == + -1) { + die("recvfrom() 319"); + } else if (recv_len >= (ssize_t)sizeof(struct ptp_common_message_header)) { + // print_buffer(buf, recv_len); + switch (buf[0] & 0xF) { + case Sync: { // if it's a sync + struct ptp_sync_message *msg = (struct ptp_sync_message *)buf; + t2 = reception_time; + memset(&m, 0, sizeof(m)); + m.header.transportSpecificAndMessageID = 0x11; + m.header.reservedAndVersionPTP = 0x02; + m.header.messageLength = htons(44); + m.header.flags = htons(0x608); + m.header.sourcePortID = htons(1); + m.header.controlOtherMessage = 5; + m.header.sequenceId = msg->header.sequenceId; + struct ifaddrs *ifaddr = NULL; + struct ifaddrs *ifa = NULL; + + if ((status = getifaddrs(&ifaddr) == -1)) { + fprintf(stderr, "getifaddrs: %s\n", 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)) { + memcpy(&m.header.clockIdentity, &s->sll_addr, s->sll_halen); + found = 1; + } + } + } + freeifaddrs(ifaddr); + } + t3 = get_time_now(); + if (sendto(s319, &m, sizeof(m), 0, (const struct sockaddr *)&si_other, slen) == -1) { + fprintf(stderr, "sendto: %s\n", strerror(errno)); + return 4; + } + } break; + default: + break; + } + } + } + + if (FD_ISSET(s320, &readSockSet)) { + // printf("S320 Client query incoming...\n"); + // try to receive some data, this is a blocking call + if ((recv_len = recvfrom(s320, buf, BUFLEN, 0, (struct sockaddr *)&si_other, &slen)) == + -1) { + die("recvfrom() 320"); + + } else if (recv_len >= (ssize_t)sizeof(struct ptp_common_message_header)) { + //print_buffer(buf, recv_len); + switch (buf[0] & 0xF) { + case Follow_Up: { + struct ptp_follow_up_message *msg = (struct ptp_follow_up_message *)buf; + 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; + t1 = preciseOriginTimestamp; + } break; + case Delay_Resp: { + struct ptp_delay_resp_message *msg = (struct ptp_delay_resp_message *)buf; + uint16_t seconds_hi = nctohs(&msg->delay_resp.receiveTimestamp[0]); + uint32_t seconds_low = nctohl(&msg->delay_resp.receiveTimestamp[2]); + uint32_t nanoseconds = nctohl(&msg->delay_resp.receiveTimestamp[6]); + uint64_t receiveTimestamp = seconds_hi; + receiveTimestamp = receiveTimestamp << 32; + receiveTimestamp = receiveTimestamp + seconds_low; + receiveTimestamp = receiveTimestamp * 1000000000L; + receiveTimestamp = receiveTimestamp + nanoseconds; + t4 = receiveTimestamp; + t5 = reception_time; // t5 - t3 gives us the out-and-back time locally -- an instantaneous quality index + + // calculate delay and calculate offset + //fprintf(stderr, "t1: %016" PRIx64 ", t2: %" PRIx64 ", t3: %" PRIx64 ", t4: %" PRIx64 ".\n",t1,t2,t3,t4); + //fprintf(stderr, "nominal remote transaction time: %" PRIx64 " = %" PRIu64 "ns; local transaction time: %" PRIx64 " = %" PRId64 "ns.\n", t4-t1, t4-t1, t3-t2, t3-t2); + uint64_t offset = t1 - t2; + if (previous_offset == 0) + fprintf(stderr, "offset: %" PRIx64 ".\n", offset); + else { + int64_t variation = offset - previous_offset; + fprintf(stderr, "remote transaction time: %f, offset: %" PRIx64 ", variation: %f, turnaround: %f \n", (t4-t1) * 0.000000001, offset, variation * 0.000000001, (t5 - t2) * 0.000000001); + } + previous_offset = offset; + //fprintf(stderr, "Offset: %" PRIx64 ", delay %f.\n", offset, delay*0.000000001); + + } break; + + default: + // fprintf(stderr, "320 other\n"); + break; + } + } + } + + } else if (retval < 0) { + // check errno/WSAGetLastError(), call perror(), etc ... + } + } + close(s319); + close(s320); + return 0; +} \ No newline at end of file