[Qemu-devel] [PATCH 12/18] slirp: Adding IPv6 UDP support

[Samuel Thibault <samuel.thibault@ens-lyon.org>]

This patch adds udp6_input() and udp6_output().
It also adds the IPv6 case in sorecvfrom().
Finally, udp_input() is called by ip6_input().

Signed-off-by: Guillaume Subiron <maethor@subiron.org>
---
 slirp/Makefile.objs |   2 +-
 slirp/ip6_input.c   |   3 +-
 slirp/socket.c      |   7 ++-
 slirp/udp.h         |   5 ++
 slirp/udp6.c        | 149 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 162 insertions(+), 4 deletions(-)
 create mode 100644 slirp/udp6.c

diff --git a/slirp/Makefile.objs b/slirp/Makefile.objs
index 2dfe8e0..faa32b6 100644
--- a/slirp/Makefile.objs
+++ b/slirp/Makefile.objs
@@ -1,3 +1,3 @@
 common-obj-y = cksum.o if.o ip_icmp.o ip6_icmp.o ip6_input.o ip6_output.o ip_input.o ip_output.o dnssearch.o
 common-obj-y += slirp.o mbuf.o misc.o sbuf.o socket.o tcp_input.o tcp_output.o
-common-obj-y += tcp_subr.o tcp_timer.o udp.o bootp.o tftp.o arp_table.o ndp_table.o
+common-obj-y += tcp_subr.o tcp_timer.o udp.o udp6.o bootp.o tftp.o arp_table.o ndp_table.o
diff --git a/slirp/ip6_input.c b/slirp/ip6_input.c
index af098a5..3290af8 100644
--- a/slirp/ip6_input.c
+++ b/slirp/ip6_input.c
@@ -62,8 +62,7 @@ void ip6_input(struct mbuf *m)
         icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE);
         break;
     case IPPROTO_UDP:
-        /* :TODO:maethor:130312: UDP */
-        icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE);
+        udp6_input(m);
         break;
     case IPPROTO_ICMPV6:
         icmp6_input(m);
diff --git a/slirp/socket.c b/slirp/socket.c
index f333fcf..31bbb7e 100644
--- a/slirp/socket.c
+++ b/slirp/socket.c
@@ -540,8 +540,13 @@ sorecvfrom(struct socket *so)
 	                   (struct sockaddr_in *) &daddr,
 	                   so->so_iptos);
 	        break;
-	    default:
+	    case AF_INET6:
+	        udp6_output(so, m, (struct sockaddr_in6 *) &saddr,
+	                    (struct sockaddr_in6 *) &daddr);
 	        break;
+	    default:
+	       assert(0);
+	       break;
 	    }
 	  } /* rx error */
 	} /* if ping packet */
