From fecdbd6b17e438f2e2ebd1b56e5745c6046e4ad0 Mon Sep 17 00:00:00 2001 From: Vladimir 'phcoder' Serbinenko Date: Sat, 9 Jul 2011 00:27:27 +0200 Subject: [PATCH] support ip fragmentation --- grub-core/lib/priority_queue.c | 7 + grub-core/net/arp.c | 4 +- grub-core/net/ethernet.c | 2 + grub-core/net/ip.c | 414 +++++++++++++++++++++++++++------ grub-core/net/netbuff.c | 3 +- grub-core/net/tftp.c | 20 +- grub-core/net/udp.c | 2 + include/grub/net.h | 4 + include/grub/net/netbuff.h | 16 +- 9 files changed, 374 insertions(+), 98 deletions(-) diff --git a/grub-core/lib/priority_queue.c b/grub-core/lib/priority_queue.c index 5b02ef381..772b7b854 100644 --- a/grub-core/lib/priority_queue.c +++ b/grub-core/lib/priority_queue.c @@ -104,6 +104,13 @@ grub_priority_queue_top (grub_priority_queue_t pq) return element (pq, 0); } +void +grub_priority_queue_destroy (grub_priority_queue_t pq) +{ + grub_free (pq->els); + grub_free (pq); +} + grub_priority_queue_t grub_priority_queue_new (grub_size_t elsize, grub_comparator_t cmp) diff --git a/grub-core/net/arp.c b/grub-core/net/arp.c index d726f2c3a..b45bd4c9f 100644 --- a/grub-core/net/arp.c +++ b/grub-core/net/arp.c @@ -38,7 +38,7 @@ grub_net_arp_resolve (struct grub_net_network_level_interface *inf, struct grub_net_buff nb; struct arphdr *arp_header; grub_net_link_level_address_t target_hw_addr; - char *aux, arp_data[128]; + grub_uint8_t *aux, arp_data[128]; grub_err_t err; int i; @@ -74,7 +74,7 @@ grub_net_arp_resolve (struct grub_net_network_level_interface *inf, arp_header->hln = 6; arp_header->pln = 4; arp_header->op = grub_cpu_to_be16 (ARP_REQUEST); - aux = (char *) arp_header + sizeof (*arp_header); + aux = (grub_uint8_t *) arp_header + sizeof (*arp_header); /* Sender hardware address. */ grub_memcpy (aux, &inf->hwaddress.mac, 6); diff --git a/grub-core/net/ethernet.c b/grub-core/net/ethernet.c index acd33bcf6..c368eda54 100644 --- a/grub-core/net/ethernet.c +++ b/grub-core/net/ethernet.c @@ -57,6 +57,8 @@ send_ethernet_packet (struct grub_net_network_level_interface *inf, struct etherhdr *eth; grub_err_t err; + COMPILE_TIME_ASSERT (sizeof (*eth) < GRUB_NET_MAX_LINK_HEADER_SIZE); + err = grub_netbuff_push (nb, sizeof (*eth)); if (err) return err; diff --git a/grub-core/net/ip.c b/grub-core/net/ip.c index eaecc9ea6..9144eabc6 100644 --- a/grub-core/net/ip.c +++ b/grub-core/net/ip.c @@ -24,6 +24,8 @@ #include #include #include +#include +#include struct iphdr { grub_uint8_t verhdrlen; @@ -56,6 +58,39 @@ struct ip6hdr grub_uint8_t daddr[16]; } __attribute__ ((packed)); +static int +cmp (const void *a__, const void *b__) +{ + struct grub_net_buff *a_ = *(struct grub_net_buff **) a__; + struct grub_net_buff *b_ = *(struct grub_net_buff **) b__; + struct iphdr *a = (struct iphdr *) a_->data; + struct iphdr *b = (struct iphdr *) b_->data; + /* We want the first elements to be on top. */ + if ((grub_be_to_cpu16 (a->frags) & OFFSET_MASK) + < (grub_be_to_cpu16 (b->frags) & OFFSET_MASK)) + return +1; + if ((grub_be_to_cpu16 (a->frags) & OFFSET_MASK) + > (grub_be_to_cpu16 (b->frags) & OFFSET_MASK)) + return -1; + return 0; +} + +struct reassemble +{ + struct reassemble *next; + grub_uint32_t source; + grub_uint32_t dest; + grub_uint16_t id; + grub_uint8_t proto; + grub_uint64_t last_time; + grub_priority_queue_t pq; + grub_uint8_t *asm_buffer; + grub_size_t total_len; + grub_size_t cur_ptr; +}; + +struct reassemble *reassembles; + grub_uint16_t grub_net_ip_chksum (void *ipv, grub_size_t len) { @@ -78,6 +113,72 @@ grub_net_ip_chksum (void *ipv, grub_size_t len) return grub_cpu_to_be16 ((~sum) & 0x0000FFFF); } +static int id = 0x2400; + +static grub_err_t +send_fragmented (struct grub_net_network_level_interface * inf, + const grub_net_network_level_address_t * target, + struct grub_net_buff * nb, + grub_net_ip_protocol_t proto, + grub_net_link_level_address_t ll_target_addr) +{ + grub_size_t off = 0; + grub_size_t fraglen; + grub_err_t err; + + fraglen = (inf->card->mtu - sizeof (struct iphdr)) & ~7; + id++; + + while (nb->tail - nb->data) + { + grub_size_t len = fraglen; + struct grub_net_buff *nb2; + struct iphdr *iph; + + if ((grub_ssize_t) len > nb->tail - nb->data) + len = nb->tail - nb->data; + nb2 = grub_netbuff_alloc (fraglen + sizeof (struct iphdr) + + GRUB_NET_MAX_LINK_HEADER_SIZE); + if (!nb2) + return grub_errno; + err = grub_netbuff_reserve (nb2, GRUB_NET_MAX_LINK_HEADER_SIZE); + if (err) + return err; + err = grub_netbuff_put (nb2, sizeof (struct iphdr)); + if (err) + return err; + + iph = (struct iphdr *) nb2->data; + iph->verhdrlen = ((4 << 4) | 5); + iph->service = 0; + iph->len = grub_cpu_to_be16 (len + sizeof (struct iphdr)); + iph->ident = grub_cpu_to_be16 (id); + iph->frags = grub_cpu_to_be16 (off | (((grub_ssize_t) len + == nb->tail - nb->data) + ? 0 : MORE_FRAGMENTS)); + iph->ttl = 0xff; + iph->protocol = proto; + iph->src = inf->address.ipv4; + iph->dest = target->ipv4; + off += len / 8; + + iph->chksum = 0; + iph->chksum = grub_net_ip_chksum ((void *) nb2->data, sizeof (*iph)); + err = grub_netbuff_put (nb2, len); + if (err) + return err; + grub_memcpy (iph + 1, nb->data, len); + err = grub_netbuff_pull (nb, len); + if (err) + return err; + err = send_ethernet_packet (inf, nb2, ll_target_addr, + GRUB_NET_ETHERTYPE_IP); + if (err) + return err; + } + return GRUB_ERR_NONE; +} + grub_err_t grub_net_send_ip_packet (struct grub_net_network_level_interface * inf, const grub_net_network_level_address_t * target, @@ -85,10 +186,19 @@ grub_net_send_ip_packet (struct grub_net_network_level_interface * inf, grub_net_ip_protocol_t proto) { struct iphdr *iph; - static int id = 0x2400; grub_net_link_level_address_t ll_target_addr; grub_err_t err; + COMPILE_TIME_ASSERT (GRUB_NET_OUR_IPV4_HEADER_SIZE == sizeof (*iph)); + + /* Determine link layer target address via ARP. */ + err = grub_net_arp_resolve (inf, target, &ll_target_addr); + if (err) + return err; + + if (nb->tail - nb->data + sizeof (struct iphdr) > inf->card->mtu) + return send_fragmented (inf, target, nb, proto, ll_target_addr); + grub_netbuff_push (nb, sizeof (*iph)); iph = (struct iphdr *) nb->data; @@ -105,88 +215,26 @@ grub_net_send_ip_packet (struct grub_net_network_level_interface * inf, iph->chksum = 0; iph->chksum = grub_net_ip_chksum ((void *) nb->data, sizeof (*iph)); - /* Determine link layer target address via ARP. */ - err = grub_net_arp_resolve (inf, target, &ll_target_addr); - if (err) - return err; return send_ethernet_packet (inf, nb, ll_target_addr, GRUB_NET_ETHERTYPE_IP); } -grub_err_t -grub_net_recv_ip_packets (struct grub_net_buff * nb, - const struct grub_net_card * card, - const grub_net_link_level_address_t * hwaddress) +static grub_err_t +handle_dgram (struct grub_net_buff *nb, + const struct grub_net_card *card, + const grub_net_link_level_address_t *hwaddress, + grub_net_ip_protocol_t proto, grub_uint32_t src, + grub_uint32_t dst) { - struct iphdr *iph = (struct iphdr *) nb->data; - grub_err_t err; struct grub_net_network_level_interface *inf = NULL; + grub_err_t err; grub_net_network_level_address_t source; - if (((grub_addr_t) nb->data) & 3) - grub_fatal ("unaligned %p\n", nb->data); - - if ((iph->verhdrlen >> 4) != 4) - { - grub_dprintf ("net", "Bad IP version: %d\n", (iph->verhdrlen >> 4)); - grub_netbuff_free (nb); - return GRUB_ERR_NONE; - } - - if ((iph->verhdrlen & 0xf) < 5) - { - grub_dprintf ("net", "IP header too short: %d\n", - (iph->verhdrlen & 0xf)); - grub_netbuff_free (nb); - return GRUB_ERR_NONE; - } - - err = grub_netbuff_pull (nb, (iph->verhdrlen & 0xf) * sizeof (grub_uint32_t)); - if (err) - { - grub_netbuff_free (nb); - return err; - } - - /* Check size*/ - { - grub_size_t expected_size = grub_be_to_cpu16 (iph->len); - grub_size_t actual_size = (nb->tail - nb->data - + (iph->verhdrlen & 0xf) - * sizeof (grub_uint32_t)); - if (actual_size > expected_size) - { - err = grub_netbuff_unput (nb, actual_size - expected_size); - if (err) - { - grub_netbuff_free (nb); - return err; - } - } - if (actual_size < expected_size) - { - grub_dprintf ("net", "Cut IP packet actual: %" PRIuGRUB_SIZE - ", expected %" PRIuGRUB_SIZE "\n", actual_size, - expected_size); - grub_netbuff_free (nb); - return GRUB_ERR_NONE; - } - } - - /* Fragmented packet. Bad. */ - if (((grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS) != 0) - || (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) != 0) - { - /* FIXME. */ - grub_netbuff_free (nb); - return GRUB_ERR_NONE; - } - /* DHCP needs special treatment since we don't know IP yet. */ { struct udphdr *udph; udph = (struct udphdr *) nb->data; - if (iph->protocol == GRUB_NET_IP_UDP && grub_be_to_cpu16 (udph->dst) == 68) + if (proto == GRUB_NET_IP_UDP && grub_be_to_cpu16 (udph->dst) == 68) { FOR_NET_NETWORK_LEVEL_INTERFACES (inf) if (inf->card == card @@ -209,7 +257,7 @@ grub_net_recv_ip_packets (struct grub_net_buff * nb, { if (inf->card == card && inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4 - && inf->address.ipv4 == iph->dest + && inf->address.ipv4 == dst && grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0) break; } @@ -221,9 +269,9 @@ grub_net_recv_ip_packets (struct grub_net_buff * nb, } source.type = GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4; - source.ipv4 = iph->src; + source.ipv4 = src; - switch (iph->protocol) + switch (proto) { case GRUB_NET_IP_UDP: return grub_net_recv_udp_packet (nb, inf, &source); @@ -233,6 +281,224 @@ grub_net_recv_ip_packets (struct grub_net_buff * nb, grub_netbuff_free (nb); break; } + return GRUB_ERR_NONE; +} + +static void +free_rsm (struct reassemble *rsm) +{ + struct grub_net_buff **nb; + while ((nb = grub_priority_queue_top (rsm->pq))) + { + grub_netbuff_free (*nb); + grub_priority_queue_pop (rsm->pq); + } + grub_free (rsm->asm_buffer); + grub_priority_queue_destroy (rsm->pq); +} + +static void +free_old_fragments (void) +{ + struct reassemble *rsm, **prev; + grub_uint64_t limit_time = grub_get_time_ms () - 90000; + + for (prev = &reassembles, rsm = *prev; rsm; prev = &rsm->next, rsm = *prev) + if (rsm->last_time < limit_time) + { + *prev = rsm->next; + free_rsm (rsm); + } +} + +grub_err_t +grub_net_recv_ip_packets (struct grub_net_buff * nb, + const struct grub_net_card * card, + const grub_net_link_level_address_t * hwaddress) +{ + struct iphdr *iph = (struct iphdr *) nb->data; + grub_err_t err; + struct reassemble *rsm, **prev; + + if ((iph->verhdrlen >> 4) != 4) + { + grub_dprintf ("net", "Bad IP version: %d\n", (iph->verhdrlen >> 4)); + grub_netbuff_free (nb); + return GRUB_ERR_NONE; + } + + if ((iph->verhdrlen & 0xf) < 5) + { + grub_dprintf ("net", "IP header too short: %d\n", + (iph->verhdrlen & 0xf)); + grub_netbuff_free (nb); + return GRUB_ERR_NONE; + } + + if (nb->tail - nb->data < (grub_ssize_t) ((iph->verhdrlen & 0xf) + * sizeof (grub_uint32_t))) + { + grub_dprintf ("net", "IP packet too short: %d\n", + (iph->verhdrlen & 0xf)); + grub_netbuff_free (nb); + return GRUB_ERR_NONE; + } + + /* Check size. */ + { + grub_size_t expected_size = grub_be_to_cpu16 (iph->len); + grub_size_t actual_size = (nb->tail - nb->data); + if (actual_size > expected_size) + { + err = grub_netbuff_unput (nb, actual_size - expected_size); + if (err) + { + grub_netbuff_free (nb); + return err; + } + } + if (actual_size < expected_size) + { + grub_dprintf ("net", "Cut IP packet actual: %" PRIuGRUB_SIZE + ", expected %" PRIuGRUB_SIZE "\n", actual_size, + expected_size); + grub_netbuff_free (nb); + return GRUB_ERR_NONE; + } + } + + /* Unfragmented packet. Good. */ + if (((grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS) == 0) + && (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) == 0) + { + err = grub_netbuff_pull (nb, ((iph->verhdrlen & 0xf) + * sizeof (grub_uint32_t))); + if (err) + { + grub_netbuff_free (nb); + return err; + } + return handle_dgram (nb, card, hwaddress, iph->protocol, + iph->src, iph->dest); + } + + for (prev = &reassembles, rsm = *prev; rsm; prev = &rsm->next, rsm = *prev) + if (rsm->source == iph->src && rsm->dest == iph->dest + && rsm->id == iph->ident && rsm->proto == iph->protocol) + break; + if (!rsm) + { + rsm = grub_malloc (sizeof (*rsm)); + if (!rsm) + return grub_errno; + rsm->source = iph->src; + rsm->dest = iph->dest; + rsm->id = iph->ident; + rsm->proto = iph->protocol; + rsm->next = reassembles; + reassembles = rsm; + prev = &reassembles; + rsm->pq = grub_priority_queue_new (sizeof (struct grub_net_buff **), cmp); + if (!rsm->pq) + { + grub_free (rsm); + return grub_errno; + } + rsm->asm_buffer = 0; + rsm->total_len = 0; + rsm->cur_ptr = 0; + } + + rsm->last_time = grub_get_time_ms (); + free_old_fragments (); + + err = grub_priority_queue_push (rsm->pq, &nb); + if (err) + return err; + + if (!(grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS)) + { + rsm->total_len = (8 * (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) + + (nb->tail - nb->data)); + rsm->total_len -= ((iph->verhdrlen & 0xf) * sizeof (grub_uint32_t)); + rsm->asm_buffer = grub_zalloc (rsm->total_len); + if (!rsm->asm_buffer) + { + *prev = rsm->next; + free_rsm (rsm); + return grub_errno; + } + } + if (!rsm->asm_buffer) + return GRUB_ERR_NONE; + + while (1) + { + struct grub_net_buff **nb_top_p, *nb_top; + grub_size_t copy; + grub_uint8_t *res; + grub_size_t res_len; + struct grub_net_buff *ret; + grub_net_ip_protocol_t proto; + grub_uint32_t src; + grub_uint32_t dst; + + nb_top_p = grub_priority_queue_top (rsm->pq); + if (!nb_top_p) + return GRUB_ERR_NONE; + nb_top = *nb_top_p; + grub_priority_queue_pop (rsm->pq); + iph = (struct iphdr *) nb_top->data; + err = grub_netbuff_pull (nb_top, ((iph->verhdrlen & 0xf) + * sizeof (grub_uint32_t))); + if (err) + { + grub_netbuff_free (nb_top); + return err; + } + if (rsm->cur_ptr < (grub_size_t) 8 * (grub_be_to_cpu16 (iph->frags) + & OFFSET_MASK)) + return GRUB_ERR_NONE; + + rsm->cur_ptr = (8 * (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) + + (nb_top->tail - nb_top->head)); + if ((grub_size_t) 8 * (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) + >= rsm->total_len) + { + grub_netbuff_free (nb_top); + continue; + } + copy = nb_top->tail - nb_top->data; + if (rsm->total_len - 8 * (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) + < copy) + copy = rsm->total_len - 8 * (grub_be_to_cpu16 (iph->frags) + & OFFSET_MASK); + grub_memcpy (&rsm->asm_buffer[8 * (grub_be_to_cpu16 (iph->frags) + & OFFSET_MASK)], + nb_top->data, copy); + + if ((grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS)) + continue; + + res = rsm->asm_buffer; + proto = rsm->proto; + src = rsm->source; + dst = rsm->dest; + + rsm->asm_buffer = 0; + res_len = rsm->total_len; + *prev = rsm->next; + free_rsm (rsm); + ret = grub_malloc (sizeof (*ret)); + if (!ret) + { + grub_free (res); + return grub_errno; + } + ret->data = ret->head = res; + ret->tail = ret->end = res + res_len; + return handle_dgram (ret, card, hwaddress, proto, src, dst); + } return GRUB_ERR_NONE; } diff --git a/grub-core/net/netbuff.c b/grub-core/net/netbuff.c index d20104ab0..cb6106fc7 100644 --- a/grub-core/net/netbuff.c +++ b/grub-core/net/netbuff.c @@ -21,7 +21,6 @@ #include #include - grub_err_t grub_netbuff_put (struct grub_net_buff *nb, grub_size_t len) { @@ -87,7 +86,7 @@ grub_netbuff_alloc (grub_size_t len) return NULL; nb = (struct grub_net_buff *) ((grub_uint8_t *) data + len); nb->head = nb->data = nb->tail = data; - nb->end = (char *) nb; + nb->end = (grub_uint8_t *) nb; return nb; } diff --git a/grub-core/net/tftp.c b/grub-core/net/tftp.c index 13c4971f9..659e836c0 100644 --- a/grub-core/net/tftp.c +++ b/grub-core/net/tftp.c @@ -112,10 +112,10 @@ tftp_receive (grub_net_udp_socket_t sock __attribute__ ((unused)), { grub_file_t file = f; struct tftphdr *tftph = (void *) nb->data; - char nbdata[512]; + grub_uint8_t nbdata[512]; tftp_data_t data = file->data; grub_err_t err; - char *ptr; + grub_uint8_t *ptr; struct grub_net_buff nb_ack; nb_ack.head = nbdata; @@ -130,15 +130,11 @@ tftp_receive (grub_net_udp_socket_t sock __attribute__ ((unused)), for (ptr = nb->data + sizeof (tftph->opcode); ptr < nb->tail;) { if (grub_memcmp (ptr, "tsize\0", sizeof ("tsize\0") - 1) == 0) - { - data->file_size = grub_strtoul (ptr + sizeof ("tsize\0") - 1, - 0, 0); - } + data->file_size = grub_strtoul ((char *) ptr + sizeof ("tsize\0") + - 1, 0, 0); if (grub_memcmp (ptr, "blksize\0", sizeof ("blksize\0") - 1) == 0) - { - data->block_size = grub_strtoul (ptr + sizeof ("blksize\0") - 1, - 0, 0); - } + data->block_size = grub_strtoul ((char *) ptr + sizeof ("blksize\0") + - 1, 0, 0); while (ptr < nb->tail && *ptr) ptr++; ptr++; @@ -210,7 +206,7 @@ tftp_open (struct grub_file *file, const char *filename) int i; int rrqlen; int hdrlen; - char open_data[1500]; + grub_uint8_t open_data[1500]; struct grub_net_buff nb; tftp_data_t data; grub_err_t err; @@ -312,7 +308,7 @@ tftp_close (struct grub_file *file) if (data->sock) { - char nbdata[512]; + grub_uint8_t nbdata[512]; grub_err_t err; struct grub_net_buff nb_err; struct tftphdr *tftph; diff --git a/grub-core/net/udp.c b/grub-core/net/udp.c index 15bc1f490..0a43fe24f 100644 --- a/grub-core/net/udp.c +++ b/grub-core/net/udp.c @@ -110,6 +110,8 @@ grub_net_send_udp_packet (const grub_net_udp_socket_t socket, struct udphdr *udph; grub_err_t err; + COMPILE_TIME_ASSERT (GRUB_NET_UDP_HEADER_SIZE == sizeof (*udph)); + err = grub_netbuff_push (nb, sizeof (*udph)); if (err) return err; diff --git a/include/grub/net.h b/include/grub/net.h index 83f260d63..e7631b884 100644 --- a/include/grub/net.h +++ b/include/grub/net.h @@ -27,6 +27,10 @@ #include #include +#define GRUB_NET_MAX_LINK_HEADER_SIZE 64 +#define GRUB_NET_UDP_HEADER_SIZE 8 +#define GRUB_NET_OUR_IPV4_HEADER_SIZE 20 + typedef enum grub_link_level_protocol_id { GRUB_NET_LINK_LEVEL_PROTOCOL_ETHERNET diff --git a/include/grub/net/netbuff.h b/include/grub/net/netbuff.h index 245e813c3..5fafd89f6 100644 --- a/include/grub/net/netbuff.h +++ b/include/grub/net/netbuff.h @@ -8,14 +8,14 @@ struct grub_net_buff { - /*Pointer to the start of the buffer*/ - char *head; - /*Pointer to the data */ - char *data; - /*Pointer to the tail */ - char *tail; - /*Pointer to the end of the buffer*/ - char *end; + /* Pointer to the start of the buffer. */ + grub_uint8_t *head; + /* Pointer to the data. */ + grub_uint8_t *data; + /* Pointer to the tail. */ + grub_uint8_t *tail; + /* Pointer to the end of the buffer. */ + grub_uint8_t *end; }; grub_err_t grub_netbuff_put (struct grub_net_buff *net_buff ,grub_size_t len);