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
|
#include <stdlib.h>
#include <stddef.h>
#include <string.h>
#include <assert.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_;
int sock = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP);
int zero = 0;
assert(setsockopt(sock, IPPROTO_IP, IP_HDRINCL, &zero, sizeof zero) >= 0);
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);
if (sendto(sock, &msg, ICMP_PAYLOAD_OFFSET + length, 0, (struct sockaddr*)&addr, sizeof addr) < 0) {
return -1;
}
return 0;
}
|
