git.sur5r.net Git - u-boot/blob - arch/sandbox/cpu/eth-raw-os.c

   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 }