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