diff --git a/port.c b/port.c index e5c079e..43105d1 100644 --- a/port.c +++ b/port.c @@ -366,6 +366,21 @@ static void free_foreign_masters(struct port *p) } } +static int fup_sync_ok(struct ptp_message *fup, struct ptp_message *sync) +{ + int64_t tfup, tsync; + tfup = tmv_to_nanoseconds(timespec_to_tmv(fup->hwts.sw)); + tsync = tmv_to_nanoseconds(timespec_to_tmv(sync->hwts.sw)); + /* + * NB - If the sk_check_fupsync option is not enabled, then + * both of these time stamps will be zero. + */ + if (tfup < tsync) { + return 0; + } + return 1; +} + static int incapable_ignore(struct port *p, struct ptp_message *m) { if (port_capable(p)) { @@ -1808,6 +1823,7 @@ static void process_sync(struct port *p, struct ptp_message *m) } if (p->syfu == SF_HAVE_FUP && + fup_sync_ok(p->last_syncfup, m) && p->last_syncfup->header.sequenceId == m->header.sequenceId) { event = SYNC_MATCH; } else { diff --git a/raw.c b/raw.c index 326ccd2..d87edbf 100644 --- a/raw.c +++ b/raw.c @@ -209,6 +209,9 @@ static int raw_open(struct transport *t, char *name, if (sk_timestamping_init(efd, name, ts_type, TRANS_IEEE_802_3)) goto no_timestamping; + if (sk_general_init(gfd)) + goto no_timestamping; + fda->fd[FD_EVENT] = efd; fda->fd[FD_GENERAL] = gfd; return 0; diff --git a/sk.c b/sk.c index a13d14d..fe74164 100644 --- a/sk.c +++ b/sk.c @@ -36,6 +36,7 @@ /* globals */ int sk_tx_timeout = 1; +int sk_check_fupsync; /* private methods */ @@ -91,6 +92,16 @@ int sk_interface_index(int fd, char *name) return ifreq.ifr_ifindex; } +int sk_general_init(int fd) +{ + int on = sk_check_fupsync ? 1 : 0; + if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPNS, &on, sizeof(on)) < 0) { + pr_err("ioctl SO_TIMESTAMPNS failed: %m"); + return -1; + } + return 0; +} + int sk_get_ts_info(char *name, struct sk_ts_info *sk_info) { #ifdef ETHTOOL_GET_TS_INFO @@ -205,7 +216,7 @@ int sk_receive(int fd, void *buf, int buflen, struct cmsghdr *cm; struct iovec iov = { buf, buflen }; struct msghdr msg; - struct timespec *ts = NULL; + struct timespec *sw, *ts = NULL; memset(control, 0, sizeof(control)); memset(&msg, 0, sizeof(msg)); @@ -241,7 +252,14 @@ int sk_receive(int fd, void *buf, int buflen, return -1; } ts = (struct timespec *) CMSG_DATA(cm); - break; + } + if (SOL_SOCKET == level && SO_TIMESTAMPNS == type) { + if (cm->cmsg_len < sizeof(*sw)) { + pr_warning("short SO_TIMESTAMPNS message"); + return -1; + } + sw = (struct timespec *) CMSG_DATA(cm); + hwts->sw = *sw; } } @@ -325,5 +343,10 @@ int sk_timestamping_init(int fd, char *device, enum timestamp_type type, return -1; } + /* Enable the sk_check_fupsync option, perhaps. */ + if (sk_general_init(fd)) { + return -1; + } + return 0; } diff --git a/sk.h b/sk.h index 6070537..895840f 100644 --- a/sk.h +++ b/sk.h @@ -46,6 +46,13 @@ struct sk_ts_info { */ int sk_interface_index(int fd, char *device); +/** + * Prepare a given socket for PTP "general" messages. + * @param fd An open socket. + * @return Zero on success, non-zero otherwise. + */ +int sk_general_init(int fd); + /** * Obtain supported timestamping information * @param name The name of the interface @@ -102,4 +109,11 @@ int sk_timestamping_init(int fd, char *device, enum timestamp_type type, */ extern int sk_tx_timeout; +/** + * Enables the SO_TIMESTAMPNS socket option on the both the event and + * general sockets in order to test the order of paired sync and + * follow up messages using their network stack receipt time stamps. + */ +extern int sk_check_fupsync; + #endif diff --git a/transport.h b/transport.h index 46af456..34934fe 100644 --- a/transport.h +++ b/transport.h @@ -57,6 +57,7 @@ enum timestamp_type { struct hw_timestamp { enum timestamp_type type; struct timespec ts; + struct timespec sw; }; struct transport; diff --git a/udp.c b/udp.c index fea0122..be7f2b7 100644 --- a/udp.c +++ b/udp.c @@ -179,6 +179,9 @@ static int udp_open(struct transport *t, char *name, struct fdarray *fda, if (sk_timestamping_init(efd, name, ts_type, TRANS_UDP_IPV4)) goto no_timestamping; + if (sk_general_init(gfd)) + goto no_timestamping; + fda->fd[FD_EVENT] = efd; fda->fd[FD_GENERAL] = gfd; return 0; diff --git a/udp6.c b/udp6.c index 2cb0179..e0d1256 100644 --- a/udp6.c +++ b/udp6.c @@ -191,6 +191,9 @@ static int udp6_open(struct transport *t, char *name, struct fdarray *fda, if (sk_timestamping_init(efd, name, ts_type, TRANS_UDP_IPV6)) goto no_timestamping; + if (sk_general_init(gfd)) + goto no_timestamping; + fda->fd[FD_EVENT] = efd; fda->fd[FD_GENERAL] = gfd; return 0;