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 <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 <linux/if_ether.h>
  24 #include <linux/if_packet.h>
  25
  26 int sandbox_eth_raw_os_start(const char *ifname, unsigned char *ethmac,
  27                             struct eth_sandbox_raw_priv *priv)
  28 {
  29         struct sockaddr_ll *device;
  30         struct packet_mreq mr;
  31         int ret;
  32         int flags;
  33
  34         /* Prepare device struct */
  35         priv->device = malloc(sizeof(struct sockaddr_ll));
  36         if (priv->device == NULL)
  37                 return -ENOMEM;
  38         device = priv->device;
  39         memset(device, 0, sizeof(struct sockaddr_ll));
  40         device->sll_ifindex = if_nametoindex(ifname);
  41         device->sll_family = AF_PACKET;
  42         memcpy(device->sll_addr, ethmac, 6);
  43         device->sll_halen = htons(6);
  44
  45         /* Open socket */
  46         priv->sd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
  47         if (priv->sd < 0) {
  48                 printf("Failed to open socket: %d %s\n", errno,
  49                        strerror(errno));
  50                 return -errno;
  51         }
  52         /* Bind to the specified interface */
  53         ret = setsockopt(priv->sd, SOL_SOCKET, SO_BINDTODEVICE, ifname,
  54                    strlen(ifname) + 1);
  55         if (ret < 0) {
  56                 printf("Failed to bind to '%s': %d %s\n", ifname, errno,
  57                        strerror(errno));
  58                 return -errno;
  59         }
  60
  61         /* Make the socket non-blocking */
  62         flags = fcntl(priv->sd, F_GETFL, 0);
  63         fcntl(priv->sd, F_SETFL, flags | O_NONBLOCK);
  64
  65         /* Enable promiscuous mode to receive responses meant for us */
  66         mr.mr_ifindex = device->sll_ifindex;
  67         mr.mr_type = PACKET_MR_PROMISC;
  68         ret = setsockopt(priv->sd, SOL_PACKET, PACKET_ADD_MEMBERSHIP,
  69                    &mr, sizeof(mr));
  70         if (ret < 0) {
  71                 struct ifreq ifr;
  72
  73                 printf("Failed to set promiscuous mode: %d %s\n"
  74                        "Falling back to the old \"flags\" way...\n",
  75                         errno, strerror(errno));
  76                 strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
  77                 if (ioctl(priv->sd, SIOCGIFFLAGS, &ifr) < 0) {
  78                         printf("Failed to read flags: %d %s\n", errno,
  79                                strerror(errno));
  80                         return -errno;
  81                 }
  82                 ifr.ifr_flags |= IFF_PROMISC;
  83                 if (ioctl(priv->sd, SIOCSIFFLAGS, &ifr) < 0) {
  84                         printf("Failed to write flags: %d %s\n", errno,
  85                                strerror(errno));
  86                         return -errno;
  87                 }
  88         }
  89         return 0;
  90 }
  91
  92 int sandbox_eth_raw_os_send(void *packet, int length,
  93                             const struct eth_sandbox_raw_priv *priv)
  94 {
  95         int retval;
  96
  97         if (!priv->sd || !priv->device)
  98                 return -EINVAL;
  99
 100         retval = sendto(priv->sd, packet, length, 0,
 101                         (struct sockaddr *)priv->device,
 102                         sizeof(struct sockaddr_ll));
 103         if (retval < 0) {
 104                 printf("Failed to send packet: %d %s\n", errno,
 105                        strerror(errno));
 106                 return -errno;
 107         }
 108         return retval;
 109 }
 110
 111 int sandbox_eth_raw_os_recv(void *packet, int *length,
 112                             const struct eth_sandbox_raw_priv *priv)
 113 {
 114         int retval;
 115         int saddr_size;
 116
 117         if (!priv->sd || !priv->device)
 118                 return -EINVAL;
 119         saddr_size = sizeof(struct sockaddr);
 120         retval = recvfrom(priv->sd, packet, 1536, 0,
 121                           (struct sockaddr *)priv->device,
 122                           (socklen_t *)&saddr_size);
 123         *length = 0;
 124         if (retval >= 0) {
 125                 *length = retval;
 126                 return 0;
 127         }
 128         /* The socket is non-blocking, so expect EAGAIN when there is no data */
 129         if (errno == EAGAIN)
 130                 return 0;
 131         return -errno;
 132 }
 133
 134 void sandbox_eth_raw_os_stop(struct eth_sandbox_raw_priv *priv)
 135 {
 136         free(priv->device);
 137         priv->device = NULL;
 138         close(priv->sd);
 139         priv->sd = -1;
 140 }