ping.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <netinet/ip_icmp.h>
#include <arpa/inet.h>
#include <sys/time.h>
#include <sys/select.h>


struct tv32 {
	uint32_t tv32_sec;
	uint32_t tv32_usec;
};


#define MAXICMPLEN 76 // 8B header + (60B IP header + 8B DATA)
#define TV32_LEN	(sizeof(struct tv32))


uint8_t icmp_type 	  = ICMP_ECHO;
uint8_t icmp_type_rsp = ICMP_ECHOREPLY;

uint8_t outpackhdr[IP_MAXPACKET];
uint8_t *outpack; 

int ident;
int datalen = 56;

/* counters */
long nsend   = 1;
long nrecv   = 0;
int interval = 1000;

/* timing */
double tmin = 99999999.0;
double tmax = 0.0;
double tsum = 0.0;
double tsumsq = 0.0;

struct addrinfo *dst;
int sock_icmp;
char dstIP[INET_ADDRSTRLEN];

void pinger(struct timeval *tv_send);
uint16_t in_cksum(uint16_t *h, int hlen);
struct timeval tvsub(struct timeval *tv1, struct timeval *tv2);
void pr_pack(uint8_t *packet, int len, struct sockaddr_in *from, struct timeval *tv_recv);

int main(int argc, char *argv[])
{
	if (argc != 2) {
		printf("Usage: %s destination\n", argv[0]);
		exit(EXIT_FAILURE);
	}

	struct addrinfo hints;
	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;

	int res;
	if ((res = getaddrinfo(argv[1], NULL, &hints, &dst)) != 0) {
		fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(res));
		exit(EXIT_FAILURE);
	}

	if ((sock_icmp = socket(dst->ai_family, dst->ai_socktype, dst->ai_protocol)) == -1) {
		perror("socket");
		exit(EXIT_FAILURE);
	}

	struct sockaddr_in *dstAddr = (struct sockaddr_in *)dst->ai_addr;
	inet_ntop(AF_INET, &dstAddr->sin_addr, dstIP, INET_ADDRSTRLEN);

	printf("ping %s(%s)\n", dst->ai_canonname, dstIP);

	outpack = outpackhdr + sizeof(struct ip);
	ident = getpid() & 0xFFFF;

	struct timeval tv_send;
	pinger(&tv_send);

	struct timeval intvl;
	intvl.tv_sec = interval / 1000; 
	intvl.tv_usec = interval % 1000 * 1000;

	uint8_t packet[IP_MAXPACKET];
	memset(packet, 0, IP_MAXPACKET);

	struct sockaddr_in from;
	socklen_t fromlen = sizeof from;

	int n, c;
	while (1) {
		fd_set rfds;
		FD_ZERO(&rfds);	
		FD_SET(sock_icmp, &rfds);

		struct timeval now, timeout;
		gettimeofday(&now, NULL);
		timeout.tv_sec  = tv_send.tv_sec + intvl.tv_sec - 1 - now.tv_sec;
		timeout.tv_usec = tv_send.tv_usec + intvl.tv_usec + 1000000 - now.tv_usec;
		while (timeout.tv_usec >= 1000000) {
			timeout.tv_usec -= 1000000;
			timeout.tv_sec++;
		}
		if (timeout.tv_sec < 0) {
			timeout.tv_sec = timeout.tv_usec = 0;
		}
		n = select(sock_icmp + 1, &rfds, NULL, NULL, &timeout);
		if (n < 0) {
			perror("select");
			continue;
		}
		if (n == 0) {
			continue;
		}
		if (n == 1) {
			if ((c = recvfrom(sock_icmp, packet, IP_MAXPACKET, 0, (struct sockaddr *)&from, &fromlen)) < 0) {
				perror("recvfrom");
				exit(EXIT_FAILURE);
			}
			struct timeval tv_recv;	
			gettimeofday(&tv_recv, NULL);
			pr_pack(packet, c, &from, &tv_recv);
		}
		timeout.tv_usec += timeout.tv_sec * 1000000;
		usleep(timeout.tv_usec);
		pinger(&tv_send);
	}
	
	return 0;
}

