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 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 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 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 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 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 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