Ping

Makefile

CC=gcc
CFLAGS=-Wall -g -O2

all: ping

ping: ping.o
	$(CC) $(CFLAGS) -o $@ $^

ping.o: ping.c

.c.o:
	$(CC) $(CFLAGS) -c $<

clean:
	$(RM) *.o ping

ping.c

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

#include<unistd.h>
#include<netdb.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<netinet/in.h>
#include<netinet/ip.h>

#include<sys/time.h>
#include<poll.h>

typedef enum{
    Success, Failure, Timeout
} Status;

/*
 * ICMP
 */
#define ICMP_ECHO            8
#define ICMP_ECHOREPLY       0
#define ICMP6_ECHO_REQUEST 128
#define ICMP6_ECHO_REPLY   129
typedef struct{
    uint8_t type;
    uint8_t code;
    uint16_t checksum;
    uint16_t id;
    uint16_t seq;
    uint8_t data[];
}ICMP_Echo;

/*
 * Address
 */

Status get_icmp_protocol_number(int family, int *protocol)
{
    if(family==AF_INET){
	*protocol=IPPROTO_ICMP;
    }else if(family==AF_INET6){
	*protocol=IPPROTO_ICMPV6;
    }else{
	fputs("Unknown protocol\n", stderr);
	return Failure;
    }
    return Success;
}

struct addrinfo* getaddrinfo_icmp(int family, const char* node)
{
    int protocol;
    struct addrinfo hints;
    struct addrinfo *addrs;
    int err;

    if(get_icmp_protocol_number(family, &protocol)!=Success){
	return NULL;
    }
    hints.ai_flags=0;
    hints.ai_family=family;
    hints.ai_socktype=SOCK_RAW;
    hints.ai_protocol=protocol;
    hints.ai_addrlen=0;
    hints.ai_addr=NULL;
    hints.ai_canonname=NULL;
    hints.ai_next=NULL;

    err=getaddrinfo(node, NULL, &hints, &addrs);
    if(err!=0){
	if(err==EAI_SYSTEM){
	    perror("getaddrinfo");
	}else{
	    fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(err));
	}
	return NULL;
    }
    return addrs;
}

/*
 * Data
 */
typedef struct {
    uint16_t id;
    uint16_t seq;
    size_t datalen;
    uint8_t *data;
} PingData;

uint16_t calc_checksum(const ICMP_Echo *icmpdata, size_t length)
{
    const uint8_t *data=(const uint8_t*)icmpdata;
    int i;
    uint32_t checksum=0;
    for(i=0; i+1<length; i+=2){
	checksum+=(data[i+1]<<8)+data[i];
    }
    if(i<length){
	checksum+=data[i];
    }
    checksum=(checksum&0xffff)+(checksum>>16);
    checksum=(checksum&0xffff)+(checksum>>16);
    return ~checksum;
}

ICMP_Echo* create_ping4_packet(const PingData *pingdata, size_t *packetlen)
{
    ICMP_Echo *packet;

    *packetlen=sizeof(ICMP_Echo)+pingdata->datalen;
    packet=calloc(*packetlen, 1);
    if(packet==NULL){
	perror("allocate send buffer");
	return NULL;
    }

    packet->type=ICMP_ECHO;
    packet->code=0;
    packet->checksum=0;
    packet->id=pingdata->id;
    packet->seq=pingdata->seq;
    memcpy(packet->data, pingdata->data, pingdata->datalen);
    packet->checksum=calc_checksum(packet, *packetlen);

    return packet;
}

ICMP_Echo* create_ping6_packet(const PingData *pingdata, size_t *packetlen)
{
    ICMP_Echo *packet;

    *packetlen=sizeof(ICMP_Echo)+pingdata->datalen;
    packet=calloc(*packetlen, 1);
    if(packet==NULL){
	perror("allocate send buffer");
	return NULL;
    }

    packet->type=ICMP6_ECHO_REQUEST;
    packet->code=0;
    packet->checksum=0;
    packet->id=pingdata->id;
    packet->seq=pingdata->seq;
    memcpy(packet->data, pingdata->data, pingdata->datalen);

    /* skip checksum */
    /* ICMPv6 checksum require pseudo header */

    return packet;
}

