2 * Copyright (c) 2015 National Instruments
5 * Joe Hershberger <joe.hershberger@ni.com>
7 * SPDX-License-Identifier: GPL-2.0
10 #include <asm/eth-raw-os.h>
14 #include <netinet/in.h>
15 #include <netinet/ip.h>
16 #include <netinet/udp.h>
20 #include <sys/types.h>
21 #include <sys/ioctl.h>
22 #include <sys/socket.h>
25 #include <arpa/inet.h>
26 #include <linux/if_ether.h>
27 #include <linux/if_packet.h>
29 static int _raw_packet_start(const char *ifname, unsigned char *ethmac,
30 struct eth_sandbox_raw_priv *priv)
32 struct sockaddr_ll *device;
33 struct packet_mreq mr;
37 /* Prepare device struct */
38 priv->device = malloc(sizeof(struct sockaddr_ll));
39 if (priv->device == NULL)
41 device = priv->device;
42 memset(device, 0, sizeof(struct sockaddr_ll));
43 device->sll_ifindex = if_nametoindex(ifname);
44 device->sll_family = AF_PACKET;
45 memcpy(device->sll_addr, ethmac, 6);
46 device->sll_halen = htons(6);
49 priv->sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
51 printf("Failed to open socket: %d %s\n", errno,
55 /* Bind to the specified interface */
56 ret = setsockopt(priv->sd, SOL_SOCKET, SO_BINDTODEVICE, ifname,
59 printf("Failed to bind to '%s': %d %s\n", ifname, errno,
64 /* Make the socket non-blocking */
65 flags = fcntl(priv->sd, F_GETFL, 0);
66 fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
68 /* Enable promiscuous mode to receive responses meant for us */
69 mr.mr_ifindex = device->sll_ifindex;
70 mr.mr_type = PACKET_MR_PROMISC;
71 ret = setsockopt(priv->sd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
76 printf("Failed to set promiscuous mode: %d %s\n"
77 "Falling back to the old \"flags\" way...\n",
78 errno, strerror(errno));
79 strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
80 if (ioctl(priv->sd, SIOCGIFFLAGS, &ifr) < 0) {
81 printf("Failed to read flags: %d %s\n", errno,
85 ifr.ifr_flags |= IFF_PROMISC;
86 if (ioctl(priv->sd, SIOCSIFFLAGS, &ifr) < 0) {
87 printf("Failed to write flags: %d %s\n", errno,
95 static int _local_inet_start(struct eth_sandbox_raw_priv *priv)
97 struct sockaddr_in *device;
102 /* Prepare device struct */
103 priv->device = malloc(sizeof(struct sockaddr_in));
104 if (priv->device == NULL)
106 device = priv->device;
107 memset(device, 0, sizeof(struct sockaddr_in));
108 device->sin_family = AF_INET;
109 device->sin_addr.s_addr = htonl(INADDR_LOOPBACK);
113 * Since we specify UDP here, any incoming ICMP packets will
114 * not be received, so things like ping will not work on this
115 * localhost interface.
117 priv->sd = socket(AF_INET, SOCK_RAW, IPPROTO_UDP);
119 printf("Failed to open socket: %d %s\n", errno,
124 /* Make the socket non-blocking */
125 flags = fcntl(priv->sd, F_GETFL, 0);
126 fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
128 /* Include the UDP/IP headers on send and receive */
129 ret = setsockopt(priv->sd, IPPROTO_IP, IP_HDRINCL, &one,
132 printf("Failed to set header include option: %d %s\n", errno,
136 priv->local_bind_sd = -1;
137 priv->local_bind_udp_port = 0;
141 int sandbox_eth_raw_os_start(const char *ifname, unsigned char *ethmac,
142 struct eth_sandbox_raw_priv *priv)
145 return _local_inet_start(priv);
147 return _raw_packet_start(ifname, ethmac, priv);
150 int sandbox_eth_raw_os_send(void *packet, int length,
151 struct eth_sandbox_raw_priv *priv)
154 struct udphdr *udph = packet + sizeof(struct iphdr);
156 if (!priv->sd || !priv->device)
160 * This block of code came about when testing tftp on the localhost
161 * interface. When using the RAW AF_INET API, the network stack is still
162 * in play responding to incoming traffic based on open "ports". Since
163 * it is raw (at the IP layer, no Ethernet) the network stack tells the
164 * TFTP server that the port it responded to is closed. This causes the
165 * TFTP transfer to be aborted. This block of code inspects the outgoing
166 * packet as formulated by the u-boot network stack to determine the
167 * source port (that the TFTP server will send packets back to) and
168 * opens a typical UDP socket on that port, thus preventing the network
169 * stack from sending that ICMP message claiming that the port has no
172 if (priv->local && (priv->local_bind_sd == -1 ||
173 priv->local_bind_udp_port != udph->source)) {
174 struct iphdr *iph = packet;
175 struct sockaddr_in addr;
177 if (priv->local_bind_sd != -1)
178 close(priv->local_bind_sd);
180 /* A normal UDP socket is required to bind */
181 priv->local_bind_sd = socket(AF_INET, SOCK_DGRAM, 0);
182 if (priv->local_bind_sd < 0) {
183 printf("Failed to open bind sd: %d %s\n", errno,
187 priv->local_bind_udp_port = udph->source;
190 * Bind the UDP port that we intend to use as our source port
191 * so that the kernel will not send an ICMP port unreachable
192 * message to the server
194 addr.sin_family = AF_INET;
195 addr.sin_port = udph->source;
196 addr.sin_addr.s_addr = iph->saddr;
197 retval = bind(priv->local_bind_sd, &addr, sizeof(addr));
199 printf("Failed to bind: %d %s\n", errno,
203 retval = sendto(priv->sd, packet, length, 0,
204 (struct sockaddr *)priv->device,
205 sizeof(struct sockaddr_ll));
207 printf("Failed to send packet: %d %s\n", errno,
214 int sandbox_eth_raw_os_recv(void *packet, int *length,
215 const struct eth_sandbox_raw_priv *priv)
220 if (!priv->sd || !priv->device)
222 saddr_size = sizeof(struct sockaddr);
223 retval = recvfrom(priv->sd, packet, 1536, 0,
224 (struct sockaddr *)priv->device,
225 (socklen_t *)&saddr_size);
231 /* The socket is non-blocking, so expect EAGAIN when there is no data */
237 void sandbox_eth_raw_os_stop(struct eth_sandbox_raw_priv *priv)
244 if (priv->local_bind_sd != -1)
245 close(priv->local_bind_sd);
246 priv->local_bind_sd = -1;
247 priv->local_bind_udp_port = 0;