Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (c) 2013 |
| 3 | * Guillaume Subiron |
| 4 | */ |
| 5 | |
| 6 | #include "qemu/osdep.h" |
| 7 | #include "qemu-common.h" |
| 8 | #include "slirp.h" |
Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 9 | #include "udp.h" |
Thomas Huth | 7b14399 | 2016-06-28 12:48:31 +0200 | [diff] [blame] | 10 | #include "dhcpv6.h" |
Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 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 | |
Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 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 | |
Thomas Huth | 7b14399 | 2016-06-28 12:48:31 +0200 | [diff] [blame] | 65 | /* handle DHCPv6 */ |
| 66 | if (ntohs(uh->uh_dport) == DHCPV6_SERVER_PORT && |
| 67 | (in6_equal(&ip->ip_dst, &slirp->vhost_addr6) || |
Philippe Mathieu-Daudé | 318116a | 2018-01-08 14:29:01 -0300 | [diff] [blame] | 68 | in6_dhcp_multicast(&ip->ip_dst))) { |
Thomas Huth | 7b14399 | 2016-06-28 12:48:31 +0200 | [diff] [blame] | 69 | m->m_data += iphlen; |
| 70 | m->m_len -= iphlen; |
| 71 | dhcpv6_input(&lhost, m); |
| 72 | m->m_data -= iphlen; |
| 73 | m->m_len += iphlen; |
| 74 | goto bad; |
| 75 | } |
Thomas Huth | fad7fb9 | 2016-03-15 10:31:23 +0100 | [diff] [blame] | 76 | |
| 77 | /* handle TFTP */ |
| 78 | if (ntohs(uh->uh_dport) == TFTP_SERVER && |
| 79 | !memcmp(ip->ip_dst.s6_addr, slirp->vhost_addr6.s6_addr, 16)) { |
| 80 | m->m_data += iphlen; |
| 81 | m->m_len -= iphlen; |
| 82 | tftp_input((struct sockaddr_storage *)&lhost, m); |
| 83 | m->m_data -= iphlen; |
| 84 | m->m_len += iphlen; |
| 85 | goto bad; |
| 86 | } |
| 87 | |
Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 88 | so = solookup(&slirp->udp_last_so, &slirp->udb, |
| 89 | (struct sockaddr_storage *) &lhost, NULL); |
| 90 | |
| 91 | if (so == NULL) { |
| 92 | /* If there's no socket for this packet, create one. */ |
| 93 | so = socreate(slirp); |
| 94 | if (!so) { |
| 95 | goto bad; |
| 96 | } |
| 97 | if (udp_attach(so, AF_INET6) == -1) { |
| 98 | DEBUG_MISC((dfd, " udp6_attach errno = %d-%s\n", |
| 99 | errno, strerror(errno))); |
| 100 | sofree(so); |
| 101 | goto bad; |
| 102 | } |
| 103 | |
| 104 | /* Setup fields */ |
| 105 | so->so_lfamily = AF_INET6; |
| 106 | so->so_laddr6 = ip->ip_src; |
| 107 | so->so_lport6 = uh->uh_sport; |
| 108 | } |
| 109 | |
| 110 | so->so_ffamily = AF_INET6; |
| 111 | so->so_faddr6 = ip->ip_dst; /* XXX */ |
| 112 | so->so_fport6 = uh->uh_dport; /* XXX */ |
| 113 | |
| 114 | iphlen += sizeof(struct udphdr); |
| 115 | m->m_len -= iphlen; |
| 116 | m->m_data += iphlen; |
| 117 | |
| 118 | /* |
| 119 | * Now we sendto() the packet. |
| 120 | */ |
| 121 | if (sosendto(so, m) == -1) { |
| 122 | m->m_len += iphlen; |
| 123 | m->m_data -= iphlen; |
| 124 | *ip = save_ip; |
| 125 | DEBUG_MISC((dfd, "udp tx errno = %d-%s\n", errno, strerror(errno))); |
Samuel Thibault | c17c072 | 2016-03-20 14:52:32 +0100 | [diff] [blame] | 126 | icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE); |
Guillaume Subiron | 15d62af | 2016-03-15 10:31:20 +0100 | [diff] [blame] | 127 | goto bad; |
| 128 | } |
| 129 | |
| 130 | m_free(so->so_m); /* used for ICMP if error on sorecvfrom */ |
| 131 | |
| 132 | /* restore the orig mbuf packet */ |
| 133 | m->m_len += iphlen; |
| 134 | m->m_data -= iphlen; |
| 135 | *ip = save_ip; |
| 136 | so->so_m = m; |
| 137 | |
| 138 | return; |
| 139 | bad: |
| 140 | m_free(m); |
| 141 | } |
| 142 | |
| 143 | int udp6_output(struct socket *so, struct mbuf *m, |
| 144 | struct sockaddr_in6 *saddr, struct sockaddr_in6 *daddr) |
| 145 | { |
| 146 | struct ip6 *ip; |
| 147 | struct udphdr *uh; |
| 148 | |
| 149 | DEBUG_CALL("udp6_output"); |
| 150 | DEBUG_ARG("so = %lx", (long)so); |
| 151 | DEBUG_ARG("m = %lx", (long)m); |
| 152 | |
| 153 | /* adjust for header */ |
| 154 | m->m_data -= sizeof(struct udphdr); |
| 155 | m->m_len += sizeof(struct udphdr); |
| 156 | uh = mtod(m, struct udphdr *); |
| 157 | m->m_data -= sizeof(struct ip6); |
| 158 | m->m_len += sizeof(struct ip6); |
| 159 | ip = mtod(m, struct ip6 *); |
| 160 | |
| 161 | /* Build IP header */ |
| 162 | ip->ip_pl = htons(m->m_len - sizeof(struct ip6)); |
| 163 | ip->ip_nh = IPPROTO_UDP; |
| 164 | ip->ip_src = saddr->sin6_addr; |
| 165 | ip->ip_dst = daddr->sin6_addr; |
| 166 | |
| 167 | /* Build UDP header */ |
| 168 | uh->uh_sport = saddr->sin6_port; |
| 169 | uh->uh_dport = daddr->sin6_port; |
| 170 | uh->uh_ulen = ip->ip_pl; |
| 171 | uh->uh_sum = 0; |
| 172 | uh->uh_sum = ip6_cksum(m); |
| 173 | if (uh->uh_sum == 0) { |
| 174 | uh->uh_sum = 0xffff; |
| 175 | } |
| 176 | |
| 177 | return ip6_output(so, m, 0); |
| 178 | } |