Ping
#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