ICMP_Echo* create_ping_packet(int family, const PingData *pingdata, size_t *packetlen)
{
    if(family==AF_INET){
	return create_ping4_packet(pingdata, packetlen);
    }
    if(family==AF_INET6){
	return create_ping6_packet(pingdata, packetlen);
    }
    fputs("Unknown protocol\n", stderr);
    return NULL;
}

const ICMP_Echo* get_icmp_body(const uint8_t *data, size_t datalen, size_t *bodylen)
{
    struct ip *header=(struct ip*)data;
    size_t headerlen;

    if(datalen<1){
	fputs("IPv4 header is short\n", stderr);
	return NULL;
    }
    headerlen=(header->ip_hl)*4;
    if(headerlen>datalen){
	fputs("IPv4 header is short\n", stderr);
	return NULL;
    }
    *bodylen=datalen-headerlen;
    return (const ICMP_Echo*)(data+headerlen);
}

int is_my_responce(int family, const uint8_t *data, size_t datalen,
	const ICMP_Echo *senddata)
{
    const ICMP_Echo *body;
    size_t bodylen;
    uint8_t replycode;

    if(family==AF_INET){
	body=get_icmp_body(data, datalen, &bodylen);
	if(body==NULL){
	    return 0;
	}
	replycode=ICMP_ECHOREPLY;
    }else if(family==AF_INET6){
	body=(const ICMP_Echo*)data;
	bodylen=datalen;
	replycode=ICMP6_ECHO_REPLY;
    }else{
	return 0;
    }
    if(bodylen<sizeof(ICMP_Echo)){
	return 0;
    }
    if(body->type!=replycode){
	return 0;
    }
    if(body->id!=senddata->id){
	return 0;
    }
    if(body->seq!=senddata->seq){
	return 0;
    }
    return 1;
}


Status check_ping4_responce(const ICMP_Echo *recvdata, size_t recvlen,
	const ICMP_Echo *senddata, size_t sendlen)
{
    uint16_t checksum;
    size_t payloadlen;

    if(recvlen<sizeof(ICMP_Echo)){
	fprintf(stderr, "recieved data is short. length=%zd\n", recvlen);
	return Failure;
    }

    checksum=calc_checksum(recvdata, recvlen);
    if(checksum!=0){
	fprintf(stderr, "checksum error. checksum=%hx\n",
		(unsigned short)checksum);
	return Failure;
    }

    if(recvdata->type!=ICMP_ECHOREPLY){
	fprintf(stderr,
		"recieved data is not ICMP echo reply, type=%hhu, code=%hhu\n",
		recvdata->type, recvdata->code);
	return Failure;
    }

    if(senddata->id!=recvdata->id){
	fprintf(stderr, "different id send=%hu recv=%hu\n",
		senddata->id, recvdata->id);
	return Failure;
    }
    if(senddata->seq!=recvdata->seq){
	fprintf(stderr, "different seq send=%hu recv=%hu\n",
		senddata->seq, recvdata->seq);
	return Failure;
    }

    payloadlen=recvlen-sizeof(ICMP_Echo);
    if(sendlen!=recvlen || memcmp(senddata->data, recvdata->data, payloadlen)!=0){
	fprintf(stderr, "different payload\n");
	return Failure;
    }

    return Success;
}

Status check_ping6_responce(const ICMP_Echo *recvdata, size_t recvlen,
	const ICMP_Echo *senddata, size_t sendlen)
{
    size_t payloadlen;

    if(recvlen<sizeof(ICMP_Echo)){
	fprintf(stderr, "recieved data is short. length=%zd\n", recvlen);
	return Failure;
    }

    /* skip checksum */
    /* IPv6 checksum need pseudo header. */

    if(recvdata->type!=ICMP6_ECHO_REPLY){
	fprintf(stderr,
		"recieved data is not ICMPv6 echo reply, type=%hhu, code=%hhu\n",
		recvdata->type, recvdata->code);
	return Failure;
    }

    if(senddata->id!=recvdata->id){
	fprintf(stderr, "different id send=%hu recv=%hu\n",
		senddata->id, recvdata->id);
	return Failure;
    }
    if(senddata->seq!=recvdata->seq){
	fprintf(stderr, "different seq send=%hu recv=%hu\n",
		senddata->seq, recvdata->seq);
	return Failure;
    }

    payloadlen=recvlen-sizeof(ICMP_Echo);
    if(sendlen!=recvlen || memcmp(senddata->data, recvdata->data, payloadlen)!=0){
	fprintf(stderr, "different payload\n");
	return Failure;
    }

    return Success;
}

