552 lines
16 KiB
C
552 lines
16 KiB
C
|
// SPDX-License-Identifier: GPL-2.0-only
|
||
|
|
/*
|
||
|
|
* This program demonstrates how the various time stamping features in
|
||
|
|
* the Linux kernel work. It emulates the behavior of a PTP
|
||
|
|
* implementation in stand-alone master mode by sending PTPv1 Sync
|
||
|
|
* multicasts once every second. It looks for similar packets, but
|
||
|
|
* beyond that doesn't actually implement PTP.
|
||
|
|
*
|
||
|
|
* Outgoing packets are time stamped with SO_TIMESTAMPING with or
|
||
|
|
* without hardware support.
|
||
|
|
*
|
||
|
|
* Incoming packets are time stamped with SO_TIMESTAMPING with or
|
||
|
|
* without hardware support, SIOCGSTAMP[NS] (per-socket time stamp) and
|
||
|
|
* SO_TIMESTAMP[NS].
|
||
|
|
*
|
||
|
|
* Copyright (C) 2009 Intel Corporation.
|
||
|
|
* Author: Patrick Ohly <patrick.ohly@intel.com>
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <errno.h>
|
||
|
|
#include <string.h>
|
||
|
|
|
||
|
|
#include <sys/time.h>
|
||
|
|
#include <sys/socket.h>
|
||
|
|
#include <sys/select.h>
|
||
|
|
#include <sys/ioctl.h>
|
||
|
|
#include <arpa/inet.h>
|
||
|
|
#include <net/if.h>
|
||
|
|
|
||
|
|
#include <asm/types.h>
|
||
|
|
#include <linux/net_tstamp.h>
|
||
|
|
#include <linux/errqueue.h>
|
||
|
|
#include <linux/sockios.h>
|
||
|
|
|
||
|
|
#ifndef SO_TIMESTAMPING
|
||
|
|
# define SO_TIMESTAMPING 37
|
||
|
|
# define SCM_TIMESTAMPING SO_TIMESTAMPING
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifndef SO_TIMESTAMPNS
|
||
|
|
# define SO_TIMESTAMPNS 35
|
||
|
|
#endif
|
||
|
|
|
||
|
|
static void usage(const char *error)
|
||
|
|
{
|
||
|
|
if (error)
|
||
|
|
printf("invalid option: %s\n", error);
|
||
|
|
printf("timestamping <interface> [bind_phc_index] [option]*\n\n"
|
||
|
|
"Options:\n"
|
||
|
|
" IP_MULTICAST_LOOP - looping outgoing multicasts\n"
|
||
|
|
" SO_TIMESTAMP - normal software time stamping, ms resolution\n"
|
||
|
|
" SO_TIMESTAMPNS - more accurate software time stamping\n"
|
||
|
|
" SOF_TIMESTAMPING_TX_HARDWARE - hardware time stamping of outgoing packets\n"
|
||
|
|
" SOF_TIMESTAMPING_TX_SOFTWARE - software fallback for outgoing packets\n"
|
||
|
|
" SOF_TIMESTAMPING_RX_HARDWARE - hardware time stamping of incoming packets\n"
|
||
|
|
" SOF_TIMESTAMPING_RX_SOFTWARE - software fallback for incoming packets\n"
|
||
|
|
" SOF_TIMESTAMPING_SOFTWARE - request reporting of software time stamps\n"
|
||
|
|
" SOF_TIMESTAMPING_RAW_HARDWARE - request reporting of raw HW time stamps\n"
|
||
|
|
" SOF_TIMESTAMPING_BIND_PHC - request to bind a PHC of PTP vclock\n"
|
||
|
|
" SIOCGSTAMP - check last socket time stamp\n"
|
||
|
|
" SIOCGSTAMPNS - more accurate socket time stamp\n"
|
||
|
|
" PTPV2 - use PTPv2 messages\n");
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void bail(const char *error)
|
||
|
|
{
|
||
|
|
printf("%s: %s\n", error, strerror(errno));
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
static const unsigned char sync[] = {
|
||
|
|
0x00, 0x01, 0x00, 0x01,
|
||
|
|
0x5f, 0x44, 0x46, 0x4c,
|
||
|
|
0x54, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x01, 0x01,
|
||
|
|
|
||
|
|
/* fake uuid */
|
||
|
|
0x00, 0x01,
|
||
|
|
0x02, 0x03, 0x04, 0x05,
|
||
|
|
|
||
|
|
0x00, 0x01, 0x00, 0x37,
|
||
|
|
0x00, 0x00, 0x00, 0x08,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x49, 0x05, 0xcd, 0x01,
|
||
|
|
0x29, 0xb1, 0x8d, 0xb0,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x01,
|
||
|
|
|
||
|
|
/* fake uuid */
|
||
|
|
0x00, 0x01,
|
||
|
|
0x02, 0x03, 0x04, 0x05,
|
||
|
|
|
||
|
|
0x00, 0x00, 0x00, 0x37,
|
||
|
|
0x00, 0x00, 0x00, 0x04,
|
||
|
|
0x44, 0x46, 0x4c, 0x54,
|
||
|
|
0x00, 0x00, 0xf0, 0x60,
|
||
|
|
0x00, 0x01, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x01,
|
||
|
|
0x00, 0x00, 0xf0, 0x60,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x04,
|
||
|
|
0x44, 0x46, 0x4c, 0x54,
|
||
|
|
0x00, 0x01,
|
||
|
|
|
||
|
|
/* fake uuid */
|
||
|
|
0x00, 0x01,
|
||
|
|
0x02, 0x03, 0x04, 0x05,
|
||
|
|
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00
|
||
|
|
};
|
||
|
|
|
||
|
|
static const unsigned char sync_v2[] = {
|
||
|
|
0x00, 0x02, 0x00, 0x2C,
|
||
|
|
0x00, 0x00, 0x02, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0xFF,
|
||
|
|
0xFE, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x01, 0x00, 0x01,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
0x00, 0x00, 0x00, 0x00,
|
||
|
|
};
|
||
|
|
|
||
|
|
static void sendpacket(int sock, struct sockaddr *addr, socklen_t addr_len, int ptpv2)
|
||
|
|
{
|
||
|
|
size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
|
||
|
|
const void *sync_p = ptpv2 ? sync_v2 : sync;
|
||
|
|
struct timeval now;
|
||
|
|
int res;
|
||
|
|
|
||
|
|
res = sendto(sock, sync_p, sync_len, 0, addr, addr_len);
|
||
|
|
gettimeofday(&now, 0);
|
||
|
|
if (res < 0)
|
||
|
|
printf("%s: %s\n", "send", strerror(errno));
|
||
|
|
else
|
||
|
|
printf("%ld.%06ld: sent %d bytes\n",
|
||
|
|
(long)now.tv_sec, (long)now.tv_usec,
|
||
|
|
res);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void printpacket(struct msghdr *msg, int res,
|
||
|
|
char *data,
|
||
|
|
int sock, int recvmsg_flags,
|
||
|
|
int siocgstamp, int siocgstampns, int ptpv2)
|
||
|
|
{
|
||
|
|
struct sockaddr_in *from_addr = (struct sockaddr_in *)msg->msg_name;
|
||
|
|
size_t sync_len = ptpv2 ? sizeof(sync_v2) : sizeof(sync);
|
||
|
|
const void *sync_p = ptpv2 ? sync_v2 : sync;
|
||
|
|
struct cmsghdr *cmsg;
|
||
|
|
struct timeval tv;
|
||
|
|
struct timespec ts;
|
||
|
|
struct timeval now;
|
||
|
|
|
||
|
|
gettimeofday(&now, 0);
|
||
|
|
|
||
|
|
printf("%ld.%06ld: received %s data, %d bytes from %s, %zu bytes control messages\n",
|
||
|
|
(long)now.tv_sec, (long)now.tv_usec,
|
||
|
|
(recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
|
||
|
|
res,
|
||
|
|
inet_ntoa(from_addr->sin_addr),
|
||
|
|
msg->msg_controllen);
|
||
|
|
for (cmsg = CMSG_FIRSTHDR(msg);
|
||
|
|
cmsg;
|
||
|
|
cmsg = CMSG_NXTHDR(msg, cmsg)) {
|
||
|
|
printf(" cmsg len %zu: ", cmsg->cmsg_len);
|
||
|
|
switch (cmsg->cmsg_level) {
|
||
|
|
case SOL_SOCKET:
|
||
|
|
printf("SOL_SOCKET ");
|
||
|
|
switch (cmsg->cmsg_type) {
|
||
|
|
case SO_TIMESTAMP: {
|
||
|
|
struct timeval *stamp =
|
||
|
|
(struct timeval *)CMSG_DATA(cmsg);
|
||
|
|
printf("SO_TIMESTAMP %ld.%06ld",
|
||
|
|
(long)stamp->tv_sec,
|
||
|
|
(long)stamp->tv_usec);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case SO_TIMESTAMPNS: {
|
||
|
|
struct timespec *stamp =
|
||
|
|
(struct timespec *)CMSG_DATA(cmsg);
|
||
|
|
printf("SO_TIMESTAMPNS %ld.%09ld",
|
||
|
|
(long)stamp->tv_sec,
|
||
|
|
(long)stamp->tv_nsec);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case SO_TIMESTAMPING: {
|
||
|
|
struct timespec *stamp =
|
||
|
|
(struct timespec *)CMSG_DATA(cmsg);
|
||
|
|
printf("SO_TIMESTAMPING ");
|
||
|
|
printf("SW %ld.%09ld ",
|
||
|
|
(long)stamp->tv_sec,
|
||
|
|
(long)stamp->tv_nsec);
|
||
|
|
stamp++;
|
||
|
|
/* skip deprecated HW transformed */
|
||
|
|
stamp++;
|
||
|
|
printf("HW raw %ld.%09ld",
|
||
|
|
(long)stamp->tv_sec,
|
||
|
|
(long)stamp->tv_nsec);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
printf("type %d", cmsg->cmsg_type);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
case IPPROTO_IP:
|
||
|
|
printf("IPPROTO_IP ");
|
||
|
|
switch (cmsg->cmsg_type) {
|
||
|
|
case IP_RECVERR: {
|
||
|
|
struct sock_extended_err *err =
|
||
|
|
(struct sock_extended_err *)CMSG_DATA(cmsg);
|
||
|
|
printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
|
||
|
|
strerror(err->ee_errno),
|
||
|
|
err->ee_origin,
|
||
|
|
#ifdef SO_EE_ORIGIN_TIMESTAMPING
|
||
|
|
err->ee_origin == SO_EE_ORIGIN_TIMESTAMPING ?
|
||
|
|
"bounced packet" : "unexpected origin"
|
||
|
|
#else
|
||
|
|
"probably SO_EE_ORIGIN_TIMESTAMPING"
|
||
|
|
#endif
|
||
|
|
);
|
||
|
|
if (res < sync_len)
|
||
|
|
printf(" => truncated data?!");
|
||
|
|
else if (!memcmp(sync_p, data + res - sync_len, sync_len))
|
||
|
|
printf(" => GOT OUR DATA BACK (HURRAY!)");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
case IP_PKTINFO: {
|
||
|
|
struct in_pktinfo *pktinfo =
|
||
|
|
(struct in_pktinfo *)CMSG_DATA(cmsg);
|
||
|
|
printf("IP_PKTINFO interface index %u",
|
||
|
|
pktinfo->ipi_ifindex);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
default:
|
||
|
|
printf("type %d", cmsg->cmsg_type);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
break;
|
||
|
|
default:
|
||
|
|
printf("level %d type %d",
|
||
|
|
cmsg->cmsg_level,
|
||
|
|
cmsg->cmsg_type);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
printf("\n");
|
||
|
|
}
|
||
|
|
|
||
|
|
if (siocgstamp) {
|
||
|
|
if (ioctl(sock, SIOCGSTAMP, &tv))
|
||
|
|
printf(" %s: %s\n", "SIOCGSTAMP", strerror(errno));
|
||
|
|
else
|
||
|
|
printf("SIOCGSTAMP %ld.%06ld\n",
|
||
|
|
(long)tv.tv_sec,
|
||
|
|
(long)tv.tv_usec);
|
||
|
|
}
|
||
|
|
if (siocgstampns) {
|
||
|
|
if (ioctl(sock, SIOCGSTAMPNS, &ts))
|
||
|
|
printf(" %s: %s\n", "SIOCGSTAMPNS", strerror(errno));
|
||
|
|
else
|
||
|
|
printf("SIOCGSTAMPNS %ld.%09ld\n",
|
||
|
|
(long)ts.tv_sec,
|
||
|
|
(long)ts.tv_nsec);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void recvpacket(int sock, int recvmsg_flags,
|
||
|
|
int siocgstamp, int siocgstampns, int ptpv2)
|
||
|
|
{
|
||
|
|
char data[256];
|
||
|
|
struct msghdr msg;
|
||
|
|
struct iovec entry;
|
||
|
|
struct sockaddr_in from_addr;
|
||
|
|
struct {
|
||
|
|
struct cmsghdr cm;
|
||
|
|
char control[512];
|
||
|
|
} control;
|
||
|
|
int res;
|
||
|
|
|
||
|
|
memset(&msg, 0, sizeof(msg));
|
||
|
|
msg.msg_iov = &entry;
|
||
|
|
msg.msg_iovlen = 1;
|
||
|
|
entry.iov_base = data;
|
||
|
|
entry.iov_len = sizeof(data);
|
||
|
|
msg.msg_name = (caddr_t)&from_addr;
|
||
|
|
msg.msg_namelen = sizeof(from_addr);
|
||
|
|
msg.msg_control = &control;
|
||
|
|
msg.msg_controllen = sizeof(control);
|
||
|
|
|
||
|
|
res = recvmsg(sock, &msg, recvmsg_flags|MSG_DONTWAIT);
|
||
|
|
if (res < 0) {
|
||
|
|
printf("%s %s: %s\n",
|
||
|
|
"recvmsg",
|
||
|
|
(recvmsg_flags & MSG_ERRQUEUE) ? "error" : "regular",
|
||
|
|
strerror(errno));
|
||
|
|
} else {
|
||
|
|
printpacket(&msg, res, data,
|
||
|
|
sock, recvmsg_flags,
|
||
|
|
siocgstamp, siocgstampns, ptpv2);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
int main(int argc, char **argv)
|
||
|
|
{
|
||
|
|
int so_timestamp = 0;
|
||
|
|
int so_timestampns = 0;
|
||
|
|
int siocgstamp = 0;
|
||
|
|
int siocgstampns = 0;
|
||
|
|
int ip_multicast_loop = 0;
|
||
|
|
int ptpv2 = 0;
|
||
|
|
char *interface;
|
||
|
|
int i;
|
||
|
|
int enabled = 1;
|
||
|
|
int sock;
|
||
|
|
struct ifreq device;
|
||
|
|
struct ifreq hwtstamp;
|
||
|
|
struct hwtstamp_config hwconfig, hwconfig_requested;
|
||
|
|
struct so_timestamping so_timestamping_get = { 0, 0 };
|
||
|
|
struct so_timestamping so_timestamping = { 0, 0 };
|
||
|
|
struct sockaddr_in addr;
|
||
|
|
struct ip_mreq imr;
|
||
|
|
struct in_addr iaddr;
|
||
|
|
int val;
|
||
|
|
socklen_t len;
|
||
|
|
struct timeval next;
|
||
|
|
size_t if_len;
|
||
|
|
|
||
|
|
if (argc < 2)
|
||
|
|
usage(0);
|
||
|
|
interface = argv[1];
|
||
|
|
if_len = strlen(interface);
|
||
|
|
if (if_len >= IFNAMSIZ) {
|
||
|
|
printf("interface name exceeds IFNAMSIZ\n");
|
||
|
|
exit(1);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (argc >= 3 && sscanf(argv[2], "%d", &so_timestamping.bind_phc) == 1)
|
||
|
|
val = 3;
|
||
|
|
else
|
||
|
|
val = 2;
|
||
|
|
|
||
|
|
for (i = val; i < argc; i++) {
|
||
|
|
if (!strcasecmp(argv[i], "SO_TIMESTAMP"))
|
||
|
|
so_timestamp = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "SO_TIMESTAMPNS"))
|
||
|
|
so_timestampns = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "SIOCGSTAMP"))
|
||
|
|
siocgstamp = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "SIOCGSTAMPNS"))
|
||
|
|
siocgstampns = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "IP_MULTICAST_LOOP"))
|
||
|
|
ip_multicast_loop = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "PTPV2"))
|
||
|
|
ptpv2 = 1;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_HARDWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_TX_HARDWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_TX_SOFTWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_HARDWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_RX_HARDWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RX_SOFTWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_RX_SOFTWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_SOFTWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_SOFTWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_RAW_HARDWARE"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_RAW_HARDWARE;
|
||
|
|
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_BIND_PHC"))
|
||
|
|
so_timestamping.flags |= SOF_TIMESTAMPING_BIND_PHC;
|
||
|
|
else
|
||
|
|
usage(argv[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||
|
|
if (sock < 0)
|
||
|
|
bail("socket");
|
||
|
|
|
||
|
|
memset(&device, 0, sizeof(device));
|
||
|
|
memcpy(device.ifr_name, interface, if_len + 1);
|
||
|
|
if (ioctl(sock, SIOCGIFADDR, &device) < 0)
|
||
|
|
bail("getting interface IP address");
|
||
|
|
|
||
|
|
memset(&hwtstamp, 0, sizeof(hwtstamp));
|
||
|
|
memcpy(hwtstamp.ifr_name, interface, if_len + 1);
|
||
|
|
hwtstamp.ifr_data = (void *)&hwconfig;
|
||
|
|
memset(&hwconfig, 0, sizeof(hwconfig));
|
||
|
|
hwconfig.tx_type =
|
||
|
|
(so_timestamping.flags & SOF_TIMESTAMPING_TX_HARDWARE) ?
|
||
|
|
HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
|
||
|
|
hwconfig.rx_filter =
|
||
|
|
(so_timestamping.flags & SOF_TIMESTAMPING_RX_HARDWARE) ?
|
||
|
|
ptpv2 ? HWTSTAMP_FILTER_PTP_V2_L4_SYNC :
|
||
|
|
HWTSTAMP_FILTER_PTP_V1_L4_SYNC : HWTSTAMP_FILTER_NONE;
|
||
|
|
hwconfig_requested = hwconfig;
|
||
|
|
if (ioctl(sock, SIOCSHWTSTAMP, &hwtstamp) < 0) {
|
||
|
|
if ((errno == EINVAL || errno == ENOTSUP) &&
|
||
|
|
hwconfig_requested.tx_type == HWTSTAMP_TX_OFF &&
|
||
|
|
hwconfig_requested.rx_filter == HWTSTAMP_FILTER_NONE)
|
||
|
|
printf("SIOCSHWTSTAMP: disabling hardware time stamping not possible\n");
|
||
|
|
else
|
||
|
|
bail("SIOCSHWTSTAMP");
|
||
|
|
}
|
||
|
|
printf("SIOCSHWTSTAMP: tx_type %d requested, got %d; rx_filter %d requested, got %d\n",
|
||
|
|
hwconfig_requested.tx_type, hwconfig.tx_type,
|
||
|
|
hwconfig_requested.rx_filter, hwconfig.rx_filter);
|
||
|
|
|
||
|
|
/* bind to PTP port */
|
||
|
|
addr.sin_family = AF_INET;
|
||
|
|
addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||
|
|
addr.sin_port = htons(319 /* PTP event port */);
|
||
|
|
if (bind(sock,
|
||
|
|
(struct sockaddr *)&addr,
|
||
|
|
sizeof(struct sockaddr_in)) < 0)
|
||
|
|
bail("bind");
|
||
|
|
|
||
|
|
if (setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, interface, if_len))
|
||
|
|
bail("bind device");
|
||
|
|
|
||
|
|
/* set multicast group for outgoing packets */
|
||
|
|
inet_aton("224.0.1.130", &iaddr); /* alternate PTP domain 1 */
|
||
|
|
addr.sin_addr = iaddr;
|
||
|
|
imr.imr_multiaddr.s_addr = iaddr.s_addr;
|
||
|
|
imr.imr_interface.s_addr =
|
||
|
|
((struct sockaddr_in *)&device.ifr_addr)->sin_addr.s_addr;
|
||
|
|
if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_IF,
|
||
|
|
&imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0)
|
||
|
|
bail("set multicast");
|
||
|
|
|
||
|
|
/* join multicast group, loop our own packet */
|
||
|
|
if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP,
|
||
|
|
&imr, sizeof(struct ip_mreq)) < 0)
|
||
|
|
bail("join multicast group");
|
||
|
|
|
||
|
|
if (setsockopt(sock, IPPROTO_IP, IP_MULTICAST_LOOP,
|
||
|
|
&ip_multicast_loop, sizeof(enabled)) < 0) {
|
||
|
|
bail("loop multicast");
|
||
|
|
}
|
||
|
|
|
||
|
|
/* set socket options for time stamping */
|
||
|
|
if (so_timestamp &&
|
||
|
|
setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
|
||
|
|
&enabled, sizeof(enabled)) < 0)
|
||
|
|
bail("setsockopt SO_TIMESTAMP");
|
||
|
|
|
||
|
|
if (so_timestampns &&
|
||
|
|
setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS,
|
||
|
|
&enabled, sizeof(enabled)) < 0)
|
||
|
|
bail("setsockopt SO_TIMESTAMPNS");
|
||
|
|
|
||
|
|
if (so_timestamping.flags &&
|
||
|
|
setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping,
|
||
|
|
sizeof(so_timestamping)) < 0)
|
||
|
|
bail("setsockopt SO_TIMESTAMPING");
|
||
|
|
|
||
|
|
/* request IP_PKTINFO for debugging purposes */
|
||
|
|
if (setsockopt(sock, SOL_IP, IP_PKTINFO,
|
||
|
|
&enabled, sizeof(enabled)) < 0)
|
||
|
|
printf("%s: %s\n", "setsockopt IP_PKTINFO", strerror(errno));
|
||
|
|
|
||
|
|
/* verify socket options */
|
||
|
|
len = sizeof(val);
|
||
|
|
if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMP, &val, &len) < 0)
|
||
|
|
printf("%s: %s\n", "getsockopt SO_TIMESTAMP", strerror(errno));
|
||
|
|
else
|
||
|
|
printf("SO_TIMESTAMP %d\n", val);
|
||
|
|
|
||
|
|
if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPNS, &val, &len) < 0)
|
||
|
|
printf("%s: %s\n", "getsockopt SO_TIMESTAMPNS",
|
||
|
|
strerror(errno));
|
||
|
|
else
|
||
|
|
printf("SO_TIMESTAMPNS %d\n", val);
|
||
|
|
|
||
|
|
len = sizeof(so_timestamping_get);
|
||
|
|
if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_get,
|
||
|
|
&len) < 0) {
|
||
|
|
printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
|
||
|
|
strerror(errno));
|
||
|
|
} else {
|
||
|
|
printf("SO_TIMESTAMPING flags %d, bind phc %d\n",
|
||
|
|
so_timestamping_get.flags, so_timestamping_get.bind_phc);
|
||
|
|
if (so_timestamping_get.flags != so_timestamping.flags ||
|
||
|
|
so_timestamping_get.bind_phc != so_timestamping.bind_phc)
|
||
|
|
printf(" not expected, flags %d, bind phc %d\n",
|
||
|
|
so_timestamping.flags, so_timestamping.bind_phc);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* send packets forever every five seconds */
|
||
|
|
gettimeofday(&next, 0);
|
||
|
|
next.tv_sec = (next.tv_sec + 1) / 5 * 5;
|
||
|
|
next.tv_usec = 0;
|
||
|
|
while (1) {
|
||
|
|
struct timeval now;
|
||
|
|
struct timeval delta;
|
||
|
|
long delta_us;
|
||
|
|
int res;
|
||
|
|
fd_set readfs, errorfs;
|
||
|
|
|
||
|
|
gettimeofday(&now, 0);
|
||
|
|
delta_us = (long)(next.tv_sec - now.tv_sec) * 1000000 +
|
||
|
|
(long)(next.tv_usec - now.tv_usec);
|
||
|
|
if (delta_us > 0) {
|
||
|
|
/* continue waiting for timeout or data */
|
||
|
|
delta.tv_sec = delta_us / 1000000;
|
||
|
|
delta.tv_usec = delta_us % 1000000;
|
||
|
|
|
||
|
|
FD_ZERO(&readfs);
|
||
|
|
FD_ZERO(&errorfs);
|
||
|
|
FD_SET(sock, &readfs);
|
||
|
|
FD_SET(sock, &errorfs);
|
||
|
|
printf("%ld.%06ld: select %ldus\n",
|
||
|
|
(long)now.tv_sec, (long)now.tv_usec,
|
||
|
|
delta_us);
|
||
|
|
res = select(sock + 1, &readfs, 0, &errorfs, &delta);
|
||
|
|
gettimeofday(&now, 0);
|
||
|
|
printf("%ld.%06ld: select returned: %d, %s\n",
|
||
|
|
(long)now.tv_sec, (long)now.tv_usec,
|
||
|
|
res,
|
||
|
|
res < 0 ? strerror(errno) : "success");
|
||
|
|
if (res > 0) {
|
||
|
|
if (FD_ISSET(sock, &readfs))
|
||
|
|
printf("ready for reading\n");
|
||
|
|
if (FD_ISSET(sock, &errorfs))
|
||
|
|
printf("has error\n");
|
||
|
|
recvpacket(sock, 0,
|
||
|
|
siocgstamp,
|
||
|
|
siocgstampns, ptpv2);
|
||
|
|
recvpacket(sock, MSG_ERRQUEUE,
|
||
|
|
siocgstamp,
|
||
|
|
siocgstampns, ptpv2);
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
/* write one packet */
|
||
|
|
sendpacket(sock,
|
||
|
|
(struct sockaddr *)&addr,
|
||
|
|
sizeof(addr), ptpv2);
|
||
|
|
next.tv_sec += 5;
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|