11 #include "nmsg_port_net.h"
26 #define REASM_IP_HASH_SIZE 1021U
45 struct reasm_id_ipv4 {
46 uint8_t ip_src[4], ip_dst[4];
55 struct reasm_id_ipv6 {
56 uint8_t ip_src[16], ip_dst[16];
62 struct reasm_id_ipv4 ipv4;
63 struct reasm_id_ipv6 ipv6;
67 struct reasm_frag_entry {
72 struct reasm_frag_entry *next;
83 struct reasm_ip_entry {
85 unsigned len, holes, frag_count, hash;
87 enum entry_state state;
88 enum reasm_proto protocol;
89 struct reasm_frag_entry *frags;
90 struct reasm_ip_entry *prev, *next;
91 struct reasm_ip_entry *time_prev, *time_next;
104 struct reasm_ip_entry *table[REASM_IP_HASH_SIZE];
105 struct reasm_ip_entry *time_first, *time_last;
106 unsigned waiting, max_waiting, timed_out, dropped_frags;
107 reasm_time_t timeout;
114 static unsigned reasm_ipv4_hash (
const struct reasm_id_ipv4 *
id);
115 static unsigned reasm_ipv6_hash (
const struct reasm_id_ipv6 *
id);
122 static bool add_fragment (
struct reasm_ip_entry *entry,
struct reasm_frag_entry *frag,
bool last_frag);
127 static bool is_complete (
struct reasm_ip_entry *entry);
132 static void assemble (
struct reasm_ip_entry *entry,
unsigned char *out_packet,
unsigned *output_len);
137 static void drop_entry (
struct reasm_ip *reasm,
struct reasm_ip_entry *entry);
138 static void free_entry (
struct reasm_ip_entry *entry);
143 static void process_timeouts (
struct reasm_ip *reasm, reasm_time_t now);
150 static struct reasm_frag_entry *frag_from_ipv6 (
const unsigned char *packet,
unsigned frag_hdr_offset, uint32_t *ip_id,
bool *last_frag);
155 static bool reasm_id_equal (
enum reasm_proto proto,
const union reasm_id *left,
const union reasm_id *right);
161 static struct reasm_frag_entry *parse_packet (
const unsigned char *packet,
unsigned len,
unsigned frag_hdr_offset,
enum reasm_proto *protocol,
union reasm_id *
id,
unsigned *hash,
bool *last_frag);
165 reasm_ipv4_hash (
const struct reasm_id_ipv4 *
id)
170 for (i = 0; i < 4; i++) {
171 hash = 37U * hash +
id->ip_src[i];
172 hash = 37U * hash +
id->ip_dst[i];
175 hash = 59U * hash +
id->ip_id;
177 hash = 47U * hash +
id->ip_proto;
184 reasm_ipv6_hash (
const struct reasm_id_ipv6 *
id)
189 for (i = 0; i < 16; i++) {
190 hash = 37U * hash +
id->ip_src[i];
191 hash = 37U * hash +
id->ip_dst[i];
194 hash = 59U * hash +
id->ip_id;
201 reasm_ip_next (
struct reasm_ip *reasm,
const unsigned char *packet,
unsigned len,
unsigned frag_hdr_offset, reasm_time_t timestamp,
unsigned char *out_packet,
unsigned *output_len)
203 enum reasm_proto proto;
207 struct reasm_frag_entry *frag;
208 struct reasm_ip_entry *entry;
210 process_timeouts (reasm, timestamp);
212 frag = parse_packet (packet, len, frag_hdr_offset, &proto, &
id, &hash, &last_frag);
218 hash %= REASM_IP_HASH_SIZE;
219 entry = reasm->table[hash];
220 while (entry != NULL && (proto != entry->protocol || !reasm_id_equal (proto, &
id, &entry->id)))
224 struct reasm_frag_entry *list_head;
226 entry = malloc (
sizeof (*entry));
232 list_head = malloc (
sizeof (*list_head));
233 if (list_head == NULL) {
239 memset(entry, 0,
sizeof *entry);
243 entry->frags = list_head;
245 entry->protocol = proto;
246 entry->timeout = timestamp + reasm->timeout;
247 entry->state = STATE_ACTIVE;
249 entry->next = reasm->table[hash];
250 entry->time_prev = reasm->time_last;
251 entry->time_next = NULL;
253 memset(list_head, 0,
sizeof *list_head);
255 list_head->offset = 0;
256 list_head->data_offset = 0;
257 list_head->data = NULL;
259 if (entry->next != NULL)
260 entry->next->prev = entry;
261 reasm->table[hash] = entry;
263 if (reasm->time_last != NULL)
264 reasm->time_last->time_next = entry;
266 reasm->time_first = entry;
267 reasm->time_last = entry;
270 if (reasm->waiting > reasm->max_waiting)
271 reasm->max_waiting = reasm->waiting;
274 if (entry->state != STATE_ACTIVE) {
275 reasm->dropped_frags++;
282 if (!add_fragment (entry, frag, last_frag)) {
283 entry->state = STATE_INVALID;
284 reasm->dropped_frags += entry->frag_count + 1;
291 if (!is_complete (entry)) {
296 assemble (entry, out_packet, output_len);
297 drop_entry (reasm, entry);
303 add_fragment (
struct reasm_ip_entry *entry,
struct reasm_frag_entry *frag,
bool last_frag)
305 bool fit_left, fit_right;
306 struct reasm_frag_entry *cur, *next;
324 if (!last_frag && (frag->len & 7) != 0)
327 if (entry->len != 0 && frag->len + frag->offset > entry->len)
336 entry->len = frag->offset + frag->len;
343 while (cur->next != NULL && cur->next->offset <= frag->offset)
350 if (cur->offset + cur->len > frag->offset)
352 else if (cur->offset + cur->len == frag->offset)
358 if (frag->offset + frag->len > next->offset)
360 else if (frag->offset + frag->len == next->offset)
367 if (frag->len != 0) {
368 frag->next = cur->next;
371 if (fit_left && fit_right)
373 else if (!fit_left && !fit_right)
384 if (last_frag && fit_left)
396 struct reasm_ip *reasm = malloc (
sizeof (*reasm));
400 memset (reasm, 0,
sizeof (*reasm));
406 reasm_ip_free (
struct reasm_ip *reasm)
408 while (reasm->time_first != NULL)
409 drop_entry (reasm, reasm->time_first);
415 is_complete (
struct reasm_ip_entry *entry)
417 return entry->holes == 0;
422 assemble (
struct reasm_ip_entry *entry,
unsigned char *out_packet,
unsigned *output_len)
424 struct reasm_frag_entry *frag = entry->frags->next;
425 unsigned offset0 = frag->data_offset;
427 switch (entry->protocol) {
437 if (entry->len + offset0 > *output_len) {
443 *output_len = entry->len + offset0;
446 memcpy (out_packet, frag->data, offset0);
449 while (frag != NULL) {
450 memcpy (out_packet + offset0 + frag->offset, frag->data + frag->data_offset, frag->len);
455 switch (entry->protocol) {
458 unsigned i, hl = 4 * ip_header->ip_hl;
460 ip_header->ip_len = htons (offset0 + entry->len);
461 ip_header->ip_off = 0;
462 ip_header->ip_sum = 0;
465 for (i = 0; i < hl; i += 2) {
466 uint16_t cur = (uint16_t) out_packet[i] << 8 | out_packet[i + 1];
468 if ((sum & 0x80000000) != 0)
469 sum = (sum & 0xffff) + (sum >> 16);
471 while ((sum >> 16) != 0)
472 sum = (sum & 0xffff) + (sum >> 16);
473 ip_header->ip_sum = htons (~sum);
477 struct ip6_hdr *ip6_header = (
struct ip6_hdr *) out_packet;
478 ip6_header->ip6_plen = htons (offset0 + entry->len - 40);
488 drop_entry (
struct reasm_ip *reasm,
struct reasm_ip_entry *entry)
490 if (entry->prev != NULL)
491 entry->prev->next = entry->next;
493 reasm->table[entry->hash] = entry->next;
495 if (entry->next != NULL)
496 entry->next->prev = entry->prev;
498 if (entry->time_prev != NULL)
499 entry->time_prev->time_next = entry->time_next;
501 reasm->time_first = entry->time_next;
503 if (entry->time_next != NULL)
504 entry->time_next->time_prev = entry->time_prev;
506 reasm->time_last = entry->time_prev;
515 free_entry (
struct reasm_ip_entry *entry)
517 struct reasm_frag_entry *frag = entry->frags, *next;
518 while (frag != NULL) {
520 if (frag->data != NULL)
531 reasm_ip_waiting (
const struct reasm_ip *reasm)
533 return reasm->waiting;
538 reasm_ip_max_waiting (
const struct reasm_ip *reasm)
540 return reasm->max_waiting;
545 reasm_ip_timed_out (
const struct reasm_ip *reasm)
547 return reasm->timed_out;
552 reasm_ip_dropped_frags (
const struct reasm_ip *reasm)
554 return reasm->dropped_frags;
559 reasm_ip_set_timeout (
struct reasm_ip *reasm, reasm_time_t timeout)
561 if (reasm->time_first != NULL)
564 reasm->timeout = timeout;
570 process_timeouts (
struct reasm_ip *reasm, reasm_time_t now)
572 while (reasm->time_first != NULL && reasm->time_first->timeout < now) {
574 drop_entry (reasm, reasm->time_first);
579 static struct reasm_frag_entry *
580 frag_from_ipv6 (
const unsigned char *packet,
unsigned frag_hdr_offset, uint32_t *ip_id,
bool *last_frag)
582 const struct ip6_hdr *ip6_header = (
const struct ip6_hdr *) packet;
583 unsigned offset = 40;
584 uint8_t nxt = ip6_header->ip6_nxt;
585 unsigned total_len = 40 + ntohs (ip6_header->ip6_plen);
586 unsigned last_nxt = offsetof (
struct ip6_hdr, ip6_nxt);
587 struct reasm_frag_entry *frag;
588 const struct ip6_frag *frag_header;
589 unsigned char *frag_data;
604 if (frag_hdr_offset != 0)
605 offset = frag_hdr_offset;
607 while (nxt == IPPROTO_HOPOPTS || nxt == IPPROTO_ROUTING || nxt == IPPROTO_DSTOPTS) {
610 if (offset + 2 > total_len)
613 exthdr_len = 8 + 8 * packet[offset + 1];
614 if (offset + exthdr_len > total_len)
617 nxt = packet[offset];
619 offset += exthdr_len;
622 if (nxt != IPPROTO_FRAGMENT)
626 if (offset + 8 > total_len)
629 frag = malloc (
sizeof (*frag));
633 frag_header = (
const struct ip6_frag *) (packet + offset);
636 frag_data = malloc (total_len);
637 if (frag_data == NULL)
639 memcpy (frag_data, packet, total_len);
649 frag_data[last_nxt] = frag_header->ip6f_nxt;
651 memset(frag, 0,
sizeof *frag);
652 frag->len = total_len - offset;
653 frag->data_offset = offset;
654 frag->offset = ntohs (frag_header->ip6f_offlg & IP6F_OFF_MASK);
655 frag->data = frag_data;
657 *ip_id = ntohl (frag_header->ip6f_ident);
658 *last_frag = (frag_header->ip6f_offlg & IP6F_MORE_FRAG) == 0;
665 reasm_id_equal (
enum reasm_proto proto,
const union reasm_id *left,
const union reasm_id *right)
669 return memcmp (left->ipv4.ip_src, right->ipv4.ip_src, 4) == 0
670 && memcmp (left->ipv4.ip_dst, right->ipv4.ip_dst, 4) == 0
671 && left->ipv4.ip_id == right->ipv4.ip_id
672 && left->ipv4.ip_proto == right->ipv4.ip_proto;
674 return memcmp (left->ipv6.ip_src, right->ipv6.ip_src, 16) == 0
675 && memcmp (left->ipv6.ip_dst, right->ipv6.ip_dst, 16) == 0
676 && left->ipv6.ip_id == right->ipv6.ip_id;
683 static struct reasm_frag_entry *
684 parse_packet (
const unsigned char *packet,
unsigned len,
unsigned frag_hdr_offset,
enum reasm_proto *protocol,
union reasm_id *
id,
unsigned *hash,
bool *last_frag)
687 struct reasm_frag_entry *frag = NULL;
689 switch (ip_header->ip_v) {
691 uint16_t offset = ntohs (ip_header->ip_off);
693 *protocol = PROTO_IPV4;
694 if (len >= (
unsigned) ntohs (ip_header->ip_len) &&
695 (offset & (IP_MF | IP_OFFMASK)) != 0)
697 unsigned pl_hl, pl_len, pl_off;
700 frag = malloc (
sizeof (*frag));
704 pl_hl = ip_header->ip_hl * 4;
705 pl_len = ntohs (ip_header->ip_len);
706 pl_off = (offset & IP_OFFMASK) * 8;
707 frag_data = malloc (pl_len);
708 if (frag_data == NULL)
710 memcpy (frag_data, packet, pl_len);
712 frag->len = pl_len - pl_hl;
713 frag->offset = pl_off;
714 frag->data_offset = ip_header->ip_hl * 4;
715 frag->data = frag_data;
717 *last_frag = (offset & IP_MF) == 0;
719 memcpy (id->ipv4.ip_src, &ip_header->ip_src, 4);
720 memcpy (id->ipv4.ip_dst, &ip_header->ip_dst, 4);
721 id->ipv4.ip_id = ntohs (ip_header->ip_id);
722 id->ipv4.ip_proto = ip_header->ip_p;
724 *hash = reasm_ipv4_hash (&id->ipv4);
730 const struct ip6_hdr *ip6_header =
731 (
const struct ip6_hdr *) packet;
732 *protocol = PROTO_IPV6;
733 if (len >= (
unsigned) ntohs (ip6_header->ip6_plen) + 40)
734 frag = frag_from_ipv6 (packet, frag_hdr_offset, &id->ipv6.ip_id, last_frag);
736 memcpy (id->ipv6.ip_src, &ip6_header->ip6_src, 16);
737 memcpy (id->ipv6.ip_dst, &ip6_header->ip6_dst, 16);
738 *hash = reasm_ipv6_hash (&id->ipv6);