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