slirp: Adding IPv6 UDP support

This adds the sin6 case in the fhost and lhost unions and related macros.
It adds udp6_input() and udp6_output().
It adds the IPv6 case in sorecvfrom().
Finally, udp_input() is called by ip6_input().

Signed-off-by: Guillaume Subiron <maethor@subiron.org>
Signed-off-by: Samuel Thibault <samuel.thibault@ens-lyon.org>
Reviewed-by: Thomas Huth <thuth@redhat.com>

6 files changed, 208 insertions, 9 deletions

diff --git a/slirp/Makefile.objs b/slirp/Makefile.objs
index 4e3a289b08..6748e4f60a 100644
--- a/slirp/Makefile.objs
+++ b/slirp/Makefile.objs
@@ -1,5 +1,5 @@
 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 \
+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 b6a438d7af..9ca6d32b1a 100644
--- a/slirp/ip6_input.c
+++ b/slirp/ip6_input.c
@@ -58,7 +58,7 @@ void ip6_input(struct mbuf *m)
         icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE);
         break;
     case IPPROTO_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 32b1ba3bd5..55150f5e5e 100644
--- a/slirp/socket.c
+++ b/slirp/socket.c
@@ -505,13 +505,37 @@ sorecvfrom(struct socket *so)
 	  DEBUG_MISC((dfd, " did recvfrom %d, errno = %d-%s\n",
 		      m->m_len, errno,strerror(errno)));
 	  if(m->m_len<0) {
-	    u_char code=ICMP_UNREACH_PORT;
-
-	    if(errno == EHOSTUNREACH) code=ICMP_UNREACH_HOST;
-	    else if(errno == ENETUNREACH) code=ICMP_UNREACH_NET;
-
-	    DEBUG_MISC((dfd," rx error, tx icmp ICMP_UNREACH:%i\n", code));
-	    icmp_send_error(so->so_m, ICMP_UNREACH, code, 0, strerror(errno));
+	    /* Report error as ICMP */
+	    switch (so->so_lfamily) {
+	    uint8_t code;
+	    case AF_INET:
+	      code = ICMP_UNREACH_PORT;
+
+	      if (errno == EHOSTUNREACH) {
+		code = ICMP_UNREACH_HOST;
+	      } else if (errno == ENETUNREACH) {
+		code = ICMP_UNREACH_NET;
+	      }
+
+	      DEBUG_MISC((dfd, " rx error, tx icmp ICMP_UNREACH:%i\n", code));
+	      icmp_send_error(so->so_m, ICMP_UNREACH, code, 0, strerror(errno));
+	      break;
+	    case AF_INET6:
+	      code = ICMP6_UNREACH_PORT;
+
+	      if (errno == EHOSTUNREACH) {
+		code = ICMP6_UNREACH_ADDRESS;
+	      } else if (errno == ENETUNREACH) {
+		code = ICMP6_UNREACH_NO_ROUTE;
+	      }
+
+	      DEBUG_MISC((dfd, " rx error, tx icmp6 ICMP_UNREACH:%i\n", code));
+	      icmp6_send_error(so->so_m, ICMP6_UNREACH, code);
+	      break;
+	    default:
+	      g_assert_not_reached();
+	      break;
+	    }
 	    m_free(m);
 	  } else {
 	  /*
@@ -541,7 +565,12 @@ sorecvfrom(struct socket *so)
 	                   (struct sockaddr_in *) &daddr,
 	                   so->so_iptos);
 	        break;
+	    case AF_INET6:
+	        udp6_output(so, m, (struct sockaddr_in6 *) &saddr,
+	                    (struct sockaddr_in6 *) &daddr);
+	        break;
 	    default:
+	        g_assert_not_reached();
 	        break;
 	    }
 	  } /* rx error */
diff --git a/slirp/socket.h b/slirp/socket.h
index bcebce110d..e9c9b053dc 100644
--- a/slirp/socket.h
+++ b/slirp/socket.h
@@ -34,17 +34,23 @@ struct socket {
   union {   /* foreign host */
       struct sockaddr_storage ss;
       struct sockaddr_in sin;
+      struct sockaddr_in6 sin6;
   } fhost;
 #define so_faddr fhost.sin.sin_addr
 #define so_fport fhost.sin.sin_port
+#define so_faddr6 fhost.sin6.sin6_addr
+#define so_fport6 fhost.sin6.sin6_port
 #define so_ffamily fhost.ss.ss_family
 
   union {   /* local host */
       struct sockaddr_storage ss;
       struct sockaddr_in sin;
+      struct sockaddr_in6 sin6;
   } lhost;
 #define so_laddr lhost.sin.sin_addr
 #define so_lport lhost.sin.sin_port
+#define so_laddr6 lhost.sin6.sin6_addr
+#define so_lport6 lhost.sin6.sin6_port
 #define so_lfamily lhost.ss.ss_family
 
   uint8_t	so_iptos;	/* Type of service */
diff --git a/slirp/udp.h b/slirp/udp.h
index 2f9de3886c..10cc7809b1 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 0000000000..a9a36716a0
--- /dev/null
+++ b/slirp/udp6.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2013
+ * Guillaume Subiron
+ */
+
+#include "qemu/osdep.h"
+#include "qemu-common.h"
+#include "slirp.h"
+#include "qemu/osdep.h"
+#include "udp.h"
+
+void udp6_input(struct mbuf *m)
+{
+    Slirp *slirp = m->slirp;
+    struct ip6 *ip, save_ip;
+    struct udphdr *uh;
+    int iphlen = sizeof(struct ip6);
+    int len;
+    struct socket *so;
+    struct sockaddr_in6 lhost;
+
+    DEBUG_CALL("udp6_input");
+    DEBUG_ARG("m = %lx", (long)m);
+
+    if (slirp->restricted) {
+        goto bad;
+    }
+
+    ip = mtod(m, struct ip6 *);
+    m->m_len -= iphlen;
+    m->m_data += iphlen;
+    uh = mtod(m, struct udphdr *);
+    m->m_len += iphlen;
+    m->m_data -= iphlen;
+
+    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);
+    }
+
+    /*
+     * Save a copy of the IP header in case we want restore it
+     * for sending an ICMP error message in response.
+     */
+    save_ip = *ip;
+
+    /* TODO handle DHCP/BOOTP */
+    /* TODO handle TFTP */
+
+    /* Locate pcb for datagram. */
+    lhost.sin6_family = AF_INET6;
+    lhost.sin6_addr = ip->ip_src;
+    lhost.sin6_port = uh->uh_sport;
+
+    so = solookup(&slirp->udp_last_so, &slirp->udb,
+                  (struct sockaddr_storage *) &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 */
+
+    iphlen += sizeof(struct udphdr);
+    m->m_len -= iphlen;
+    m->m_data += iphlen;
+
+    /*
+     * Now we sendto() the packet.
+     */
+    if (sosendto(so, m) == -1) {
+        m->m_len += iphlen;
+        m->m_data -= iphlen;
+        *ip = save_ip;
+        DEBUG_MISC((dfd, "udp tx errno = %d-%s\n", errno, strerror(errno)));
+        /* TODO: 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 += iphlen;
+    m->m_data -= iphlen;
+    *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);
+    if (uh->uh_sum == 0) {
+        uh->uh_sum = 0xffff;
+    }
+
+    return ip6_output(so, m, 0);
+}