27 #include "nmsg_port_net.h"
45 #define REASM_IP_HASH_SIZE 1021U
56 struct reasm_ip_entry *table[REASM_IP_HASH_SIZE];
57 struct reasm_ip_entry *time_first, *time_last;
58 unsigned waiting, max_waiting, timed_out, dropped_frags;
59 struct timespec timeout;
65 static unsigned reasm_ipv4_hash(
const struct reasm_id_ipv4 *
id);
66 static unsigned reasm_ipv6_hash(
const struct reasm_id_ipv6 *
id);
68 static void remove_entry(
struct reasm_ip *reasm,
struct reasm_ip_entry *entry);
73 static void process_timeouts(
struct reasm_ip *reasm,
const struct timespec *now);
80 static struct reasm_frag_entry *frag_from_ipv6(
const uint8_t *packet,
81 uint32_t *ip_id,
bool *last_frag);
86 static bool reasm_id_equal(
enum reasm_proto proto,
87 const union reasm_id *left,
const union reasm_id *right);
90 reasm_ipv4_hash(
const struct reasm_id_ipv4 *
id) {
94 for (i = 0; i < 4; i++) {
95 hash = 37U * hash +
id->ip_src[i];
96 hash = 37U * hash +
id->ip_dst[i];
99 hash = 59U * hash +
id->ip_id;
101 hash = 47U * hash +
id->ip_proto;
107 reasm_ipv6_hash(
const struct reasm_id_ipv6 *
id) {
111 for (i = 0; i < 16; i++) {
112 hash = 37U * hash +
id->ip_src[i];
113 hash = 37U * hash +
id->ip_dst[i];
116 hash = 59U * hash +
id->ip_id;
122 reasm_ip_next(
struct reasm_ip *reasm,
const uint8_t *packet,
unsigned len,
123 const struct timespec *timestamp,
struct reasm_ip_entry **out_entry)
125 enum reasm_proto proto;
129 struct reasm_frag_entry *frag;
130 struct reasm_ip_entry *entry;
132 process_timeouts(reasm, timestamp);
134 frag = reasm_parse_packet(packet, len, timestamp, &proto, &
id, &hash, &last_frag);
140 hash %= REASM_IP_HASH_SIZE;
141 entry = reasm->table[hash];
142 while (entry != NULL &&
143 (proto != entry->protocol ||
144 !reasm_id_equal(proto, &
id, &entry->id)))
150 struct reasm_frag_entry *list_head;
152 entry = malloc(
sizeof(*entry));
158 list_head = malloc(
sizeof(*list_head));
159 if (list_head == NULL) {
165 memset(entry, 0,
sizeof(*entry));
169 entry->frags = list_head;
171 entry->protocol = proto;
172 entry->timeout.tv_sec = timestamp->tv_sec + reasm->timeout.tv_sec;
173 entry->state = STATE_ACTIVE;
175 entry->next = reasm->table[hash];
176 entry->time_prev = reasm->time_last;
177 entry->time_next = NULL;
179 memset(list_head, 0,
sizeof(*list_head));
181 list_head->offset = 0;
182 list_head->data_offset = 0;
183 list_head->data = NULL;
185 if (entry->next != NULL)
186 entry->next->prev = entry;
187 reasm->table[hash] = entry;
189 if (reasm->time_last != NULL)
190 reasm->time_last->time_next = entry;
192 reasm->time_first = entry;
193 reasm->time_last = entry;
196 if (reasm->waiting > reasm->max_waiting)
197 reasm->max_waiting = reasm->waiting;
200 if (entry->state != STATE_ACTIVE) {
201 reasm->dropped_frags++;
208 if (!reasm_add_fragment(entry, frag, last_frag)) {
209 entry->state = STATE_INVALID;
210 reasm->dropped_frags += entry->frag_count + 1;
217 if (!reasm_is_complete(entry)) {
222 remove_entry(reasm, entry);
228 reasm_add_fragment(
struct reasm_ip_entry *entry,
struct reasm_frag_entry *frag,
bool last_frag) {
229 bool fit_left, fit_right;
230 struct reasm_frag_entry *cur, *next;
248 if (!last_frag && (frag->len & 7) != 0)
251 if (entry->len != 0 && frag->len + frag->offset > entry->len) {
262 entry->len = frag->offset + frag->len;
269 while (cur->next != NULL && cur->next->offset <= frag->offset)
276 if (cur->offset + cur->len > frag->offset) {
279 }
else if (cur->offset + cur->len == frag->offset) {
288 if (frag->offset + frag->len > next->offset) {
292 else if (frag->offset + frag->len == next->offset)
299 if (frag->len != 0) {
300 frag->next = cur->next;
303 if (fit_left && fit_right)
305 else if (!fit_left && !fit_right)
316 if (last_frag && fit_left)
325 struct reasm_ip *reasm = malloc(
sizeof(*reasm));
329 memset(reasm, 0,
sizeof(*reasm));
334 reasm_ip_free(
struct reasm_ip *reasm) {
335 while (reasm->time_first != NULL) {
336 struct reasm_ip_entry *entry;
338 entry = reasm->time_first;
339 remove_entry(reasm, entry);
340 reasm_free_entry(entry);
346 reasm_is_complete(
struct reasm_ip_entry *entry) {
347 return (entry->holes == 0);
351 reasm_assemble(
struct reasm_ip_entry *entry, uint8_t *out_packet,
size_t *output_len) {
352 struct reasm_frag_entry *frag = entry->frags->next;
353 unsigned offset0 = frag->data_offset;
355 switch (entry->protocol) {
365 if (entry->len + offset0 > *output_len) {
371 *output_len = entry->len + offset0;
374 memcpy(out_packet, frag->data, offset0);
375 if (entry->protocol == PROTO_IPV6)
376 out_packet[frag->last_nxt] = frag->ip6f_nxt;
379 while (frag != NULL) {
380 memcpy(out_packet + offset0 + frag->offset,
381 frag->data + frag->data_offset,
387 switch (entry->protocol) {
390 unsigned i, hl = 4 * ip_header->ip_hl;
392 ip_header->ip_len = htons(offset0 + entry->len);
393 ip_header->ip_off = 0;
394 ip_header->ip_sum = 0;
397 for (i = 0; i < hl; i += 2) {
398 uint16_t cur = (uint16_t) out_packet[i] << 8 | out_packet[i + 1];
400 if ((sum & 0x80000000) != 0)
401 sum = (sum & 0xffff) + (sum >> 16);
403 while ((sum >> 16) != 0)
404 sum = (sum & 0xffff) + (sum >> 16);
405 ip_header->ip_sum = htons(~sum);
409 uint16_t plen = offset0 + entry->len - 40;
410 store_net16(out_packet + offsetof(
struct ip6_hdr, ip6_plen), plen);
419 remove_entry(
struct reasm_ip *reasm,
struct reasm_ip_entry *entry) {
420 if (entry->prev != NULL)
421 entry->prev->next = entry->next;
423 reasm->table[entry->hash] = entry->next;
425 if (entry->next != NULL)
426 entry->next->prev = entry->prev;
428 if (entry->time_prev != NULL)
429 entry->time_prev->time_next = entry->time_next;
431 reasm->time_first = entry->time_next;
433 if (entry->time_next != NULL)
434 entry->time_next->time_prev = entry->time_prev;
436 reasm->time_last = entry->time_prev;
442 reasm_free_entry(
struct reasm_ip_entry *entry) {
443 struct reasm_frag_entry *frag = entry->frags, *next;
444 while (frag != NULL) {
446 if (frag->data != NULL)
455 reasm_ip_waiting(
const struct reasm_ip *reasm) {
456 return (reasm->waiting);
460 reasm_ip_max_waiting(
const struct reasm_ip *reasm) {
461 return (reasm->max_waiting);
465 reasm_ip_timed_out(
const struct reasm_ip *reasm) {
466 return (reasm->timed_out);
470 reasm_ip_dropped_frags(
const struct reasm_ip *reasm) {
471 return (reasm->dropped_frags);
475 reasm_ip_set_timeout(
struct reasm_ip *reasm,
const struct timespec *timeout) {
476 if (reasm->time_first != NULL)
478 memcpy(&reasm->timeout, timeout,
sizeof(*timeout));
483 process_timeouts(
struct reasm_ip *reasm,
const struct timespec *now) {
484 while (reasm->time_first != NULL &&
485 reasm->time_first->timeout.tv_sec < now->tv_sec)
487 struct reasm_ip_entry *entry;
489 entry = reasm->time_first;
490 remove_entry(reasm, entry);
491 reasm_free_entry(entry);
496 static struct reasm_frag_entry *
497 frag_from_ipv6(
const uint8_t *packet, uint32_t *ip_id,
bool *last_frag) {
498 unsigned offset = 40;
501 unsigned last_nxt = offsetof(
struct ip6_hdr, ip6_nxt);
502 struct reasm_frag_entry *frag;
505 nxt = packet[offsetof(
struct ip6_hdr, ip6_nxt)];
506 load_net16(packet + offsetof(
struct ip6_hdr, ip6_plen), &total_len);
522 while (nxt == IPPROTO_HOPOPTS || nxt == IPPROTO_ROUTING || nxt == IPPROTO_DSTOPTS) {
525 if (offset + 2 > total_len) {
530 exthdr_len = 8 + 8 * packet[offset + 1];
531 if (offset + exthdr_len > total_len) {
536 nxt = packet[offset];
538 offset += exthdr_len;
541 if (nxt != IPPROTO_FRAGMENT)
544 if (offset + 8 > total_len) {
549 frag = malloc(
sizeof(*frag));
553 struct ip6_frag frag_hdr;
554 memcpy(&frag_hdr, packet + offset,
sizeof(
struct ip6_frag));
557 frag_data = malloc(total_len);
558 if (frag_data == NULL)
560 memcpy(frag_data, packet, total_len);
562 memset(frag, 0,
sizeof(*frag));
563 frag->last_nxt = last_nxt;
564 frag->ip6f_nxt = frag_hdr.ip6f_nxt;
565 frag->len = total_len - offset;
566 frag->data_offset = offset;
567 frag->offset = ntohs(frag_hdr.ip6f_offlg & IP6F_OFF_MASK);
568 frag->data = frag_data;
570 *ip_id = ntohl(frag_hdr.ip6f_ident);
571 *last_frag = (frag_hdr.ip6f_offlg & IP6F_MORE_FRAG) == 0;
577 reasm_id_equal(
enum reasm_proto proto,
const union reasm_id *left,
const union reasm_id *right)
581 return (memcmp(left->ipv4.ip_src, right->ipv4.ip_src, 4) == 0 &&
582 memcmp(left->ipv4.ip_dst, right->ipv4.ip_dst, 4) == 0 &&
583 left->ipv4.ip_id == right->ipv4.ip_id &&
584 left->ipv4.ip_proto == right->ipv4.ip_proto);
586 return (memcmp(left->ipv6.ip_src, right->ipv6.ip_src, 16) == 0 &&
587 memcmp(left->ipv6.ip_dst, right->ipv6.ip_dst, 16) == 0 &&
588 left->ipv6.ip_id == right->ipv6.ip_id);
594 struct reasm_frag_entry *
595 reasm_parse_packet(
const uint8_t *packet,
unsigned len,
596 const struct timespec *ts,
597 enum reasm_proto *protocol,
union reasm_id *
id,
598 unsigned *hash,
bool *last_frag)
601 struct reasm_frag_entry *frag = NULL;
603 switch (ip_header->ip_v) {
605 uint16_t offset = ntohs(ip_header->ip_off);
607 *protocol = PROTO_IPV4;
608 if (len >= (
unsigned) ntohs(ip_header->ip_len) &&
609 (offset & (IP_MF | IP_OFFMASK)) != 0)
611 unsigned pl_hl, pl_len, pl_off;
614 frag = malloc(
sizeof(*frag));
618 pl_hl = ip_header->ip_hl * 4;
619 pl_len = ntohs(ip_header->ip_len);
620 pl_off = (offset & IP_OFFMASK) * 8;
621 frag_data = malloc(pl_len);
622 if (frag_data == NULL)
624 memcpy(frag_data, packet, pl_len);
626 frag->len = pl_len - pl_hl;
627 frag->offset = pl_off;
628 frag->data_offset = ip_header->ip_hl * 4;
629 frag->data = frag_data;
631 *last_frag = (offset & IP_MF) == 0;
633 memcpy(id->ipv4.ip_src, &ip_header->ip_src, 4);
634 memcpy(id->ipv4.ip_dst, &ip_header->ip_dst, 4);
635 id->ipv4.ip_id = ntohs(ip_header->ip_id);
636 id->ipv4.ip_proto = ip_header->ip_p;
638 *hash = reasm_ipv4_hash(&id->ipv4);
645 load_net16(packet + offsetof(
struct ip6_hdr, ip6_plen), &plen);
646 *protocol = PROTO_IPV6;
647 if (len >= plen + 40U)
648 frag = frag_from_ipv6(packet, &id->ipv6.ip_id, last_frag);
650 memcpy(id->ipv6.ip_src,
651 packet + offsetof(
struct ip6_hdr, ip6_src), 16);
652 memcpy(id->ipv6.ip_dst,
653 packet + offsetof(
struct ip6_hdr, ip6_dst), 16);
654 *hash = reasm_ipv6_hash(&id->ipv6);
664 memcpy(&frag->ts, ts,
sizeof(*ts));