Last active
May 1, 2019 01:36
-
-
Save yshl/97fec3eb8ad7e3218da0 to your computer and use it in GitHub Desktop.
Ping
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment