Skip to content

Instantly share code, notes, and snippets.

@yshl

yshl/Makefile

Last active May 1, 2019
Embed
What would you like to do?
Ping
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
#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
You can’t perform that action at this time.