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