#include <grub/net.h>
#include <grub/net/netbuff.h>
#include <grub/mm.h>
+#include <grub/priority_queue.h>
+#include <grub/time.h>
struct iphdr {
grub_uint8_t verhdrlen;
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)
{
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,
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;
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);
}
+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 grub_net_network_level_interface *inf = NULL;
+ grub_err_t err;
+ grub_net_network_level_address_t source;
+
+ /* DHCP needs special treatment since we don't know IP yet. */
+ {
+ struct udphdr *udph;
+ udph = (struct udphdr *) nb->data;
+ if (proto == GRUB_NET_IP_UDP && grub_be_to_cpu16 (udph->dst) == 68)
+ {
+ FOR_NET_NETWORK_LEVEL_INTERFACES (inf)
+ if (inf->card == card
+ && inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_DHCP_RECV
+ && grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0)
+ {
+ err = grub_netbuff_pull (nb, sizeof (*udph));
+ if (err)
+ return err;
+ grub_net_process_dhcp (nb, inf->card);
+ grub_netbuff_free (nb);
+ return GRUB_ERR_NONE;
+ }
+ grub_netbuff_free (nb);
+ return GRUB_ERR_NONE;
+ }
+ }
+
+ FOR_NET_NETWORK_LEVEL_INTERFACES (inf)
+ {
+ if (inf->card == card
+ && inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4
+ && inf->address.ipv4 == dst
+ && grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0)
+ break;
+ }
+
+ if (!inf)
+ {
+ grub_netbuff_free (nb);
+ return GRUB_ERR_NONE;
+ }
+
+ source.type = GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4;
+ source.ipv4 = src;
+
+ switch (proto)
+ {
+ case GRUB_NET_IP_UDP:
+ return grub_net_recv_udp_packet (nb, inf, &source);
+ case GRUB_NET_IP_ICMP:
+ return grub_net_recv_icmp_packet (nb, inf, &source);
+ default:
+ 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,
{
struct iphdr *iph = (struct iphdr *) nb->data;
grub_err_t err;
- struct grub_net_network_level_interface *inf = NULL;
- grub_net_network_level_address_t source;
-
- if (((grub_addr_t) nb->data) & 3)
- grub_fatal ("unaligned %p\n", nb->data);
+ struct reassemble *rsm, **prev;
if ((iph->verhdrlen >> 4) != 4)
{
return GRUB_ERR_NONE;
}
- err = grub_netbuff_pull (nb, (iph->verhdrlen & 0xf) * sizeof (grub_uint32_t));
- if (err)
+ 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 err;
+ return GRUB_ERR_NONE;
}
- /* Check size*/
+ /* 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));
+ grub_size_t actual_size = (nb->tail - nb->data);
if (actual_size > expected_size)
{
err = grub_netbuff_unput (nb, actual_size - expected_size);
}
}
- /* Fragmented packet. Bad. */
- if (((grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS) != 0)
- || (grub_be_to_cpu16 (iph->frags) & OFFSET_MASK) != 0)
+ /* Unfragmented packet. Good. */
+ 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;
+ 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);
}
- /* 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)
- {
- FOR_NET_NETWORK_LEVEL_INTERFACES (inf)
- if (inf->card == card
- && inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_DHCP_RECV
- && grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0)
- {
- err = grub_netbuff_pull (nb, sizeof (*udph));
- if (err)
- return err;
- grub_net_process_dhcp (nb, inf->card);
- grub_netbuff_free (nb);
- return GRUB_ERR_NONE;
- }
- grub_netbuff_free (nb);
- return GRUB_ERR_NONE;
- }
- }
-
- FOR_NET_NETWORK_LEVEL_INTERFACES (inf)
- {
- if (inf->card == card
- && inf->address.type == GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4
- && inf->address.ipv4 == iph->dest
- && grub_net_hwaddr_cmp (&inf->hwaddress, hwaddress) == 0)
+ 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 (!inf)
+ if (!rsm)
{
- grub_netbuff_free (nb);
- return GRUB_ERR_NONE;
+ 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;
}
- source.type = GRUB_NET_NETWORK_LEVEL_PROTOCOL_IPV4;
- source.ipv4 = iph->src;
+ rsm->last_time = grub_get_time_ms ();
+ free_old_fragments ();
- switch (iph->protocol)
+ err = grub_priority_queue_push (rsm->pq, &nb);
+ if (err)
+ return err;
+
+ if (!(grub_be_to_cpu16 (iph->frags) & MORE_FRAGMENTS))
{
- case GRUB_NET_IP_UDP:
- return grub_net_recv_udp_packet (nb, inf, &source);
- case GRUB_NET_IP_ICMP:
- return grub_net_recv_icmp_packet (nb, inf, &source);
- default:
- grub_netbuff_free (nb);
- break;
+ 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;
projects / thirdparty / grub.git / commitdiff
