dump-main.c 3.46 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20
#include <stdint.h>
#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <time.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/utsname.h>
#include <net/if.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/udp.h>
#include <linux/if_ether.h>
#include <net/if_arp.h>
#include <netpacket/packet.h>

21
#include <ppsi/ieee1588_types.h> /* from ../include */
22
#include "decent_types.h"
23
#include "ptpdump.h"
24 25 26 27 28

#ifndef ETH_P_1588
#define ETH_P_1588     0x88F7
#endif

29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47
void print_spaces(struct TimeInternal *ti)
{
	static struct TimeInternal prev_ti;
	int i, diffms;

	if (prev_ti.seconds) {

		diffms = (ti->seconds - prev_ti.seconds) * 1000
			+ (ti->nanoseconds / 1000 / 1000)
			- (prev_ti.nanoseconds / 1000 / 1000);
		/* empty lines, one every .25 seconds, at most 10 of them */
		for (i = 250; i < 2500 && i < diffms; i += 250)
			printf("\n");
		printf("TIMEDELTA: %i ms\n", diffms);
	}
	prev_ti = *ti;
}


48 49
int main(int argc, char **argv)
{
50
	int sock, ret;
51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86
	struct packet_mreq req;
	struct ifreq ifr;
	char *ifname = "eth0";

	sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
	if (sock < 0) {
		fprintf(stderr, "%s: socket(): %s\n", argv[0], strerror(errno));
		exit(1);
	}
	if (argc > 1)
		ifname = argv[1];

	memset(&ifr, 0, sizeof(ifr));
	strcpy(ifr.ifr_name, ifname);
	if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
		fprintf(stderr, "%s: ioctl(GETIFINDEX(%s)): %s\n", argv[0],
			ifname, strerror(errno));
		exit(1);
	}
	fprintf(stderr, "Dumping PTP packets from interface \"%s\"\n", ifname);

	memset(&req, 0, sizeof(req));
	req.mr_ifindex = ifr.ifr_ifindex;
	req.mr_type = PACKET_MR_PROMISC;

	if (setsockopt(3, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
		       &req, sizeof(req)) < 0) {
		fprintf(stderr, "%s: set promiscuous(%s): %s\n", argv[0],
			ifname, strerror(errno));
		exit(1);
	}

	/* Ok, now we are promiscuous. Just read stuff forever */
	while(1) {
		struct ethhdr *eth;
		struct iphdr *ip;
87 88 89
		static unsigned char prev[1500];
		static int prevlen;
		unsigned char buf[1500];
90 91
		struct TimeInternal ti;
		struct timeval tv;
92 93 94 95 96

		struct sockaddr_in from;
		socklen_t fromlen = sizeof(from);
		int len;

97
		len = recvfrom(sock, buf, sizeof(buf), MSG_TRUNC,
98
			       (struct sockaddr *) &from, &fromlen);
99 100 101 102 103 104

		/* Get the receive time, copy it to TimeInternal */
		gettimeofday(&tv, NULL);
		ti.seconds = tv.tv_sec;
		ti.nanoseconds = tv.tv_usec * 1000;

105 106 107 108 109 110
		if (len > sizeof(buf))
			len = sizeof(buf);
		/* for some reasons, we receive it three times, check dups */
		if (len == prevlen && !memcmp(buf, prev, len))
			continue;
		memcpy(prev, buf, len); prevlen = len;
111 112 113 114 115 116 117 118

		/* now only print ptp packets */
		if (len < ETH_HLEN)
			continue;
		eth = (void *)buf;
		ip = (void *)(buf + ETH_HLEN);
		switch(ntohs(eth->h_proto)) {
		case ETH_P_IP:
119 120 121 122 123 124 125 126 127
		{
			struct udphdr *udp = (void *)(ip + 1);
			int udpdest = ntohs(udp->dest);

			/*
			 * Filter before calling the dump function, otherwise
			 * we'll report TIMEDELAY for not-relevant frames
			 */
			if (len < ETH_HLEN + sizeof(*ip) + sizeof(*udp))
128 129 130
				continue;
			if (ip->protocol != IPPROTO_UDP)
				continue;
131 132
			if (udpdest != 319 && udpdest != 320)
				continue;
133
			print_spaces(&ti);
134
			ret = dump_udppkt("", buf, len, &ti);
135
			break;
136 137
		}

138
		case ETH_P_1588:
139
			print_spaces(&ti);
140
			ret = dump_1588pkt("", buf, len, &ti);
141 142
			break;
		default:
143
			ret = -1;
144 145
			continue;
		}
146 147
		if (ret == 0)
			putchar('\n');
148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165
		fflush(stdout);
	}

	return 0;
}