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

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