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