slirp/udp6.c

   1 /*
   2  * Copyright (c) 2013
   3  * Guillaume Subiron
   4  */
   5
   6 #include "qemu/osdep.h"
   7 #include "qemu-common.h"
   8 #include "slirp.h"
   9 #include "qemu/osdep.h"
  10 #include "udp.h"
  11
  12 void udp6_input(struct mbuf *m)
  13 {
  14     Slirp *slirp = m->slirp;
  15     struct ip6 *ip, save_ip;
  16     struct udphdr *uh;
  17     int iphlen = sizeof(struct ip6);
  18     int len;
  19     struct socket *so;
  20     struct sockaddr_in6 lhost;
  21
  22     DEBUG_CALL("udp6_input");
  23     DEBUG_ARG("m = %lx", (long)m);
  24
  25     if (slirp->restricted) {
  26         goto bad;
  27     }
  28
  29     ip = mtod(m, struct ip6 *);
  30     m->m_len -= iphlen;
  31     m->m_data += iphlen;
  32     uh = mtod(m, struct udphdr *);
  33     m->m_len += iphlen;
  34     m->m_data -= iphlen;
  35
  36     if (ip6_cksum(m)) {
  37         goto bad;
  38     }
  39
  40     len = ntohs((uint16_t)uh->uh_ulen);
  41
  42     /*
  43      * Make mbuf data length reflect UDP length.
  44      * If not enough data to reflect UDP length, drop.
  45      */
  46     if (ntohs(ip->ip_pl) != len) {
  47         if (len > ntohs(ip->ip_pl)) {
  48             goto bad;
  49         }
  50         m_adj(m, len - ntohs(ip->ip_pl));
  51         ip->ip_pl = htons(len);
  52     }
  53
  54     /*
  55      * Save a copy of the IP header in case we want restore it
  56      * for sending an ICMP error message in response.
  57      */
  58     save_ip = *ip;
  59
  60     /* Locate pcb for datagram. */
  61     lhost.sin6_family = AF_INET6;
  62     lhost.sin6_addr = ip->ip_src;
  63     lhost.sin6_port = uh->uh_sport;
  64
  65     /* TODO handle DHCP/BOOTP */
  66
  67     /* handle TFTP */
  68     if (ntohs(uh->uh_dport) == TFTP_SERVER &&
  69         !memcmp(ip->ip_dst.s6_addr, slirp->vhost_addr6.s6_addr, 16)) {
  70         m->m_data += iphlen;
  71         m->m_len -= iphlen;
  72         tftp_input((struct sockaddr_storage *)&lhost, m);
  73         m->m_data -= iphlen;
  74         m->m_len += iphlen;
  75         goto bad;
  76     }
  77
  78     so = solookup(&slirp->udp_last_so, &slirp->udb,
  79                   (struct sockaddr_storage *) &lhost, NULL);
  80
  81     if (so == NULL) {
  82         /* If there's no socket for this packet, create one. */
  83         so = socreate(slirp);
  84         if (!so) {
  85             goto bad;
  86         }
  87         if (udp_attach(so, AF_INET6) == -1) {
  88             DEBUG_MISC((dfd, " udp6_attach errno = %d-%s\n",
  89                         errno, strerror(errno)));
  90             sofree(so);
  91             goto bad;
  92         }
  93
  94         /* Setup fields */
  95         so->so_lfamily = AF_INET6;
  96         so->so_laddr6 = ip->ip_src;
  97         so->so_lport6 = uh->uh_sport;
  98     }
  99
 100     so->so_ffamily = AF_INET6;
 101     so->so_faddr6 = ip->ip_dst; /* XXX */
 102     so->so_fport6 = uh->uh_dport; /* XXX */
 103
 104     iphlen += sizeof(struct udphdr);
 105     m->m_len -= iphlen;
 106     m->m_data += iphlen;
 107
 108     /*
 109      * Now we sendto() the packet.
 110      */
 111     if (sosendto(so, m) == -1) {
 112         m->m_len += iphlen;
 113         m->m_data -= iphlen;
 114         *ip = save_ip;
 115         DEBUG_MISC((dfd, "udp tx errno = %d-%s\n", errno, strerror(errno)));
 116         /* TODO: ICMPv6 error */
 117         /*icmp_error(m, ICMP_UNREACH,ICMP_UNREACH_NET, 0,strerror(errno));*/
 118         goto bad;
 119     }
 120
 121     m_free(so->so_m);   /* used for ICMP if error on sorecvfrom */
 122
 123     /* restore the orig mbuf packet */
 124     m->m_len += iphlen;
 125     m->m_data -= iphlen;
 126     *ip = save_ip;
 127     so->so_m = m;
 128
 129     return;
 130 bad:
 131     m_free(m);
 132 }
 133
 134 int udp6_output(struct socket *so, struct mbuf *m,
 135         struct sockaddr_in6 *saddr, struct sockaddr_in6 *daddr)
 136 {
 137     struct ip6 *ip;
 138     struct udphdr *uh;
 139
 140     DEBUG_CALL("udp6_output");
 141     DEBUG_ARG("so = %lx", (long)so);
 142     DEBUG_ARG("m = %lx", (long)m);
 143
 144     /* adjust for header */
 145     m->m_data -= sizeof(struct udphdr);
 146     m->m_len += sizeof(struct udphdr);
 147     uh = mtod(m, struct udphdr *);
 148     m->m_data -= sizeof(struct ip6);
 149     m->m_len += sizeof(struct ip6);
 150     ip = mtod(m, struct ip6 *);
 151
 152     /* Build IP header */
 153     ip->ip_pl = htons(m->m_len - sizeof(struct ip6));
 154     ip->ip_nh = IPPROTO_UDP;
 155     ip->ip_src = saddr->sin6_addr;
 156     ip->ip_dst = daddr->sin6_addr;
 157
 158     /* Build UDP header */
 159     uh->uh_sport = saddr->sin6_port;
 160     uh->uh_dport = daddr->sin6_port;
 161     uh->uh_ulen = ip->ip_pl;
 162     uh->uh_sum = 0;
 163     uh->uh_sum = ip6_cksum(m);
 164     if (uh->uh_sum == 0) {
 165         uh->uh_sum = 0xffff;
 166     }
 167
 168     return ip6_output(so, m, 0);
 169 }