#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <stdlib.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <netinet/ip_icmp.h>
#include <sys/time.h>
#include <unistd.h>
#define MAXIPLEN 60
#define MAXICMPLEN 76
struct addrinfo *addrinfo;
int sock_icmp;
int datalen = 12;
uint8_t outpack[65536 - 60 - 8];
struct timeval tvorig;
long tsorig, tsrecv;
long tsdiff;
uint16_t calc_cksum(uint16_t *data, int len);
void sender(void);
int procpack(uint8_t *data, int len, struct sockaddr_in *from);
int main(int argc, char *argv[])
{
struct addrinfo hints;
int res;
struct sockaddr_in *addr;
char ip[INET_ADDRSTRLEN];
if (argc != 2) {
fprintf(stderr, "Usage: %s destination\n", argv[0]);
exit(EXIT_FAILURE);
}
memset(&hints, 0, sizeof hints);
hints.ai_flags = AI_CANONNAME;
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_RAW;
hints.ai_protocol = IPPROTO_ICMP;
if ((res = getaddrinfo(argv[1], NULL, &hints, &addrinfo)) != 0) {
fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(res));
exit(EXIT_FAILURE);
}
addr = (struct sockaddr_in *)addrinfo->ai_addr;
inet_ntop(addrinfo->ai_family, &addr->sin_addr, ip, INET_ADDRSTRLEN);
printf(
"ai_flags = %d\n"
"ai_family = %d\n"
"ai_socktype = %d\n"
"ai_protocol = %d\n"
"ai_addrlen = %d\n"
"ai_addr.sin_family = %d, sin_port = %d, sin_addr.s_addr = %s\n"
"ai_canonname = %s\n",
addrinfo->ai_flags,
addrinfo->ai_family,
addrinfo->ai_socktype,
addrinfo->ai_protocol,
addrinfo->ai_addrlen,
addr->sin_family, addr->sin_port, ip,
addrinfo->ai_canonname);
if ((sock_icmp = socket(addrinfo->ai_family, addrinfo->ai_socktype, addrinfo->ai_protocol)) < 0) {
perror("socket");
exit(EXIT_FAILURE);
}
sender();
int packlen = datalen + MAXICMPLEN + MAXIPLEN;
uint8_t *packet = (uint8_t *)malloc(packlen);
if (!packet) {
fprintf(stderr, "malloc packet");
exit(EXIT_FAILURE);
}
for (;;) {
struct sockaddr_in from;
socklen_t fromlen = sizeof from;
int n;
if ((n = recvfrom(sock_icmp, packet, packlen, 0, (struct sockaddr *)&from, &fromlen)) < 0) {
if (errno == EINTR) {
printf("recvfrom: errno = EINTR\n");
continue;
}
perror("recvfrom");
}
if (procpack(packet, n, &from) == 0) {
break;
}
}
return 0;
}
void sender(void)
{
struct icmp *icp;
int len;
int n;
icp = (struct icmp *)outpack;
icp->icmp_type = ICMP_TSTAMP;
icp->icmp_code = 0;
icp->icmp_cksum = 0;
icp->icmp_id = getpid();
icp->icmp_seq = 12345;
// 发起时间戳是自00:00开始到现在的毫秒数
gettimeofday(&tvorig, NULL);
tsorig = (tvorig.tv_sec % (24 * 60 * 60)) * 1000 + tvorig.tv_usec / 1000;
icp->icmp_otime = htonl(tsorig);
icp->icmp_rtime = 0;
icp->icmp_ttime = 0;
len = datalen + 8; // 12B data + 8B header
icp->icmp_cksum = calc_cksum((uint16_t *)icp, len);
if ((n = sendto(sock_icmp, outpack, len, 0, addrinfo->ai_addr, addrinfo->ai_addrlen)) < 0) {
perror("sendto");
exit(EXIT_FAILURE);
}
if (n != len) {
fprintf(stderr, "wrote %s %d chars, ret = %d\n", addrinfo->ai_canonname, len, n);
exit(EXIT_FAILURE);
}
}
uint16_t calc_cksum(uint16_t *data, int len)
{
int nleft = len;
uint16_t *w = data;
int sum = 0;
uint16_t answer = 0;
while (nleft > 1) {
sum += *w;
w++;
nleft -= 2;
}
if (nleft == 1) {
*(uint8_t *)(&answer) = *(uint8_t *)w;
sum += answer;
}
sum = (sum >> 16) + (sum & 0xffff);
sum += (sum >> 16);
answer = ~sum;
return answer;
}
int procpack(uint8_t *data, int len, struct sockaddr_in *from)
{
struct ip *ip;
char fromIP[INET_ADDRSTRLEN];
struct icmp *icp;
int hlen;
ip = (struct ip *)data;
hlen = ip->ip_hl * 4;
if (len < hlen + ICMP_MINLEN) {
fprintf(stderr, "packet too short (%d bytes from %s\n)", len, inet_ntop(AF_INET, &from->sin_addr, fromIP, INET_ADDRSTRLEN));
return -1;
}
len -= hlen;
icp = (struct icmp *)(data + hlen);
if (icp->icmp_type == ICMP_TSTAMPREPLY) {
if (ntohl(icp->icmp_otime) != tsorig) {
printf("originate timestamp not echoed: send %ld, received %lu\n", tsorig, (unsigned long)ntohl(icp->icmp_otime));
}
if (len != 20) {
printf("len = %d, expected len = 20", len);
return -1;
}
if (icp->icmp_id != getpid()) {
printf("received id %d\n", icp->icmp_id);
return -1;
}
tsrecv = ntohl(icp->icmp_rtime);
tsdiff = tsrecv - tsorig;
printf("orig = %ld, recv = %lu, tsmit = %lu\n", tsorig, tsrecv, (unsigned long)ntohl(icp->icmp_ttime));
printf("adjustment = %ld ms\n", tsdiff);
return 0;
}
return -1;
}