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