Status check_ping_responce(int family, const uint8_t *recvdata, size_t recvlen,
	const ICMP_Echo *senddata, size_t sendlen)
{
    const ICMP_Echo *body;
    if(family==AF_INET){
	size_t replylen;
	body=get_icmp_body(recvdata, recvlen, &replylen);
	if(body==NULL){
	    return Failure;
	}
	return check_ping4_responce(body, replylen, senddata, sendlen);
    }
    if(family==AF_INET6){
	body=(const ICMP_Echo*)recvdata;
	return check_ping6_responce(body, recvlen, senddata, sendlen);
    }
    fputs("Unknown protocol\n", stderr);
    return Failure;
}

/*
 * Time
 */
int current_timeout(int total_timeout, struct timeval starttime)
{
    int now_timeout;
    struct timeval now;

    if(gettimeofday(&now, NULL)==-1){
	return total_timeout;
    }
    now_timeout=total_timeout-(now.tv_sec-starttime.tv_sec)*1000
	-(now.tv_usec-now.tv_usec)/1000;
    if(now_timeout<0){
	return 0;
    }
    return now_timeout;
}

/*
 * Socket
 */
Status wait_socket(int sock, int timeout)
{
    struct pollfd fds;
    int ready;

    fds.fd=sock;
    fds.events=POLLIN;
    fds.revents=0;

    ready=poll(&fds, 1, timeout);
    if(ready<0){
	perror("poll");
	return Failure;
    }else if(ready==0){
	return Timeout;
    }else if(fds.revents&POLLNVAL){
	fprintf(stderr, "poll: invalid fd\n");
	return Failure;
    }else if(fds.revents&POLLERR){
	fprintf(stderr, "poll: error\n");
	return Failure;
    }else if(fds.revents&POLLIN){
	return Success;
    }
    return Failure;
}

/* Send */
Status send_ping(int sock, const struct addrinfo *address,
	const ICMP_Echo *data, size_t datalen)
{
    ssize_t sendlen;

    sendlen=sendto(sock, data, datalen, 0, address->ai_addr, address->ai_addrlen);
    if(sendlen==-1){
	perror("send");
	return Failure;
    }
    if(sendlen<datalen){
	fprintf(stderr, "sent message is short\n");
	return Failure;
    }
    return Success;
}

/* Receive */
Status recv_ping(int sock, struct addrinfo *address, uint8_t *buffer, size_t *buflen)
{
    socklen_t addrlen=address->ai_addrlen;
    ssize_t recvlen;

    recvlen=recvfrom(sock, buffer, *buflen, 0, address->ai_addr, &addrlen);
    if(recvlen==-1){
	perror("recvfrom");
	return Failure;
    }
    if(addrlen>address->ai_addrlen){
	fprintf(stderr, "Warning: addrlen=%d", addrlen);
    }
    *buflen=recvlen;
    return Success;
}

/*
 * Ping
 */
