| 1 |
/* SPDX-License-Identifier: LGPL-2.1-or-later */ |
| 2 |
|
| 3 |
#include <string.h> |
| 4 |
|
| 5 |
#include "iovec-util.h" |
| 6 |
#include "iovec-wrapper.h" |
| 7 |
#include "ip-util.h" |
| 8 |
#include "log.h" |
| 9 |
|
| 10 |
union iphdr_union { |
| 11 |
struct iphdr ip; |
| 12 |
uint8_t buf[15 * 4]; /* ip->ihl is 4 bits, hence max length is 15 * 4 */ |
| 13 |
}; |
| 14 |
|
| 15 |
struct udp_pseudo_header { |
| 16 |
be32_t saddr; |
| 17 |
be32_t daddr; |
| 18 |
uint8_t unused; |
| 19 |
uint8_t protocol; |
| 20 |
be16_t len; |
| 21 |
} _packed_; |
| 22 |
|
| 23 |
static uint64_t complement_sum(uint64_t a, uint64_t b) { |
| 24 |
/* This performs one's complement addition (end-around carry). See RFC1071. */ |
| 25 |
if (a <= UINT64_MAX - b) |
| 26 |
return a + b; |
| 27 |
|
| 28 |
return a - (UINT64_MAX - b); |
| 29 |
} |
| 30 |
|
| 31 |
static uint64_t checksum_iov(uint64_t sum, const struct iovec *iov) { |
| 32 |
assert(iov); |
| 33 |
|
| 34 |
for (struct iovec i = *iov; iovec_is_set(&i); iovec_inc(&i, sizeof(uint64_t))) { |
| 35 |
uint64_t t = 0; |
| 36 |
memcpy(&t, i.iov_base, MIN(i.iov_len, sizeof(uint64_t))); |
| 37 |
sum = complement_sum(sum, t); |
| 38 |
} |
| 39 |
|
| 40 |
return sum; |
| 41 |
} |
| 42 |
|
| 43 |
static uint16_t checksum_finalize(uint64_t sum) { |
| 44 |
while ((sum >> 16) != 0) |
| 45 |
sum = (sum & 0xffffu) + (sum >> 16); |
| 46 |
|
| 47 |
return ~sum; |
| 48 |
} |
| 49 |
|
| 50 |
uint16_t ip_checksum(const void *buf, size_t len) { |
| 51 |
/* See RFC1071 */ |
| 52 |
return checksum_finalize(checksum_iov(0, &IOVEC_MAKE(buf, len))); |
| 53 |
} |
| 54 |
|
| 55 |
static uint16_t iphdr_checksum(const union iphdr_union *ip) { |
| 56 |
assert(ip); |
| 57 |
return ip_checksum(ip, ip->ip.ihl * 4); |
| 58 |
} |
| 59 |
|
| 60 |
static uint16_t udphdr_checksum( |
| 61 |
be32_t saddr, |
| 62 |
be32_t daddr, |
| 63 |
const struct udphdr *udp, |
| 64 |
const struct iovec_wrapper *payload) { |
| 65 |
|
| 66 |
assert(udp); |
| 67 |
assert(payload); |
| 68 |
|
| 69 |
/* RFC 768 */ |
| 70 |
|
| 71 |
struct udp_pseudo_header pseudo = { |
| 72 |
.saddr = saddr, |
| 73 |
.daddr = daddr, |
| 74 |
.protocol = IPPROTO_UDP, |
| 75 |
.len = udp->len, |
| 76 |
}; |
| 77 |
|
| 78 |
uint64_t sum = 0; |
| 79 |
sum = checksum_iov(sum, &IOVEC_MAKE(&pseudo, sizeof(struct udp_pseudo_header))); |
| 80 |
sum = checksum_iov(sum, &IOVEC_MAKE(udp, sizeof(struct udphdr))); |
| 81 |
|
| 82 |
uint8_t buf[2] = {}; |
| 83 |
bool odd = false; |
| 84 |
FOREACH_ARRAY(i, payload->iovec, payload->count) { |
| 85 |
if (!iovec_is_set(i)) |
| 86 |
continue; |
| 87 |
|
| 88 |
struct iovec v = *i; |
| 89 |
if (odd) { |
| 90 |
buf[1] = *(uint8_t*) v.iov_base; |
| 91 |
sum = checksum_iov(sum, &IOVEC_MAKE(buf, 2)); |
| 92 |
iovec_inc(&v, 1); |
| 93 |
} |
| 94 |
|
| 95 |
odd = v.iov_len % 2; |
| 96 |
if (odd) { |
| 97 |
buf[0] = ((uint8_t*) v.iov_base)[v.iov_len - 1]; |
| 98 |
v.iov_len--; |
| 99 |
} |
| 100 |
sum = checksum_iov(sum, &v); |
| 101 |
} |
| 102 |
if (odd) { |
| 103 |
buf[1] = 0; |
| 104 |
sum = checksum_iov(sum, &IOVEC_MAKE(buf, 2)); |
| 105 |
} |
| 106 |
|
| 107 |
return checksum_finalize(sum); |
| 108 |
} |
| 109 |
|
| 110 |
int udp_packet_build( |
| 111 |
be32_t source_addr, |
| 112 |
uint16_t source_port, |
| 113 |
be32_t destination_addr, |
| 114 |
uint16_t destination_port, |
| 115 |
int ip_service_type, |
| 116 |
const struct iovec_wrapper *payload, |
| 117 |
struct iphdr *ret_iphdr, |
| 118 |
struct udphdr *ret_udphdr) { |
| 119 |
|
| 120 |
assert(payload); |
| 121 |
assert(ret_iphdr); |
| 122 |
assert(ret_udphdr); |
| 123 |
|
| 124 |
/* When ip_service_type is negative, IPTOS_CLASS_CS6 will be used. Otherwise, it must be a valid TOS, |
| 125 |
* hence must be in 0…255. Here, we only check its range. */ |
| 126 |
if (ip_service_type > UINT8_MAX) |
| 127 |
return -EINVAL; |
| 128 |
|
| 129 |
/* iphdr.tot_len is uint16_t, hence the total length must be <= UINT16_MAX. */ |
| 130 |
size_t len = iovw_size(payload); |
| 131 |
if (len > UDP_PAYLOAD_MAX_SIZE) |
| 132 |
return -E2BIG; |
| 133 |
|
| 134 |
union iphdr_union ip = { |
| 135 |
.ip.version = IPVERSION, |
| 136 |
.ip.ihl = sizeof(struct iphdr) / 4, |
| 137 |
.ip.tos = ip_service_type >= 0 ? ip_service_type : IPTOS_CLASS_CS6, |
| 138 |
.ip.tot_len = htobe16(sizeof(struct iphdr) + sizeof(struct udphdr) + len), |
| 139 |
.ip.ttl = IPDEFTTL, |
| 140 |
.ip.protocol = IPPROTO_UDP, |
| 141 |
.ip.saddr = source_addr, |
| 142 |
.ip.daddr = destination_addr, |
| 143 |
}; |
| 144 |
|
| 145 |
ip.ip.check = iphdr_checksum(&ip); |
| 146 |
|
| 147 |
struct udphdr udp = { |
| 148 |
.source = htobe16(source_port), |
| 149 |
.dest = htobe16(destination_port), |
| 150 |
.len = htobe16(sizeof(struct udphdr) + len), |
| 151 |
}; |
| 152 |
|
| 153 |
udp.check = udphdr_checksum(source_addr, destination_addr, &udp, payload); |
| 154 |
|
| 155 |
*ret_iphdr = ip.ip; |
| 156 |
*ret_udphdr = udp; |
| 157 |
return 0; |
| 158 |
} |
| 159 |
|
| 160 |
int udp_packet_verify( |
| 161 |
const struct iovec *packet, |
| 162 |
uint16_t port, |
| 163 |
bool checksum, |
| 164 |
struct iovec *ret_payload) { |
| 165 |
|
| 166 |
assert(packet); |
| 167 |
|
| 168 |
/* This verifies IP and UDP packet headers and optionally returns the UDP payload. */ |
| 169 |
|
| 170 |
/* IP */ |
| 171 |
if (packet->iov_len < sizeof(struct iphdr)) |
| 172 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 173 |
"IPv4: packet (%zu bytes) smaller than minimum IP header (%zu bytes), ignoring packet.", |
| 174 |
packet->iov_len, sizeof(struct iphdr)); |
| 175 |
|
| 176 |
const union iphdr_union *ip = (const union iphdr_union*) packet->iov_base; |
| 177 |
if (ip->ip.version != IPVERSION) |
| 178 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 179 |
"IPv4: packet is not IPv4, ignoring packet."); |
| 180 |
|
| 181 |
size_t iphdrlen = ip->ip.ihl * 4; |
| 182 |
if (iphdrlen < sizeof(struct iphdr)) |
| 183 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 184 |
"IPv4: IP header size (%zu bytes) smaller than minimum (%zu bytes), ignoring packet.", |
| 185 |
iphdrlen, sizeof(struct iphdr)); |
| 186 |
|
| 187 |
if (packet->iov_len < iphdrlen) |
| 188 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 189 |
"IPv4: packet (%zu bytes) smaller than IP header size (%zu bytes), ignoring packet.", |
| 190 |
packet->iov_len, iphdrlen); |
| 191 |
|
| 192 |
size_t totlen = be16toh(ip->ip.tot_len); |
| 193 |
if (totlen < iphdrlen) |
| 194 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 195 |
"IPv4: packet size (%zu bytes) by IP header is smaller than the IP header size (%zu), ignoring packet.", |
| 196 |
totlen, iphdrlen); |
| 197 |
if (packet->iov_len < totlen) |
| 198 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 199 |
"IPv4: packet (%zu bytes) smaller than expected (%zu) by IP header, ignoring packet.", |
| 200 |
packet->iov_len, totlen); |
| 201 |
|
| 202 |
if (ip->ip.protocol != IPPROTO_UDP) |
| 203 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 204 |
"IPv4: not UDP, ignoring packet."); |
| 205 |
|
| 206 |
if (iphdr_checksum(ip) != 0) |
| 207 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 208 |
"IPv4: invalid IP checksum, ignoring packet."); |
| 209 |
|
| 210 |
/* UDP */ |
| 211 |
if (totlen < iphdrlen + sizeof(struct udphdr)) |
| 212 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 213 |
"UDP: packet (%zu bytes) smaller than IP header + UDP header, ignoring packet.", |
| 214 |
totlen); |
| 215 |
|
| 216 |
const struct udphdr *udp = (const struct udphdr*) ((const uint8_t*) packet->iov_base + iphdrlen); |
| 217 |
size_t udplen = be16toh(udp->len); |
| 218 |
if (udplen < sizeof(struct udphdr)) |
| 219 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 220 |
"UDP: UDP datagram (%zu bytes) smaller than UDP header (%zu bytes), ignoring packet.", |
| 221 |
udplen, sizeof(struct udphdr)); |
| 222 |
|
| 223 |
if (totlen != iphdrlen + udplen) |
| 224 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 225 |
"UDP: packet length by IP header (%zu bytes) does not match with the one by UDP header " |
| 226 |
"(IP header %zu bytes + UDP %zu bytes = %zu bytes), ignoring packet.", |
| 227 |
totlen, iphdrlen, udplen, iphdrlen + udplen); |
| 228 |
|
| 229 |
if (be16toh(udp->dest) != port) |
| 230 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 231 |
"UDP: to port %u, which is not the expected port (%u), ignoring packet.", |
| 232 |
be16toh(udp->dest), port); |
| 233 |
|
| 234 |
/* Calculate the UDP payload length from the UDP header (udplen), rather than the input packet length |
| 235 |
* (len). The packet may contain garbage at the end. */ |
| 236 |
struct iovec payload = IOVEC_MAKE( |
| 237 |
(const uint8_t*) packet->iov_base + iphdrlen + sizeof(struct udphdr), |
| 238 |
udplen - sizeof(struct udphdr)); |
| 239 |
if (checksum && udp->check != 0 && |
| 240 |
udphdr_checksum(ip->ip.saddr, ip->ip.daddr, udp, |
| 241 |
&(struct iovec_wrapper) { |
| 242 |
.iovec = &payload, |
| 243 |
.count = 1, |
| 244 |
}) != 0) |
| 245 |
return log_debug_errno(SYNTHETIC_ERRNO(EBADMSG), |
| 246 |
"UDP: invalid UDP checksum, ignoring packet."); |
| 247 |
|
| 248 |
if (ret_payload) |
| 249 |
*ret_payload = payload; |
| 250 |
return 0; |
| 251 |
} |