int main(int argc, char **argv)
{
int so_timestamping_flags = 0;
int so_timestamp = 0;
int so_timestampns = 0;
int siocgstamp = 0;
int siocgstampns = 0;
int ip_multicast_loop = 0;
char *interface;
int i;
int enabled = 1;
int sock;
struct ifreq device;
struct ifreq hwtstamp;
struct hwtstamp_config hwconfig, hwconfig_requested;
struct sockaddr_in addr;
struct ip_mreq imr;
struct in_addr iaddr;
int val;
socklen_t len;
struct timeval next;
if (argc < 2)
usage(0);
interface = argv[1];
for (i = 2; 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], "SOF_TIMESTAMPING_TX_HARDWARE"))
so_timestamping_flags |= SOF_TIMESTAMPING_TX_HARDWARE; //No message of desired type
else if (!strcasecmp(argv[i], "SOF_TIMESTAMPING_TX_SOFTWARE"))
so_timestamping_flags |= SOF_TIMESTAMPING_TX_SOFTWARE; //No message of desired type
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
usage(argv[i]);
}
sock = socket(PF_INET, SOCK_DGRAM/*SOCK_RAW*/, /*IPPROTO_UDP*/0);
if (sock < 0)
bail("socket");
memset(&device, 0, sizeof(device));
strncpy(device.ifr_name, interface, sizeof(device.ifr_name));
if (ioctl(sock, SIOCGIFADDR, &device) < 0)
bail("getting interface IP address");
memset(&hwtstamp, 0, sizeof(hwtstamp));
strncpy(hwtstamp.ifr_name, interface, sizeof(hwtstamp.ifr_name));
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) ?
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");
/* 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_flags,
sizeof(so_timestamping_flags)) < 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);
if (getsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING, &val, &len) < 0) {
printf("%s: %s\n", "getsockopt SO_TIMESTAMPING",
strerror(errno));
} else {
printf("SO_TIMESTAMPING %d\n", val);
if (val != so_timestamping_flags)
printf(" not the expected value %d\n",
so_timestamping_flags);
}
/* 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");
// printf("read from socket error queue...\n");
// recvpacket(sock, MSG_ERRQUEUE,
// siocgstamp,
// siocgstampns);
printf("read from socket queue...\n");
print_sock_buff_opt(sock);
recvpacket(sock, 0,
siocgstamp,
siocgstampns);
// print_sock_buff_opt(sock);
printf("read from socket error queue...\n");
print_sock_buff_opt(sock);
recvpacket(sock, MSG_ERRQUEUE,
siocgstamp,
siocgstampns);
// print_sock_buff_opt(sock);
}
} else {
/* write one packet */
printf("write to socket queue...\n");
// print_sock_buff_opt(sock);
sendpacket(sock,
(struct sockaddr *)&addr,
sizeof(addr));
sendpacket(sock,
(struct sockaddr *)&addr,
sizeof(addr));
// print_sock_buff_opt(sock);
next.tv_sec += 2;
continue;
}
}
return 0;
}
static void query_names(FILE *ofp, SOCKET sockfd)
{
short seq = 1000;
int npending = 0;
struct in_addr next_addr;
int have_next_addr = FALSE;
char errbuf[256];
assert( ofp != 0 );
assert( SOCKET_IS_VALID(sockfd) );
/*----------------------------------------------------------------
* Figure out our starting and ending addresses to be scanning.
* These are treated as simple long integers that are incremented
* on each loop, and we must have at least one loop to be valid.
*/
while ( have_next_addr
|| ((have_next_addr = next_target(&next_addr)) != 0)
|| (npending > 0) )
{
fd_set rfds, /* list of read descriptors */
wfds, /* list of write descriptors */
*pwfds = 0;
int n;
struct timeval tv;
/*--------------------------------------------------------
* Our select is just a bit tricky. We always are waiting
* on the read channel, but we only want to wait on the
* write channel if there are any more addresses in our
* list to process. After we've sent all the packets to
* the other end, we stop writing and do only reading.
*/
FD_ZERO(&rfds);
FD_SET(sockfd, &rfds);
timeval_set_secs(&tv, timeout_secs);
if ( have_next_addr )
{
wfds = rfds;
pwfds = &wfds;
}
if ( (n = select(sockfd+1, &rfds, pwfds, 0, &tv)) == 0 )
{
fprintf(stderr, "*timeout (normal end of scan)\n");
fflush(ofp);
break;
}
else if ( n < 0)
{
printf("ERROR [%s]\n", strerror(errno));
break;
}
/*--------------------------------------------------------
* Has the read descriptor fired?
*/
if ( n > 0 && FD_ISSET(sockfd, &rfds) )
{
int paklen;
struct sockaddr_in src;
struct NMBpacket pak;
struct NMB_query_response rsp;
memset(&src, 0, sizeof src);
memset(&rsp, 0, sizeof rsp);
paklen = (int)recvpacket(sockfd, &pak, sizeof pak,&src);
if ( verbose )
{
if ( paklen < 0 )
{
fprintf(ofp, "Error on read: %s\n",
strerror(errno));
}
else
{
fprintf(ofp, "Got %d bytes from %s\n",
paklen,
inet_ntoa(src.sin_addr) );
if ( verbose > 1 )
dump_nbtpacket(&pak, paklen, stdout);
}
}
/*------------------------------------------------
* If we actually got something from the other end,
* parse the response, plug in the remote's IP addr,
* and display it.
*/
if ( paklen <= 0 ) continue;
npending--;
if ( parse_nbtstat(&pak, paklen, &rsp, errbuf) )
{
rsp.remote = src;
if ( target_responded(&rsp.remote.sin_addr) )
{
#ifdef ENABLE_PERL
if ( gen_Perl )
generate_perl(ofp, &rsp);
else
#endif
display_nbtstat(ofp,&rsp,full_nbtstat);
}
}
else
{
fprintf(ofp, "ERROR: no parse for %s -- %s\n",
inet_ntoa(src.sin_addr),
errbuf);
}
}
/*--------------------------------------------------------
* If we have room to write one packet, do so here. Note
* that we make not notice whether the write succeeds or
* not: we don't care.
*/
if ( n > 0 && pwfds && FD_ISSET(sockfd, pwfds) )
{
struct sockaddr_in dst;
struct NMBpacket pak;
int sendlen;
memset(&dst, 0, sizeof dst);
dst.sin_family = AF_INET;
dst.sin_addr.s_addr = next_addr.s_addr;
dst.sin_port = htons(dest_portno);
have_next_addr = FALSE;
fill_namerequest(&pak, &sendlen, seq++);
if ( verbose )
{
fprintf(ofp, "sending to %s\n",
inet_ntoa(dst.sin_addr));
}
/* yes, ignore response! */
(void) sendpacket(sockfd, &pak, sendlen, &dst);
if ( write_sleep_msecs > 0 )
sleep_msecs(write_sleep_msecs);
npending++;
continue;
}
}
}