diff --git a/slirp/udp.h b/slirp/udp.h
index 15e73c1..8a4d9f5 100644
--- a/slirp/udp.h
+++ b/slirp/udp.h
@@ -83,4 +83,9 @@ struct socket * udp_listen(Slirp *, uint32_t, u_int, uint32_t, u_int,
 int udp_output(struct socket *so, struct mbuf *m,
                 struct sockaddr_in *saddr, struct sockaddr_in *daddr,
                 int iptos);
+
+void udp6_input(register struct mbuf *);
+int udp6_output(struct socket *so, struct mbuf *m,
+                struct sockaddr_in6 *saddr, struct sockaddr_in6 *daddr);
+
 #endif
diff --git a/slirp/udp6.c b/slirp/udp6.c
new file mode 100644
index 0000000..3940959
--- /dev/null
+++ b/slirp/udp6.c
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2013
+ * Guillaume Subiron
+ *
+ * Please read the file COPYRIGHT for the
+ * terms and conditions of the copyright.
+ */
+
+#include "slirp.h"
+#include "udp.h"
+
+void udp6_input(struct mbuf *m)
+{
+    Slirp *slirp = m->slirp;
+    struct ip6 *ip, save_ip;
+    struct udphdr *uh;
+    int hlen = sizeof(struct ip6);
+    int len;
+    struct socket *so;
+    struct sockaddr_storage lhost;
+
+    DEBUG_CALL("udp6_input");
+    DEBUG_ARG("m = %lx", (long)m);
+
+    if (slirp->restricted) {
+        goto bad;
+    }
+
+    ip = mtod(m, struct ip6 *);
+    m->m_len -= hlen;
+    m->m_data += hlen;
+    uh = mtod(m, struct udphdr *);
+    m->m_len += hlen;
+    m->m_data -= hlen;
+
+    if (ip6_cksum(m)) {
+        goto bad;
+    }
+
+    len = ntohs((uint16_t)uh->uh_ulen);
+
+    /*
+     * Make mbuf data length reflect UDP length.
+     * If not enough data to reflect UDP length, drop.
+     */
+    if (ntohs(ip->ip_pl) != len) {
+        if (len > ntohs(ip->ip_pl)) {
+            goto bad;
+        }
+        m_adj(m, len - ntohs(ip->ip_pl));
+        ip->ip_pl = htons(len);
+    }
+
+    /* TODO handle DHCP/BOOTP */
+    /* TODO handle TFTP */
+
+    /* Locate pcb for datagram. */
+    lhost.ss_family = AF_INET6;
+    ((struct sockaddr_in6 *)&lhost)->sin6_addr = ip->ip_src;
+    ((struct sockaddr_in6 *)&lhost)->sin6_port = uh->uh_sport;
+
+    so = solookup(&slirp->udp_last_so, &slirp->udb, &lhost, NULL);
+
+    if (so == NULL) {
+        /* If there's no socket for this packet, create one. */
+        so = socreate(slirp);
+        if (!so) {
+            goto bad;
+        }
+        if (udp_attach(so, AF_INET6) == -1) {
+            DEBUG_MISC((dfd, " udp6_attach errno = %d-%s\n",
+                        errno, strerror(errno)));
+            sofree(so);
+            goto bad;
+        }
+
+        /* Setup fields */
+        so->so_lfamily = AF_INET6;
+        so->so_laddr6 = ip->ip_src;
+        so->so_lport6 = uh->uh_sport;
+    }
+
+    so->so_ffamily = AF_INET6;
+    so->so_faddr6 = ip->ip_dst; /* XXX */
+    so->so_fport6 = uh->uh_dport; /* XXX */
+
+    hlen += sizeof(struct udphdr);
+    m->m_len -= hlen;
+    m->m_data += hlen;
+
+    /*
+     * Now we sendto() the packet.
+     */
+    if (sosendto(so, m) == -1) {
+        m->m_len += hlen;
+        m->m_data -= hlen;
+        *ip = save_ip;
+        DEBUG_MISC((dfd, "udp tx errno = %d-%s\n", errno, strerror(errno)));
+        /* :TODO:maethor:130313: ICMPv6 error */
+        /*icmp_error(m, ICMP_UNREACH,ICMP_UNREACH_NET, 0,strerror(errno));*/
+        goto bad;
+    }
+
+    m_free(so->so_m);   /* used for ICMP if error on sorecvfrom */
+
+    /* restore the orig mbuf packet */
+    m->m_len += hlen;
+    m->m_data -= hlen;
+    *ip = save_ip;
+    so->so_m = m;
+
+    return;
+bad:
+    m_free(m);
+}
+
+int udp6_output(struct socket *so, struct mbuf *m,
+        struct sockaddr_in6 *saddr, struct sockaddr_in6 *daddr)
+{
+    struct ip6 *ip;
+    struct udphdr *uh;
+
+    DEBUG_CALL("udp6_output");
+    DEBUG_ARG("so = %lx", (long)so);
+    DEBUG_ARG("m = %lx", (long)m);
+
+    /* adjust for header */
+    m->m_data -= sizeof(struct udphdr);
+    m->m_len += sizeof(struct udphdr);
+    uh = mtod(m, struct udphdr *);
+    m->m_data -= sizeof(struct ip6);
+    m->m_len += sizeof(struct ip6);
+    ip = mtod(m, struct ip6 *);
+
+    /* Build IP header */
+    ip->ip_pl = htons(m->m_len - sizeof(struct ip6));
+    ip->ip_nh = IPPROTO_UDP;
+    ip->ip_src = saddr->sin6_addr;
+    ip->ip_dst = daddr->sin6_addr;
+
+    /* Build UDP header */
+    uh->uh_sport = saddr->sin6_port;
+    uh->uh_dport = daddr->sin6_port;
+    uh->uh_ulen = ip->ip_pl;
+    uh->uh_sum = 0;
+    uh->uh_sum = ip6_cksum(m);
+
+    return ip6_output(so, m, 0);
+}
-- 
1.9.0