1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
|
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <assert.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/ip_icmp.h>
#include <arpa/inet.h>
#include "icmp.h"
#include "util.h"
#define PROT_ICMP 1
static void make_sockaddr(struct sockaddr_in *dst, const char *ip_address) {
memset(dst, 0, sizeof *dst);
dst->sin_family = AF_INET;
dst->sin_port = htons(0);
dst->sin_addr.s_addr = inet_addr(ip_address);
}
static uint16_t compute_checksum(const void *buf_, size_t buflen) {
const uint8_t *buf_bytes = (const uint8_t*)buf_;
const uint16_t *buf = (const uint16_t*)buf_;
uint32_t res = 0;
for (size_t i = 0; i < buflen / 2; i++) {
res += buf[i];
res = (res + (res >> 16)) & 0xffff;
}
if (buflen % 2 == 1) {
res += (uint16_t)buf_bytes[buflen - 1];
res = (res + (res >> 16)) & 0xffff;
}
return res;
}
int icmp_open_socket(void) {
return socket(PF_INET, SOCK_DGRAM, PROT_ICMP);
}
struct icmp_reply icmp_communicate(int sock, const char *ip_address, int seqnum, const void *data_, size_t length) {
static uint8_t buffer[ICMP_MAX_PAYLOAD_LENGTH];
const uint8_t *data = (const uint8_t*)data_;
assert(length <= ICMP_MAX_PAYLOAD_LENGTH);
struct icmp_reply errret = (struct icmp_reply){.data = NULL, .length = 0, .seqnum = 0};
struct sockaddr_in addr;
make_sockaddr(&addr, ip_address);
struct icmp_echo msg;
memset(&msg, 0, sizeof msg);
msg.type = ICMP_ECHO;
msg.code = 0;
msg.seqnum = seqnum;
memcpy(msg.payload, data, length);
if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) {
return errret;
}
struct msghdr replyhdr;
memset(&replyhdr, 0, sizeof replyhdr);
struct iovec iov1;
memset(&iov1, 0, sizeof iov1);
iov1.iov_base = &msg;
iov1.iov_len = sizeof msg;
replyhdr.msg_name = &addr;
replyhdr.msg_namelen = sizeof addr;
replyhdr.msg_iov = &iov1;
replyhdr.msg_iovlen = 1;
ssize_t nr = recvmsg(sock, &replyhdr, 0);
if (nr < 0) {
return errret;
}
size_t payloadlen = nr - ICMP_PAYLOAD_OFFSET;
assert(payloadlen <= MAX_DATAGRAM_SIZE);
memcpy(buffer, msg.payload, payloadlen);
return (struct icmp_reply){
.data = buffer,
.length = payloadlen,
.seqnum = msg.seqnum
};
}
int icmp_send_echo_reply(const char *ip_address, int id, int seqnum, const void *data_, size_t length) {
const uint8_t *data = (const uint8_t*)data_;
struct sockaddr_in addr;
make_sockaddr(&addr, ip_address);
struct icmp_echo msg;
memset(&msg, 0, sizeof msg);
msg.type = ICMP_ECHOREPLY;
msg.code = 0;
msg.id = id;
msg.seqnum = seqnum;
msg.checksum = 0;
memcpy(msg.payload, data, length);
size_t total_length = ICMP_PAYLOAD_OFFSET + length;
msg.checksum = ~compute_checksum(&msg, total_length);
int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
// The below is only necessary for sending on IPPROTO_RAW sockets
// int zero = 0;
// assert(setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &zero, sizeof zero) >= 0);
int ret = sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr);
close(sock);
if (ret < 0) return -1;
return 0;
}
|
