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 #include "dhcpv6.h"
11
udp6_input(struct mbuf * m)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 /* handle DHCPv6 */
66 if (ntohs(uh->uh_dport) == DHCPV6_SERVER_PORT &&
67 (in6_equal(&ip->ip_dst, &slirp->vhost_addr6) ||
68 in6_dhcp_multicast(&ip->ip_dst))) {
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 }
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
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)));
126 icmp6_send_error(m, ICMP6_UNREACH, ICMP6_UNREACH_NO_ROUTE);
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
udp6_output(struct socket * so,struct mbuf * m,struct sockaddr_in6 * saddr,struct sockaddr_in6 * daddr)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 }
179