#define IP_HEADER_MAX 60
Status ping_address(const struct addrinfo *address, struct addrinfo *host,
	const PingData *senddata, struct timeval *duration)
{
    int sock=-1;
    ICMP_Echo *packet=NULL;
    size_t packetlen;
    uint8_t *buffer=NULL;
    size_t buflen;
    struct timeval starttime, endtime;
    Status status=Failure, wait_status;

    packet=create_ping_packet(address->ai_family, senddata, &packetlen);
    if(packet==NULL){
	goto END;
    }

    buflen=packetlen+IP_HEADER_MAX+1;
    buffer=malloc(buflen);
    if(buffer==NULL){
	perror("allocate recv buffer");
	goto END;
    }

    sock=socket(address->ai_family, address->ai_socktype, address->ai_protocol);
    if(sock==-1){
	perror("create socket");
	goto END;
    }

    if(gettimeofday(&starttime, NULL)==-1){
	perror("gettimeofday");
	goto END;
    }

    if(send_ping(sock, address, packet, packetlen)!=Success){
	goto END;
    }

    do{
	int timeout=current_timeout(5000, starttime);
	wait_status=wait_socket(sock, timeout);
	if(wait_status!=Success){
	    status=wait_status;
	    goto END;
	}

	if(recv_ping(sock, host, buffer, &buflen)!=Success){
	    goto END;
	}
    }while(!is_my_responce(address->ai_family, buffer, buflen, packet));

    if(gettimeofday(&endtime, NULL)==-1){
	perror("gettimeofday");
	goto END;
    }
    timersub(&endtime, &starttime, duration);

    if(check_ping_responce(address->ai_family, buffer, buflen,
		packet, packetlen)!=Success){
	goto END;
    }

    status=Success;
END:
    free(packet);
    free(buffer);
    if(sock!=-1){
	if(close(sock)){
	    perror("close socket");
	    status=Failure;
	}
    }
    return status;
}

Status ping(int family, const char* node, const PingData *senddata,
	char *hostname, size_t hostlen, struct timeval *duration)
{
    struct addrinfo *address=NULL;
    struct addrinfo hostaddr={.ai_addr=NULL};
    int err;
    Status status=Failure, ping_status;

    address=getaddrinfo_icmp(family, node);
    if(address==NULL){
	goto END;
    }
    hostaddr.ai_addrlen=address->ai_addrlen;
    hostaddr.ai_addr=malloc(hostaddr.ai_addrlen);
    if(hostaddr.ai_addr==NULL){
	perror("allocate host address info");
	goto END;
    }

    ping_status=ping_address(address, &hostaddr, senddata, duration);
    if(ping_status!=Success){
	status=ping_status;
	goto END;
    }

    err=getnameinfo(hostaddr.ai_addr, hostaddr.ai_addrlen, hostname, hostlen,
	    NULL, 0, 0);
    if(err!=0){
	if(err==EAI_SYSTEM){
	    perror("getaddrinfo");
	}else{
	    fprintf(stderr, "getnameinfo: %s\n", gai_strerror(err));
	}
	goto END;
    }

    status=Success;
END:
    if(address!=NULL){
	freeaddrinfo(address);
    }
    free(hostaddr.ai_addr);
    return status;
}

int main(int argc, char** argv)
{
    int addrindex=1;
    int family=AF_INET;
    char *node;
    size_t ping_datasize=56;
    PingData senddata={.data=NULL};
    char replyhost[NI_MAXHOST];
    size_t hostlen=NI_MAXHOST;
    struct timeval duration;
    Status status;
    int exit_status=EXIT_FAILURE;
    int i;

    if(argc>1){
	if(strcmp(argv[1],"-4")==0){
	    family=AF_INET;
	    addrindex++;
	}else if(strcmp(argv[1],"-6")==0){
	    family=AF_INET6;
	    addrindex++;
	}
    }
    if(addrindex>=argc){
	puts("ping [-4|-6] address");
	goto END;
    }
    node=argv[addrindex];

    senddata.id=getpid();
    senddata.seq=1;
    senddata.datalen=ping_datasize;
    senddata.data=malloc(senddata.datalen);
    if(senddata.data==NULL){
	perror("allocate ping data");
	goto END;
    }
    for(i=0; i<ping_datasize; i++){
	senddata.data[i]=0xff&i;
    }

    status=ping(family, node, &senddata, replyhost, hostlen,
	    &duration);
    if(status==Success){
	double dt=duration.tv_sec*1000+duration.tv_usec/1000.0;
	printf("ping to %s reply from %s time %.3fms\n", node, replyhost, dt);
    }else if(status==Timeout){
	printf("ping to %s timeout\n", node);
    }else{
	goto END;
    }
    exit_status=EXIT_SUCCESS;

END:
    free(senddata.data);
    return exit_status;
}