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