icmptime.c

#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <errno.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <arpa/inet.h>

#include <netinet/ip_icmp.h>
#include <sys/time.h>
#include <unistd.h>

#define MAXIPLEN 60
#define MAXICMPLEN 76

struct addrinfo *addrinfo;
int sock_icmp;

int datalen = 12;
uint8_t outpack[65536 - 60 - 8];

struct timeval tvorig;
long tsorig, tsrecv;
long tsdiff;


uint16_t calc_cksum(uint16_t *data, int len);
void sender(void);
int procpack(uint8_t *data, int len, struct sockaddr_in *from);

int main(int argc, char *argv[])
{
	struct addrinfo hints;
	int res;
	struct sockaddr_in *addr;
	char ip[INET_ADDRSTRLEN];

	if (argc != 2) {
		fprintf(stderr, "Usage: %s destination\n", argv[0]);
		exit(EXIT_FAILURE);
	}

	memset(&hints, 0, sizeof hints);
	hints.ai_flags		= AI_CANONNAME;
	hints.ai_family 	= AF_INET;
	hints.ai_socktype	= SOCK_RAW;
	hints.ai_protocol	= IPPROTO_ICMP;
	if ((res = getaddrinfo(argv[1], NULL, &hints, &addrinfo)) != 0) {
		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(res));
		exit(EXIT_FAILURE);
	}

	addr = (struct sockaddr_in *)addrinfo->ai_addr;
	inet_ntop(addrinfo->ai_family, &addr->sin_addr, ip, INET_ADDRSTRLEN);
	printf(
"ai_flags		= %d\n"
"ai_family		= %d\n"
"ai_socktype	= %d\n"
"ai_protocol	= %d\n"
"ai_addrlen		= %d\n"
"ai_addr.sin_family = %d, sin_port = %d, sin_addr.s_addr = %s\n"
"ai_canonname = %s\n",
addrinfo->ai_flags,
addrinfo->ai_family,
addrinfo->ai_socktype,
addrinfo->ai_protocol,
addrinfo->ai_addrlen,
addr->sin_family, addr->sin_port, ip,
addrinfo->ai_canonname);

	if ((sock_icmp = socket(addrinfo->ai_family, addrinfo->ai_socktype, addrinfo->ai_protocol)) < 0) {
		perror("socket");
		exit(EXIT_FAILURE);
	}

	sender();

	int packlen = datalen + MAXICMPLEN + MAXIPLEN;
	uint8_t *packet = (uint8_t *)malloc(packlen);
	if (!packet) {
		fprintf(stderr, "malloc packet");
		exit(EXIT_FAILURE);
	}

	for (;;) {
		struct sockaddr_in from;
		socklen_t fromlen = sizeof from;
		int n;
		if ((n = recvfrom(sock_icmp, packet, packlen, 0, (struct sockaddr *)&from, &fromlen)) < 0) {
			if (errno == EINTR) {
				printf("recvfrom: errno = EINTR\n");
				continue;
			}
			perror("recvfrom");
		}
		if (procpack(packet, n, &from) == 0) {
			break;
		}
	}

	return 0;
}

void sender(void)
{
	struct icmp *icp;
	int len; 
	int n;

	icp = (struct icmp *)outpack;
	icp->icmp_type	= ICMP_TSTAMP;
	icp->icmp_code	= 0;
	icp->icmp_cksum = 0;
	icp->icmp_id	= getpid();
	icp->icmp_seq	= 12345;

	// 发起时间戳是自00:00开始到如今的毫秒数

	gettimeofday(&tvorig, NULL);
	tsorig = (tvorig.tv_sec % (24 * 60 * 60)) * 1000 + tvorig.tv_usec / 1000;
	icp->icmp_otime = htonl(tsorig);
	icp->icmp_rtime = 0;
	icp->icmp_ttime = 0;

	len = datalen + 8; // 12B data + 8B header
	icp->icmp_cksum = calc_cksum((uint16_t *)icp, len);

	if ((n = sendto(sock_icmp, outpack, len, 0, addrinfo->ai_addr, addrinfo->ai_addrlen)) < 0) {
		perror("sendto");
		exit(EXIT_FAILURE);
	}

	if (n != len) {
		fprintf(stderr, "wrote %s %d chars, ret = %d\n", addrinfo->ai_canonname, len, n);
		exit(EXIT_FAILURE);
	}
}

uint16_t calc_cksum(uint16_t *data, int len)
{
	int nleft = len;
	uint16_t *w = data;
	int sum = 0;
	uint16_t answer = 0;

	while (nleft > 1) {
		sum += *w;
		w++;
		nleft -= 2;
	}

	if (nleft == 1) {
		*(uint8_t *)(&answer) = *(uint8_t *)w;
		sum += answer;
	}

	sum = (sum >> 16) + (sum & 0xffff);
	sum += (sum >> 16);
	answer = ~sum;

	return answer;
}

int procpack(uint8_t *data, int len, struct sockaddr_in *from)
{
	struct ip *ip;
	char fromIP[INET_ADDRSTRLEN];
	struct icmp *icp;
	int hlen;

	ip = (struct ip *)data;
	hlen = ip->ip_hl * 4;
	if (len < hlen + ICMP_MINLEN) {
		fprintf(stderr, "packet too short (%d bytes from %s\n)", len, inet_ntop(AF_INET, &from->sin_addr, fromIP, INET_ADDRSTRLEN));
		return -1;
	}

	len -= hlen;
	icp = (struct icmp *)(data + hlen);
	if (icp->icmp_type == ICMP_TSTAMPREPLY) {
		if (ntohl(icp->icmp_otime) != tsorig) {
			printf("originate timestamp not echoed: send %ld, received %lu\n", tsorig, (unsigned long)ntohl(icp->icmp_otime));
		}
		if (len != 20) {
			printf("len = %d, expected len = 20", len);
			return -1;
		}
		if (icp->icmp_id != getpid()) {
			printf("received id %d\n", icp->icmp_id);
			return -1;
		}

		tsrecv = ntohl(icp->icmp_rtime);
		tsdiff = tsrecv - tsorig;

		printf("orig = %ld, recv = %lu, tsmit = %lu\n", tsorig, tsrecv, (unsigned long)ntohl(icp->icmp_ttime));
		printf("adjustment = %ld ms\n", tsdiff);

		return 0;
	}

	return -1;
}