arch/sandbox/cpu/eth-raw-os.c

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