-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathicmp.c
200 lines (166 loc) · 8.42 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
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
/*
This file is part of kiwitun.
Kiwitun is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.
Kiwitun is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with kiwitun. If not, see <http://www.gnu.org/licenses/>.
*/
#include "icmp.h"
#include <sys/socket.h>
/**
* @brief Calculate ICMP packet checksum
* @param buf Data buffer starting with ICMP header
* @param size Buffer size in bytes
* @return 0 if success, -1 otherwise
* @warning New checksum is inserted into the header.
**/
int icmp_checksum(uint8_t *buf, uint16_t size)
{
uint32_t sum = 0;
for(uint16_t i = 0; i < size; i += 2) //group data in 16-bit words
{
if(i == ICMP_HEADER_CHECKSUM_POS) //skip checksum field itself (treat is a 0x0000)
continue;
sum += (((uint32_t)buf[i] << 8) | (uint32_t)buf[i + 1]); //convert to 16-bit words and add to the sum
}
sum = (sum & 0xFFFF) + (sum >> 16); //cut carry bits, shift them and add to the checksum
if(sum & 0x10000) //check if addition generated carry bit
{
sum &= 0xFFFF; //if so, remove it
sum++; //and add one to the checksum
}
sum = ~sum; //bitwise negate (one's complement)
buf[ICMP_HEADER_CHECKSUM_POS] = (sum >> 8) & 0xFF; //store checksum
buf[ICMP_HEADER_CHECKSUM_POS + 1] = sum & 0xFF;
return 0;
}
/**
* @brief Calculate ICMPv6 packet checksum
* @param buf Data buffer with ALL data to be send (main IPv6 header, ICMPv6 header and data)
* @param size Buffer size in bytes
* @return 0 if success, -1 otherwise
* @warning New checksum is inserted into the ICMPv6 header.
**/
int icmp_checksum6(uint8_t *buf, uint16_t size)
{
struct ip6_pseudohdr phdr; //IPv6 pseudo-header
struct ip6_hdr *hdr = (struct ip6_hdr*)buf; //real IPv6 header
phdr.ip6_src = hdr->ip6_src; //fill pseudo-header
phdr.ip6_dst = hdr->ip6_dst;
phdr.ip6_next = hdr->ip6_nxt;
phdr.ip6_len = hdr->ip6_plen;
memset(&(phdr.zeros), 0, sizeof(phdr.zeros));
uint32_t sum = 0; //calculated checksum
for(uint8_t i = 0; i < sizeof(phdr); i += 2) //calculate for pseudo IP header
{
sum += (((uint32_t)(((uint8_t*)&phdr)[i])) << 8) | (uint32_t)(((uint8_t*)&phdr)[i + 1]); //convert to 16-bit words and add to the sum
}
for(uint16_t i = IPV6_HEADER_SIZE; i < size; i += 2) //group data in 16-bit words (skip real IP header)
{
if(i == (ICMP_HEADER_CHECKSUM_POS + IPV6_HEADER_SIZE)) //skip checksum field itself (treat is a 0x0000)
continue;
sum += ((((uint32_t)buf[i]) << 8) | (uint32_t)buf[i + 1]); //convert to 16-bit words and add to the sum
}
sum = (sum & 0xFFFF) + (sum >> 16); //cut carry bits, shift them and add to the checksum
if(sum & 0x10000) //check if addition generated carry bit
{
sum &= 0xFFFF; //if so, remove it
sum++; //and add one to the checksum
}
sum = ~sum; //bitwise negate (one's complement)
buf[IPV6_HEADER_SIZE + ICMP_HEADER_CHECKSUM_POS] = (sum >> 8) & 0xFF; //store checksum
buf[IPV6_HEADER_SIZE + ICMP_HEADER_CHECKSUM_POS + 1] = sum & 0xFF;
return 0;
}
int ICMP_send(int s, uint8_t *data, int size, in_addr_t source, uint8_t type, uint8_t code, uint32_t rest)
{
uint8_t buf[2 * IPV4_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE]; //prepare buffer
if(size < (IPV4_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE)) //check if there is enough data to send ICMP message
{
PRINT(LOG_DEBUG, "Not enough data to send ICMP message\n");
return -1;
}
memcpy(buf, data, IPV4_HEADER_SIZE); //copy IP header
struct ip *hdr = (struct ip*)buf; //get header
//responding to the sender of the packet than caused an ICMP response
hdr->ip_dst.s_addr = hdr->ip_src.s_addr; //copy source address to destination address
hdr->ip_p = IPV4_HEADER_PROTO_ICMP; //ICMP protocol inside
hdr->ip_id = 0; //let kernel fill ID
hdr->ip_src.s_addr = source; //store soure IP
hdr->ip_hl = IPV4_HEADER_SIZE / 4; //to be sure
hdr->ip_v = IPVX_HEADER_VERSION_4; //to be sure
hdr->ip_off = 0;
hdr->ip_tos = 0;
hdr->ip_ttl = ICMP_DEFAULT_TTL;
struct icmphdr *icmp = (struct icmphdr*)(&(buf[IPV4_HEADER_SIZE])); //get ICMP header
icmp->type = type; //set type
icmp->code = code; //set code
icmp->un.gateway = rest; //set rest of data
icmp->checksum = 0; //zero-out before calculation
memcpy(&(buf[IPV4_HEADER_SIZE + ICMP_HEADER_SIZE]), data, IPV4_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE); //copy original packet header and first 8 bytes
icmp_checksum(&(buf[IPV4_HEADER_SIZE]), ICMP_HEADER_SIZE + IPV4_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE); //calculate and insert ICMP checksum
struct sockaddr_in dest; //prepare structure for sendto()
memset(&dest, 0, sizeof(dest));
dest.sin_family = AF_INET;
dest.sin_addr.s_addr = hdr->ip_dst.s_addr; //copy address
int sent = sendto(s, buf, 2 * IPV4_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE, 0, (struct sockaddr*)(&dest), sizeof(dest)); //send packet
if(sent < 0) //error
{
DEBUG(LOG_ERR, "ICMP packet TX failed");
return -1;
}
else if(sent != (2 * IPV4_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE)) //number of bytes actually sent is different than number of bytes to be sent
{
PRINT(LOG_WARNING, "ICMP packet TX problem: %d bytes to send, %d actually sent\n", 2 * IPV4_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE, sent);
return -1;
}
return 0;
}
int ICMP_send6(int s, uint8_t *data, int size, struct in6_addr source, uint8_t type, uint8_t code, uint32_t rest)
{
uint8_t buf[2 * IPV6_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE]; //prepare buffer
if(size < (IPV6_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE)) //check if there is enough data to send ICMP message
{
PRINT(LOG_ERR, "Not enough data to send ICMP message\n");
return -1;
}
memcpy(buf, data, IPV6_HEADER_SIZE); //copy IP header
struct ip6_hdr *hdr = (struct ip6_hdr*)buf; //get header
//responding to the sender of the packet than caused an ICMP response
hdr->ip6_dst = hdr->ip6_src; //copy source address to destination address
hdr->ip6_ctlun.ip6_un1.ip6_un1_nxt = IPV6_HEADER_PROTO_ICMP6; //ICMPv6 protocol inside
hdr->ip6_ctlun.ip6_un1.ip6_un1_flow = 0; //reset this field
hdr->ip6_src = source; //store soure IP
hdr->ip6_ctlun.ip6_un2_vfc = (IPVX_HEADER_VERSION_6 << 4); //to be sure
hdr->ip6_ctlun.ip6_un1.ip6_un1_hlim = ICMP_DEFAULT_TTL; //hop limit
hdr->ip6_ctlun.ip6_un1.ip6_un1_plen = htons(ICMP_HEADER_SIZE + IPV6_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE);
struct icmp6_hdr *icmp = (struct icmp6_hdr*)(&(buf[IPV6_HEADER_SIZE])); //get ICMP header
icmp->icmp6_type = type; //set type
icmp->icmp6_code = code; //set code
icmp->icmp6_dataun.icmp6_un_data32[0] = rest; //set rest of data
icmp->icmp6_cksum = 0; //zero-out before calculation
memcpy(&(buf[IPV6_HEADER_SIZE + ICMP_HEADER_SIZE]), data, IPV6_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE); //copy original packet header and first 8 bytes
icmp_checksum6(buf, ICMP_HEADER_SIZE + 2 * IPV6_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE); //calculate and insert ICMPv6 checksum
struct sockaddr_in6 dest; //prepare structure for sendto()
memset(&dest, 0, sizeof(dest));
dest.sin6_family = AF_INET6;
dest.sin6_addr = hdr->ip6_dst; //copy address
int sent = sendto(s, buf, 2 * IPV6_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE, 0, (struct sockaddr*)(&dest), sizeof(dest)); //send packet
if(sent < 0) //error
{
DEBUG(LOG_ERR, "ICMPv6 packet TX failed");
return -1;
}
else if(sent != (2 * IPV6_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE)) //number of bytes actually sent is different than number of bytes to be sent
{
PRINT(LOG_WARNING, "ICMPv6 packet TX problem: %d bytes to send, %d actually sent\n", 2 * IPV6_HEADER_SIZE + ICMP_HEADER_SIZE + ICMP_ADDITIONAL_DATA_SIZE, sent);
return -1;
}
return 0;
}