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
11void 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;
134bad:
135 m_free(m);
136}
137
138int 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