/
icmp.c
154 lines (142 loc) · 4.26 KB
/
icmp.c
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
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "util.h"
#include "ip.h"
#include "icmp.h"
#define ICMP_BUFSIZ IP_PAYLOAD_SIZE_MAX
struct icmp_hdr {
uint8_t type;
uint8_t code;
uint16_t sum;
uint32_t values;
};
struct icmp_echo {
uint8_t type;
uint8_t code;
uint16_t sum;
uint16_t id;
uint16_t seq;
};
static char *
icmp_type_ntoa(uint8_t type) {
switch (type) {
case ICMP_TYPE_ECHOREPLY:
return "EchoReply";
case ICMP_TYPE_DEST_UNREACH:
return "DestinationUnreachable";
case ICMP_TYPE_SOURCE_QUENCH:
return "SourceQuench";
case ICMP_TYPE_REDIRECT:
return "Redirect";
case ICMP_TYPE_ECHO:
return "Echo";
case ICMP_TYPE_TIME_EXCEEDED:
return "TimeExceeded";
case ICMP_TYPE_PARAM_PROBLEM:
return "ParameterProblem";
case ICMP_TYPE_TIMESTAMP:
return "Timestamp";
case ICMP_TYPE_TIMESTAMPREPLY:
return "TimestampReply";
case ICMP_TYPE_INFO_REQUEST:
return "InformationRequest";
case ICMP_TYPE_INFO_REPLY:
return "InformationReply";
}
return "Unknown";
}
static void
icmp_dump(const uint8_t *data, size_t len)
{
struct icmp_hdr *hdr;
struct icmp_echo *echo;
flockfile(stderr);
hdr = (struct icmp_hdr *)data;
fprintf(stderr, " type: %u (%s)\n", hdr->type, icmp_type_ntoa(hdr->type));
fprintf(stderr, " code: %u\n", hdr->code);
fprintf(stderr, " sum: 0x%04x (0x%04x)\n", ntoh16(hdr->sum), ntoh16(cksum16((uint16_t *)data, len, -hdr->sum)));
switch (hdr->type) {
case ICMP_TYPE_ECHOREPLY:
case ICMP_TYPE_ECHO:
echo = (struct icmp_echo *)hdr;
fprintf(stderr, " id: %u\n", ntoh16(echo->id));
fprintf(stderr, " seq: %u\n", ntoh16(echo->seq));
break;
default:
fprintf(stderr, " values: 0x%08x\n", ntoh32(hdr->values));
break;
}
#ifdef HEXDUMP
hexdump(stderr, data, len);
#endif
funlockfile(stderr);
}
static void
icmp_input(const uint8_t *data, size_t len, ip_addr_t src, ip_addr_t dst, struct ip_iface *iface)
{
struct icmp_hdr *hdr;
char addr1[IP_ADDR_STR_LEN];
char addr2[IP_ADDR_STR_LEN];
char addr3[IP_ADDR_STR_LEN];
if (len < sizeof(*hdr)) {
errorf("too short");
return;
}
hdr = (struct icmp_hdr *)data;
if (cksum16((uint16_t *)data, len, 0) != 0) {
errorf("checksum error, sum=0x%04x, verify=0x%04x", ntoh16(hdr->sum), ntoh16(cksum16((uint16_t *)data, len, -hdr->sum)));
return;
}
debugf("%s => %s, type=%s(%u), len=%zu, iface=%s",
ip_addr_ntop(src, addr1, sizeof(addr1)),
ip_addr_ntop(dst, addr2, sizeof(addr2)),
icmp_type_ntoa(hdr->type), hdr->type, len,
ip_addr_ntop(iface->unicast, addr3, sizeof(addr3)));
icmp_dump(data, len);
switch (hdr->type) {
case ICMP_TYPE_ECHO:
if (dst != iface->unicast) {
/* message addressed to broadcast address. */
/* responds with the address of the received interface. */
dst = iface->unicast;
}
icmp_output(ICMP_TYPE_ECHOREPLY, hdr->code, hdr->values, (uint8_t *)(hdr + 1), len - sizeof(*hdr), dst, src);
break;
default:
/* ignore */
break;
}
}
int
icmp_output(uint8_t type, uint8_t code, uint32_t values, const uint8_t *data, size_t len, ip_addr_t src, ip_addr_t dst)
{
uint8_t buf[ICMP_BUFSIZ];
struct icmp_hdr *hdr;
size_t msg_len;
char addr1[IP_ADDR_STR_LEN];
char addr2[IP_ADDR_STR_LEN];
hdr = (struct icmp_hdr *)buf;
hdr->type = type;
hdr->code = code;
hdr->sum = 0;
hdr->values = values;
memcpy(hdr + 1, data, len);
msg_len = sizeof(*hdr) + len;
hdr->sum = cksum16((uint16_t *)hdr, msg_len, 0);
debugf("%s => %s, type=%s(%u), len=%zu",
ip_addr_ntop(src, addr1, sizeof(addr1)),
ip_addr_ntop(dst, addr2, sizeof(addr2)),
icmp_type_ntoa(hdr->type), hdr->type, msg_len);
icmp_dump((uint8_t *)hdr, msg_len);
return ip_output(IP_PROTOCOL_ICMP, (uint8_t *)hdr, msg_len, src, dst);
}
int
icmp_init(void)
{
if (ip_protocol_register("ICMP", IP_PROTOCOL_ICMP, icmp_input) == -1) {
errorf("ip_protocol_register() failure");
return -1;
}
return 0;
}