void pinger(struct timeval *tv_send)
{
	struct icmp *icp;
	struct tv32 tv32;
	int len, n;

	icp = (struct icmp *)outpack;
	icp->icmp_type 	= icmp_type;
	icp->icmp_code	= 0;
	icp->icmp_cksum	= 0;
	icp->icmp_id	= ident;
	icp->icmp_seq	= htons(nsend);

	gettimeofday(tv_send, NULL);
	tv32.tv32_sec  = htonl(tv_send->tv_sec);
	tv32.tv32_usec = htonl(tv_send->tv_usec);
	memcpy(icp->icmp_data, &tv32, TV32_LEN);

	len = ICMP_MINLEN + datalen;
	icp->icmp_cksum = in_cksum((uint16_t *)icp, len);

	if ((n = sendto(sock_icmp, icp, len, 0, dst->ai_addr, dst->ai_addrlen)) == -1) {
		perror("sento");
		exit(EXIT_FAILURE);
	}

	if (n != len) {
		fprintf(stderr, "partial write %s %d(%d)\n", dstIP, len, n);
		exit(EXIT_FAILURE);
	}

	nsend++;
}

void pr_pack(uint8_t *packet, int len, struct sockaddr_in *from, struct timeval *tv_recv)
{
	struct ip *ip;
	int hlen;

	ip = (struct ip *)packet;
	hlen = ip->ip_hl * 4;
	if (len < hlen + ICMP_MINLEN) {
		printf("packet too short");
		return;
	}

	struct icmp *icp;
	double triptime;
	uint8_t *tp;

	len -= hlen;
	icp = (struct icmp *)(packet + hlen);
	if (icp->icmp_type == icmp_type_rsp) {
		if (icp->icmp_id != ident) {
			return;
		}
		nrecv++;
		triptime = 0.0;
		struct tv32 tv32;
		struct timeval tv_send;
		struct timeval tv_diff;
		tp = icp->icmp_data;
		if (len - ICMP_MINLEN >= sizeof tv32) {
			memcpy(&tv32, tp, sizeof(tv32));	
			tv_send.tv_sec  = ntohl(tv32.tv32_sec);
			tv_send.tv_usec = ntohl(tv32.tv32_usec);
			tv_diff  = tvsub(tv_recv, &tv_send);	
			triptime = tv_diff.tv_sec * 1000.0 + tv_diff.tv_usec / 1000.0;
			tsum   += triptime;
			tsumsq += triptime * triptime;
			if (triptime < tmin) {
				tmin = triptime;
			}
			if (triptime > tmax) {
				tmax = triptime;
			}
		}
	}

	char fromIP[INET_ADDRSTRLEN];
	inet_ntop(AF_INET, &from->sin_addr, fromIP, INET_ADDRSTRLEN);
	printf(
		"%d bytes from %s: icmp_seq=%u ttl=%d time=%.3f ms\n", 
		len, 
		fromIP, 
		ntohs(icp->icmp_seq),
		ip->ip_ttl,
		triptime
	);
}

struct timeval tvsub(struct timeval *tv1, struct timeval *tv2)
{
	struct timeval diff;
	diff.tv_sec  = tv1->tv_sec - 1 - tv2->tv_sec;
	diff.tv_usec = tv1->tv_usec + 1000000 - tv2->tv_usec;
	if (diff.tv_usec > 1000000) {
		diff.tv_sec += 1;
		diff.tv_usec -= 1000000;
	}
	return diff;
}

uint16_t in_cksum(uint16_t *h, int hlen)
{
	int nleft, sum;
	uint16_t *w;
	uint16_t answer;

	nleft = hlen;
	sum = 0;
	w = h;

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

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

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

	return answer;
}