1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2015-2018 National Instruments
4 * Copyright (c) 2015-2018 Joe Hershberger <joe.hershberger@ni.com>
5 */
6
7 #define _GNU_SOURCE
8
9 #include <asm/eth-raw-os.h>
10 #include <errno.h>
11 #include <fcntl.h>
12 #include <net/if.h>
13 #include <netinet/in.h>
14 #include <netinet/ip.h>
15 #include <netinet/udp.h>
16 #include <stdbool.h>
17 #include <stdio.h>
18 #include <stdlib.h>
19 #include <string.h>
20 #include <sys/types.h>
21 #include <sys/ioctl.h>
22 #include <sys/socket.h>
23 #include <unistd.h>
24
25 #include <arpa/inet.h>
26 #include <linux/if_ether.h>
27 #include <linux/if_packet.h>
28
29 #include <os.h>
30
sandbox_eth_raw_if_nameindex(void)31 struct sandbox_eth_raw_if_nameindex *sandbox_eth_raw_if_nameindex(void)
32 {
33 return (struct sandbox_eth_raw_if_nameindex *)if_nameindex();
34 }
35
sandbox_eth_raw_if_freenameindex(struct sandbox_eth_raw_if_nameindex * ptr)36 void sandbox_eth_raw_if_freenameindex(struct sandbox_eth_raw_if_nameindex *ptr)
37 {
38 if_freenameindex((struct if_nameindex *)ptr);
39 }
40
sandbox_eth_raw_os_is_local(const char * ifname)41 int sandbox_eth_raw_os_is_local(const char *ifname)
42 {
43 int fd = socket(AF_INET, SOCK_DGRAM, 0);
44 struct ifreq ifr;
45 int ret = 0;
46
47 if (fd < 0)
48 return -errno;
49 memset(&ifr, 0, sizeof(ifr));
50 strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
51 ret = ioctl(fd, SIOCGIFFLAGS, &ifr);
52 if (ret < 0) {
53 ret = -errno;
54 goto out;
55 }
56 ret = !!(ifr.ifr_flags & IFF_LOOPBACK);
57 out:
58 os_close(fd);
59 return ret;
60 }
61
sandbox_eth_raw_os_idx_to_name(struct eth_sandbox_raw_priv * priv)62 int sandbox_eth_raw_os_idx_to_name(struct eth_sandbox_raw_priv *priv)
63 {
64 if (!if_indextoname(priv->host_ifindex, priv->host_ifname))
65 return -errno;
66 return 0;
67 }
68
_raw_packet_start(struct eth_sandbox_raw_priv * priv,unsigned char * ethmac)69 static int _raw_packet_start(struct eth_sandbox_raw_priv *priv,
70 unsigned char *ethmac)
71 {
72 struct sockaddr_ll *device;
73 struct packet_mreq mr;
74 int ret;
75 int flags;
76
77 /* Prepare device struct */
78 priv->local_bind_sd = -1;
79 priv->device = malloc(sizeof(struct sockaddr_ll));
80 if (priv->device == NULL)
81 return -ENOMEM;
82 device = priv->device;
83 memset(device, 0, sizeof(struct sockaddr_ll));
84 device->sll_ifindex = if_nametoindex(priv->host_ifname);
85 priv->host_ifindex = device->sll_ifindex;
86 device->sll_family = AF_PACKET;
87 memcpy(device->sll_addr, ethmac, 6);
88 device->sll_halen = htons(6);
89
90 /* Open socket */
91 priv->sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
92 if (priv->sd < 0) {
93 printf("Failed to open socket: %d %s\n", errno,
94 strerror(errno));
95 return -errno;
96 }
97 /* Bind to the specified interface */
98 ret = setsockopt(priv->sd, SOL_SOCKET, SO_BINDTODEVICE,
99 priv->host_ifname, strlen(priv->host_ifname) + 1);
100 if (ret < 0) {
101 printf("Failed to bind to '%s': %d %s\n", priv->host_ifname,
102 errno, strerror(errno));
103 return -errno;
104 }
105
106 /* Make the socket non-blocking */
107 flags = fcntl(priv->sd, F_GETFL, 0);
108 fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
109
110 /* Enable promiscuous mode to receive responses meant for us */
111 mr.mr_ifindex = device->sll_ifindex;
112 mr.mr_type = PACKET_MR_PROMISC;
113 ret = setsockopt(priv->sd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
114 &mr, sizeof(mr));
115 if (ret < 0) {
116 struct ifreq ifr;
117
118 printf("Failed to set promiscuous mode: %d %s\n"
119 "Falling back to the old \"flags\" way...\n",
120 errno, strerror(errno));
121 if (strlen(priv->host_ifname) >= IFNAMSIZ) {
122 printf("Interface name %s is too long.\n",
123 priv->host_ifname);
124 return -EINVAL;
125 }
126 strncpy(ifr.ifr_name, priv->host_ifname, IFNAMSIZ);
127 if (ioctl(priv->sd, SIOCGIFFLAGS, &ifr) < 0) {
128 printf("Failed to read flags: %d %s\n", errno,
129 strerror(errno));
130 return -errno;
131 }
132 ifr.ifr_flags |= IFF_PROMISC;
133 if (ioctl(priv->sd, SIOCSIFFLAGS, &ifr) < 0) {
134 printf("Failed to write flags: %d %s\n", errno,
135 strerror(errno));
136 return -errno;
137 }
138 }
139 return 0;
140 }
141
_local_inet_start(struct eth_sandbox_raw_priv * priv)142 static int _local_inet_start(struct eth_sandbox_raw_priv *priv)
143 {
144 struct sockaddr_in *device;
145 int ret;
146 int flags;
147 int one = 1;
148
149 /* Prepare device struct */
150 priv->local_bind_sd = -1;
151 priv->local_bind_udp_port = 0;
152 priv->device = malloc(sizeof(struct sockaddr_in));
153 if (priv->device == NULL)
154 return -ENOMEM;
155 device = priv->device;
156 memset(device, 0, sizeof(struct sockaddr_in));
157 device->sin_family = AF_INET;
158 device->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
159
160 /**
161 * Open socket
162 * Since we specify UDP here, any incoming ICMP packets will
163 * not be received, so things like ping will not work on this
164 * localhost interface.
165 */
166 priv->sd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP);
167 if (priv->sd < 0) {
168 printf("Failed to open socket: %d %s\n", errno,
169 strerror(errno));
170 return -errno;
171 }
172
173 /* Make the socket non-blocking */
174 flags = fcntl(priv->sd, F_GETFL, 0);
175 fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
176
177 /* Include the UDP/IP headers on send and receive */
178 ret = setsockopt(priv->sd, IPPROTO_IP, IP_HDRINCL, &one,
179 sizeof(one));
180 if (ret < 0) {
181 printf("Failed to set header include option: %d %s\n", errno,
182 strerror(errno));
183 return -errno;
184 }
185 return 0;
186 }
187
sandbox_eth_raw_os_start(struct eth_sandbox_raw_priv * priv,unsigned char * ethmac)188 int sandbox_eth_raw_os_start(struct eth_sandbox_raw_priv *priv,
189 unsigned char *ethmac)
190 {
191 if (priv->local)
192 return _local_inet_start(priv);
193 else
194 return _raw_packet_start(priv, ethmac);
195 }
196
sandbox_eth_raw_os_send(void * packet,int length,struct eth_sandbox_raw_priv * priv)197 int sandbox_eth_raw_os_send(void *packet, int length,
198 struct eth_sandbox_raw_priv *priv)
199 {
200 int retval;
201 struct udphdr *udph = packet + sizeof(struct iphdr);
202
203 if (priv->sd < 0 || !priv->device)
204 return -EINVAL;
205
206 /*
207 * This block of code came about when testing tftp on the localhost
208 * interface. When using the RAW AF_INET API, the network stack is still
209 * in play responding to incoming traffic based on open "ports". Since
210 * it is raw (at the IP layer, no Ethernet) the network stack tells the
211 * TFTP server that the port it responded to is closed. This causes the
212 * TFTP transfer to be aborted. This block of code inspects the outgoing
213 * packet as formulated by the u-boot network stack to determine the
214 * source port (that the TFTP server will send packets back to) and
215 * opens a typical UDP socket on that port, thus preventing the network
216 * stack from sending that ICMP message claiming that the port has no
217 * bound socket.
218 */
219 if (priv->local && (priv->local_bind_sd == -1 ||
220 priv->local_bind_udp_port != udph->source)) {
221 struct iphdr *iph = packet;
222 struct sockaddr_in addr;
223
224 if (priv->local_bind_sd != -1)
225 os_close(priv->local_bind_sd);
226
227 /* A normal UDP socket is required to bind */
228 priv->local_bind_sd = socket(AF_INET, SOCK_DGRAM, 0);
229 if (priv->local_bind_sd < 0) {
230 printf("Failed to open bind sd: %d %s\n", errno,
231 strerror(errno));
232 return -errno;
233 }
234 priv->local_bind_udp_port = udph->source;
235
236 /**
237 * Bind the UDP port that we intend to use as our source port
238 * so that the kernel will not send an ICMP port unreachable
239 * message to the server
240 */
241 addr.sin_family = AF_INET;
242 addr.sin_port = udph->source;
243 addr.sin_addr.s_addr = iph->saddr;
244 retval = bind(priv->local_bind_sd, (struct sockaddr *)&addr,
245 sizeof(addr));
246 if (retval < 0)
247 printf("Failed to bind: %d %s\n", errno,
248 strerror(errno));
249 }
250
251 retval = sendto(priv->sd, packet, length, 0,
252 (struct sockaddr *)priv->device,
253 sizeof(struct sockaddr_ll));
254 if (retval < 0) {
255 printf("Failed to send packet: %d %s\n", errno,
256 strerror(errno));
257 return -errno;
258 }
259 return retval;
260 }
261
sandbox_eth_raw_os_recv(void * packet,int * length,const struct eth_sandbox_raw_priv * priv)262 int sandbox_eth_raw_os_recv(void *packet, int *length,
263 const struct eth_sandbox_raw_priv *priv)
264 {
265 int retval;
266 int saddr_size;
267
268 if (priv->sd < 0 || !priv->device)
269 return -EINVAL;
270 saddr_size = sizeof(struct sockaddr);
271 retval = recvfrom(priv->sd, packet, 1536, 0,
272 (struct sockaddr *)priv->device,
273 (socklen_t *)&saddr_size);
274 *length = 0;
275 if (retval >= 0) {
276 *length = retval;
277 return 0;
278 }
279 /* The socket is non-blocking, so expect EAGAIN when there is no data */
280 if (errno == EAGAIN)
281 return 0;
282 return -errno;
283 }
284
sandbox_eth_raw_os_stop(struct eth_sandbox_raw_priv * priv)285 void sandbox_eth_raw_os_stop(struct eth_sandbox_raw_priv *priv)
286 {
287 free(priv->device);
288 priv->device = NULL;
289 os_close(priv->sd);
290 priv->sd = -1;
291 if (priv->local) {
292 if (priv->local_bind_sd != -1)
293 os_close(priv->local_bind_sd);
294 priv->local_bind_sd = -1;
295 priv->local_bind_udp_port = 0;
296 }
297 }
298