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