slirp/udp6.c

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