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