ethsock.c

   1 #include <netinet/if_ether.h>
   2 #include <sys/socket.h>
   3 #include <sys/types.h>
   4 #include <stdbool.h>
   5 #include <string.h>
   6 #include <stdlib.h>
   7 #include <stdio.h>
   8 #include <pcap.h>
   9 #include "ethsock.h"
  10
  11 #if defined(_WIN32) || defined(_WIN64)
  12 #define NMRPFLASH_WINDOWS
  13 #elif defined(__linux__)
  14 #define NMRPFLASH_LINUX
  15 #elif defined(__APPLE__) && defined(__MACH__)
  16 #define NMRPFLASH_OSX
  17 #elif defined(__unix__)
  18 #define NMRPFLASH_UNIX
  19 #warning "nmrp-flash is not fully supported on your operating system"
  20 #endif
  21
  22 #if defined(NMRPFLASH_WINDOWS)
  23 #include <winsock2.h>
  24 #include <iphlpapi.h>
  25 #else
  26 #include <ifaddrs.h>
  27 #if defined(NMRPFLASH_LINUX)
  28 #include <linux/if_packet.h>
  29 #elif defined (NMRPFLASH_OSX)
  30 #include <net/if_dl.h>
  31 #endif
  32 #endif
  33
  34 #ifndef MIN
  35 #define MIN(a, b) ((a) < (b) ? (a) : (b))
  36 #endif
  37
  38 struct ethsock
  39 {
  40         pcap_t *pcap;
  41         struct timeval timeout;
  42         int fd;
  43         uint8_t hwaddr[6];
  44 };
  45
  46 #ifndef NMRPFLASH_WINDOWS
  47 static bool get_hwaddr(uint8_t *hwaddr, const char *interface)
  48 {
  49         struct ifaddrs *ifas, *ifa;
  50         void *src;
  51         bool found;
  52
  53         if (getifaddrs(&ifas) != 0) {
  54                 perror("getifaddrs");
  55                 return false;
  56         }
  57
  58         found = false;
  59
  60         for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
  61                 if (!strcmp(ifa->ifa_name, interface)) {
  62 #ifdef NMRPFLASH_LINUX
  63                         if (ifa->ifa_addr->sa_family != AF_PACKET) {
  64                                 continue;
  65                         }
  66                         src = ((struct sockaddr_ll*)ifa->ifa_addr)->sll_addr;
  67 #else
  68                         if (ifa->ifa_addr->sa_family != AF_LINK) {
  69                                 continue;
  70                         }
  71                         src = LLADDR((struct sockaddr_dl*)ifa->ifa_addr);
  72 #endif
  73                         memcpy(hwaddr, src, 6);
  74                         found = true;
  75                         break;
  76                 }
  77         }
  78
  79         freeifaddrs(ifas);
  80         return found;
  81 }
  82 #else
  83 static bool get_hwaddr(uint8_t *hwaddr, const char *interface)
  84 {
  85         PIP_ADAPTER_INFO adapters, adapter;
  86         DWORD ret;
  87         ULONG i, bufLen = 0;
  88         bool found = false;
  89
  90         if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
  91                 fprintf(stderr, "GetAdaptersInfo: error %d.\n", ret);
  92                 return false;
  93         }
  94
  95         adapters = malloc(bufLen);
  96         if (!adapters) {
  97                 perror("malloc");
  98                 return false;
  99         }
 100
 101         if ((ret = GetAdaptersInfo(adapters, bufLen) == NO_ERROR)) {
 102                 for (adapter = adapters; adapter; adapter = adapter->Next) {
 103                         if (adapter->Type != MIB_IF_TYPE_ETHERNET) {
 104                                 continue;
 105                         }
 106
 107                         if (!strcmp(adapter->AdapterName, interface)) {
 108                                 if (adapter->AddressLength == 6) {
 109                                         for (i = 0; i != 6; ++i) {
 110                                                 hwaddr[i] = adapter->Address[i];
 111                                         }
 112
 113                                         found = true;
 114                                         break;
 115                                 }
 116                         }
 117                 }
 118         } else {
 119                 fprintf(stderr, "GetAdaptersInfo: error %d.\n", ret);
 120         }
 121
 122         free(adapters);
 123         return found;
 124 }
 125 #endif
 126
 127
 128 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
 129 {
 130         return sock->hwaddr;
 131 }
 132
 133 struct ethsock *ethsock_create(const char *interface, uint16_t protocol)
 134 {
 135         char buf[PCAP_ERRBUF_SIZE];
 136         struct bpf_program fp;
 137         struct ethsock *sock;
 138         int err;
 139
 140         sock = malloc(sizeof(struct ethsock));
 141         if (!sock) {
 142                 perror("malloc");
 143                 return NULL;
 144         }
 145
 146         buf[0] = '\0';
 147
 148         sock->pcap = pcap_open_live(interface, BUFSIZ, 1, 1, buf);
 149         if (!sock->pcap) {
 150                 fprintf(stderr, "%s.\n", buf);
 151                 goto cleanup_malloc;
 152         }
 153
 154         if (*buf) {
 155                 fprintf(stderr, "Warning: %s.\n", buf);
 156         }
 157
 158         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
 159                 fprintf(stderr, "Interface %s is not an ethernet interface.\n",
 160                                 interface);
 161                 goto cleanup_pcap;
 162         }
 163
 164         if (!get_hwaddr(sock->hwaddr, interface)) {
 165                 fprintf(stderr, "Failed to get MAC address of interface.\n");
 166                 goto cleanup_malloc;
 167         }
 168
 169         sock->fd = pcap_get_selectable_fd(sock->pcap);
 170         if (sock->fd == -1) {
 171                 fprintf(stderr, "No selectable file descriptor available.\n");
 172                 goto cleanup_pcap;
 173         }
 174
 175         snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
 176         err = pcap_compile(sock->pcap, &fp, buf, 0, PCAP_NETMASK_UNKNOWN);
 177         if (err) {
 178                 pcap_perror(sock->pcap, "pcap_compile");
 179                 goto cleanup_pcap;
 180         }
 181
 182         if ((err = pcap_setfilter(sock->pcap, &fp))) {
 183                 pcap_perror(sock->pcap, "pcap_setfilter");
 184                 goto cleanup_pcap;
 185         }
 186
 187         return sock;
 188
 189 cleanup_pcap:
 190         pcap_close(sock->pcap);
 191 cleanup_malloc:
 192         free(sock);
 193         return NULL;
 194 }
 195
 196 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 197 {
 198         struct pcap_pkthdr* hdr;
 199         const u_char *capbuf;
 200         int status;
 201         fd_set fds;
 202
 203         if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
 204                 FD_ZERO(&fds);
 205                 FD_SET(sock->fd, &fds);
 206
 207                 status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
 208                 if (status == -1) {
 209                         perror("select");
 210                         return -1;
 211                 } else if (status == 0) {
 212                         return 0;
 213                 }
 214         }
 215
 216         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
 217         switch (status) {
 218                 case 1:
 219                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
 220                         return hdr->caplen;
 221                 case 0:
 222                         return 0;
 223                 case -1:
 224                         pcap_perror(sock->pcap, "pcap_next_ex");
 225                         return -1;
 226                 default:
 227                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 228                         return -1;
 229         }
 230 }
 231
 232 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 233 {
 234 #ifdef NMRPFLASH_WINDOWS
 235         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 236                 return 0;
 237         } else {
 238                 pcap_perror(sock->pcap, "pcap_sendpacket");
 239                 return -1;
 240         }
 241 #else
 242         if (pcap_inject(sock->pcap, buf, len) == len) {
 243                 return 0;
 244         } else {
 245                 pcap_perror(sock->pcap, "pcap_inject");
 246                 return -1;
 247         }
 248 #endif
 249 }
 250
 251 int ethsock_close(struct ethsock *sock)
 252 {
 253         pcap_close(sock->pcap);
 254         free(sock);
 255         return 0;
 256 }
 257
 258 int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 259 {
 260         sock->timeout.tv_sec = msec / 1000;
 261         sock->timeout.tv_usec = (msec % 1000) * 1000;
 262         return 0;
 263 }
 264
 265 static bool is_ethernet(const char *interface)
 266 {
 267         pcap_t *pcap;
 268         char errbuf[PCAP_ERRBUF_SIZE];
 269         bool ret = false;
 270
 271         if ((pcap = pcap_create(interface, errbuf))) {
 272                 if (pcap_activate(pcap) == 0) {
 273                         ret = (pcap_datalink(pcap) == DLT_EN10MB);
 274                 }
 275                 pcap_close(pcap);
 276         }
 277
 278         return ret;
 279 }
 280
 281 int ethsock_list_all(void)
 282 {
 283         pcap_if_t *devs, *dev;
 284         uint8_t hwaddr[6];
 285         char errbuf[PCAP_ERRBUF_SIZE];
 286
 287         if (pcap_findalldevs(&devs, errbuf) != 0) {
 288                 fprintf(stderr, "%s.\n", errbuf);
 289                 return -1;
 290         }
 291
 292         memset(hwaddr, 0, 6);
 293
 294         for (dev = devs; dev; dev = dev->next) {
 295                 if (dev->flags & PCAP_IF_LOOPBACK) {
 296                         continue;
 297                 }
 298
 299                 if (!is_ethernet(dev->name)) {
 300                         continue;
 301                 }
 302
 303                 if (!get_hwaddr(hwaddr, dev->name)) {
 304                         continue;
 305                 }
 306
 307                 printf("%s  %02x:%02x:%02x:%02x:%02x:%02x", dev->name, hwaddr[0],
 308                                 hwaddr[1], hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5]);
 309
 310                 if (dev->description) {
 311                         printf("    (%s)\n", dev->description);
 312                 } else {
 313                         printf("\n");
 314                 }
 315         }
 316
 317         return 0;
 318 }