LXR qemu/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
  12void 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        icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE);
 117        goto bad;
 118    }
 119
 120    m_free(so->so_m);   /* used for ICMP if error on sorecvfrom */
 121
 122    /* restore the orig mbuf packet */
 123    m->m_len += iphlen;
 124    m->m_data -= iphlen;
 125    *ip = save_ip;
 126    so->so_m = m;
 127
 128    return;
 129bad:
 130    m_free(m);
 131}
 132
 133int udp6_output(struct socket *so, struct mbuf *m,
 134        struct sockaddr_in6 *saddr, struct sockaddr_in6 *daddr)
 135{
 136    struct ip6 *ip;
 137    struct udphdr *uh;
 138
 139    DEBUG_CALL("udp6_output");
 140    DEBUG_ARG("so = %lx", (long)so);
 141    DEBUG_ARG("m = %lx", (long)m);
 142
 143    /* adjust for header */
 144    m->m_data -= sizeof(struct udphdr);
 145    m->m_len += sizeof(struct udphdr);
 146    uh = mtod(m, struct udphdr *);
 147    m->m_data -= sizeof(struct ip6);
 148    m->m_len += sizeof(struct ip6);
 149    ip = mtod(m, struct ip6 *);
 150
 151    /* Build IP header */
 152    ip->ip_pl = htons(m->m_len - sizeof(struct ip6));
 153    ip->ip_nh = IPPROTO_UDP;
 154    ip->ip_src = saddr->sin6_addr;
 155    ip->ip_dst = daddr->sin6_addr;
 156
 157    /* Build UDP header */
 158    uh->uh_sport = saddr->sin6_port;
 159    uh->uh_dport = daddr->sin6_port;
 160    uh->uh_ulen = ip->ip_pl;
 161    uh->uh_sum = 0;
 162    uh->uh_sum = ip6_cksum(m);
 163    if (uh->uh_sum == 0) {
 164        uh->uh_sum = 0xffff;
 165    }
 166
 167    return ip6_output(so, m, 0);
 168}
 169