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_icmp.h>
#include<netinet/icmp6.h>

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

typedef enum{
    Success, Failure, Timeout
} Status;

/*
 * 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 uint8_t *data, size_t length)
{
    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;
}

uint8_t* create_ping4_packet(const PingData *pingdata, size_t *packetlen)
{
    uint8_t *packet;
    struct icmphdr *header;

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

    header=(struct icmphdr*)packet;
    header->type=ICMP_ECHO;
    header->code=0;
    header->un.echo.id=pingdata->id;
    header->un.echo.sequence=pingdata->seq;
    memcpy(packet+ICMP_MINLEN, pingdata->data, pingdata->datalen);
    header->checksum=calc_checksum(packet, *packetlen);

    return packet;
}

uint8_t* create_ping6_packet(const PingData *pingdata, size_t *packetlen)
{
    uint8_t *packet;
    struct icmp6_hdr *header;

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

    header=(struct icmp6_hdr*)packet;
    header->icmp6_type=ICMP6_ECHO_REQUEST;
    header->icmp6_code=0;
    header->icmp6_id=pingdata->id;
    header->icmp6_seq=pingdata->seq;
    memcpy(packet+sizeof(*header), pingdata->data, pingdata->datalen);

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

    return packet;
}

uint8_t* 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 uint8_t* get_ip4_body(const uint8_t *data, size_t datalen, size_t *bodylen)
{
    struct iphdr *header=(struct iphdr*)data;
    size_t headerlen;

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

int is_ping_request(int family, const uint8_t *data, size_t datalen)
{
    if(family==AF_INET){
	const uint8_t *body;
	size_t bodylen;

	body=get_ip4_body(data, datalen, &bodylen);
	if(body==NULL){
	    return 0;
	}
	if(bodylen<1){
	    return 0;
	}
	return body[0]==ICMP_ECHO;
    }
    if(family==AF_INET6){
	return data[0]==ICMP6_ECHO_REQUEST;
    }
    return 0;
}


Status check_ping4_responce(const uint8_t *recvdata, size_t recvlen,
	const uint8_t *senddata, size_t sendlen)
{
    const struct icmphdr *recvhead=(const struct icmphdr*)recvdata;
    const struct icmphdr *sendhead=(const struct icmphdr*)senddata;
    uint16_t checksum;
    const uint8_t *sendpayload, *recvpayload;
    size_t payloadlen;

    if(recvlen<ICMP_MINLEN){
	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(recvhead->type!=ICMP_ECHOREPLY){
	fprintf(stderr,
		"recieved data is not ICMP echo reply, type=%hhu, code=%hhu\n",
		recvhead->type, recvhead->code);
	return Failure;
    }

    if(sendhead->un.echo.id!=recvhead->un.echo.id){
	fprintf(stderr, "different id send=%hu recv=%hu\n",
		sendhead->un.echo.id, recvhead->un.echo.id);
	return Failure;
    }
    if(sendhead->un.echo.sequence!=recvhead->un.echo.sequence){
	fprintf(stderr, "different seq send=%hu recv=%hu\n",
		sendhead->un.echo.sequence, recvhead->un.echo.sequence);
	return Failure;
    }

    sendpayload=senddata+ICMP_MINLEN;
    recvpayload=recvdata+ICMP_MINLEN;
    payloadlen=recvlen-ICMP_MINLEN;
    if(sendlen!=recvlen || memcmp(sendpayload, recvpayload, payloadlen)!=0){
	fprintf(stderr, "different payload\n");
	return Failure;
    }

    return Success;
}

Status check_ping6_responce(const uint8_t *recvdata, size_t recvlen,
	const uint8_t *senddata, size_t sendlen)
{
    const struct icmp6_hdr *recvhead=(const struct icmp6_hdr*)recvdata;
    const struct icmp6_hdr *sendhead=(const struct icmp6_hdr*)senddata;
    const uint8_t *sendpayload, *recvpayload;
    size_t payloadlen;

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

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

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

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

    sendpayload=senddata+ICMP_MINLEN;
    recvpayload=recvdata+ICMP_MINLEN;
    payloadlen=recvlen-ICMP_MINLEN;
    if(sendlen!=recvlen || memcmp(sendpayload, recvpayload, 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 uint8_t *senddata, size_t sendlen)
{
    if(family==AF_INET){
	const uint8_t *replydata;
	size_t replylen;
	replydata=get_ip4_body(recvdata, recvlen, &replylen);
	if(replydata==NULL){
	    return Failure;
	}
	return check_ping4_responce(replydata, replylen, senddata, sendlen);
    }
    if(family==AF_INET6){
	return check_ping6_responce(recvdata, recvlen, senddata, sendlen);
    }
    fputs("Unknown protocol\n", stderr);
    return Failure;
}

/*
 * 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 uint8_t *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;
    uint8_t *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{
	wait_status=wait_socket(sock, 5000);
	if(wait_status!=Success){
	    status=wait_status;
	    goto END;
	}

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

    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;
    char replyhost[NI_MAXHOST];
    size_t hostlen=NI_MAXHOST;
    struct timeval duration;
    Status status;
    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");
	exit(1);
    }
    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");
	exit(1);
    }
    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{
	exit(1);
    }

    return 0;
}