ethsock.c

   1 #include <sys/types.h>
   2 #include <stdbool.h>
   3 #include <string.h>
   4 #include <stdlib.h>
   5 #include <stdio.h>
   6 #include "ethsock.h"
   7 #include "nmrpd.h"
   8
   9 #if defined(NMRPFLASH_WINDOWS)
  10 #define NMRPFLASH_NETALIAS_PREFIX "net"
  11 #define WPCAP
  12 #include <pcap.h>
  13 #else
  14 #include <pcap.h>
  15 #include <ifaddrs.h>
  16 #if defined(NMRPFLASH_LINUX)
  17 #include <linux/if_packet.h>
  18 #elif defined (NMRPFLASH_OSX)
  19 #include <net/if_dl.h>
  20 #endif
  21 #endif
  22
  23 #ifndef MIN
  24 #define MIN(a, b) ((a) < (b) ? (a) : (b))
  25 #endif
  26
  27
  28 struct ethsock
  29 {
  30         pcap_t *pcap;
  31 #ifndef NMRPFLASH_WINDOWS
  32         struct timeval timeout;
  33         int fd;
  34 #else
  35         DWORD timeout;
  36         HANDLE handle;
  37 #endif
  38         uint8_t hwaddr[6];
  39 };
  40
  41 static int x_pcap_findalldevs(pcap_if_t **devs)
  42 {
  43         char errbuf[PCAP_ERRBUF_SIZE];
  44         if (pcap_findalldevs(devs, errbuf) != 0) {
  45                 fprintf(stderr, "%s.\n", errbuf);
  46                 return -1;
  47         }
  48
  49         return 0;
  50 }
  51
  52 #ifndef NMRPFLASH_WINDOWS
  53 static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
  54 {
  55         struct ifaddrs *ifas, *ifa;
  56         void *src;
  57         bool found;
  58
  59         if (getifaddrs(&ifas) != 0) {
  60                 perror("getifaddrs");
  61                 return false;
  62         }
  63
  64         found = false;
  65
  66         for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
  67                 if (!strcmp(ifa->ifa_name, intf)) {
  68 #ifdef NMRPFLASH_LINUX
  69                         if (ifa->ifa_addr->sa_family != AF_PACKET) {
  70                                 continue;
  71                         }
  72                         src = ((struct sockaddr_ll*)ifa->ifa_addr)->sll_addr;
  73 #else
  74                         if (ifa->ifa_addr->sa_family != AF_LINK) {
  75                                 continue;
  76                         }
  77                         src = LLADDR((struct sockaddr_dl*)ifa->ifa_addr);
  78 #endif
  79                         memcpy(hwaddr, src, 6);
  80                         found = true;
  81                         break;
  82                 }
  83         }
  84
  85         freeifaddrs(ifas);
  86         return found;
  87 }
  88 #else
  89
  90 static void win_perror2(const char *msg, int err)
  91 {
  92         fprintf(stderr, "%s: error %d\n", msg, err);
  93 }
  94
  95 static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
  96 {
  97         PIP_ADAPTER_INFO adapters, adapter;
  98         DWORD ret;
  99         ULONG i, bufLen = 0;
 100         bool found = false;
 101
 102         if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
 103                 win_perror2("GetAdaptersInfo", ret);
 104                 return false;
 105         }
 106
 107         adapters = malloc(bufLen);
 108         if (!adapters) {
 109                 perror("malloc");
 110                 return false;
 111         }
 112
 113         if ((ret = GetAdaptersInfo(adapters, &bufLen) == NO_ERROR)) {
 114                 for (adapter = adapters; adapter; adapter = adapter->Next) {
 115                         if (adapter->Type != MIB_IF_TYPE_ETHERNET) {
 116                                 continue;
 117                         }
 118
 119 #ifndef NMRPFLASH_WINDOWS
 120                         if (!strcmp(intf, adapter->AdapterName))
 121 #else
 122                         /* Interface names from WinPcap are "\Device\NPF_{GUID}", while
 123                          * AdapterName from GetAdaptersInfo is just "{GUID}".*/
 124                         if (strstr(intf, adapter->AdapterName))
 125 #endif
 126                         {
 127                                 if (adapter->AddressLength == 6) {
 128                                         for (i = 0; i != 6; ++i) {
 129                                                 hwaddr[i] = adapter->Address[i];
 130                                         }
 131
 132                                         found = true;
 133                                         break;
 134                                 }
 135                         }
 136                 }
 137         } else {
 138                 win_perror2("GetAdaptersInfo", ret);
 139         }
 140
 141         free(adapters);
 142         return found;
 143 }
 144
 145 static const char *intf_alias_to_wpcap(const char *intf)
 146 {
 147         static char buf[128];
 148         pcap_if_t *devs, *dev;
 149         unsigned i = 0, dev_num = 0;
 150
 151         if (intf[0] == '\\') {
 152                 return intf;
 153         } else if (sscanf(intf, NMRPFLASH_NETALIAS_PREFIX "%u", &dev_num) != 1) {
 154                 fprintf(stderr, "Invalid interface alias.\n");
 155                 return NULL;
 156         }
 157
 158         if (x_pcap_findalldevs(&devs) != 0) {
 159                 return NULL;
 160         }
 161
 162         for (dev = devs; dev; dev = dev->next) {
 163                 if (i == dev_num) {
 164                         if (verbosity) {
 165                                 printf("%s%u: %s\n", NMRPFLASH_NETALIAS_PREFIX, i, dev->name);
 166                         }
 167                         strncpy(buf, dev->name, sizeof(buf) - 1);
 168                         buf[sizeof(buf) - 1] = '\0';
 169                         break;
 170                 }
 171         }
 172
 173         pcap_freealldevs(devs);
 174
 175         if (!dev) {
 176                 fprintf(stderr, "Interface alias not found.\n");
 177                 return NULL;
 178         }
 179
 180         return buf;
 181 }
 182
 183 static const char *intf_get_pretty_name(const char *intf)
 184 {
 185         static char buf[512];
 186         char *guid;
 187         HKEY hkey;
 188         LONG err;
 189         DWORD len;
 190
 191         guid = strstr(intf, "NPF_{");
 192         if (!guid) {
 193                 return NULL;
 194         }
 195
 196         guid += 4;
 197
 198         snprintf(buf, sizeof(buf),
 199                         "System\\CurrentControlSet\\Control\\Network\\"
 200                         "{4D36E972-E325-11CE-BFC1-08002BE10318}\\"
 201                         "%s\\Connection", guid);
 202         err = RegOpenKeyExA(HKEY_LOCAL_MACHINE, buf, 0, KEY_READ, &hkey);
 203         if (err != ERROR_SUCCESS) {
 204                 win_perror2("RegOpenKeyExA", err);
 205                 return NULL;
 206         }
 207
 208         len = sizeof(buf);
 209         err = RegQueryValueExA(hkey, "Name", NULL, NULL, (LPBYTE)buf, &len);
 210         if (err == ERROR_SUCCESS) {
 211                 intf = buf;
 212         } else {
 213                 win_perror2("RegQueryValueExA", err);
 214                 intf = NULL;
 215         }
 216
 217         RegCloseKey(hkey);
 218         return intf;
 219 }
 220 #endif
 221
 222
 223 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
 224 {
 225         return sock->hwaddr;
 226 }
 227
 228 struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
 229 {
 230         char buf[PCAP_ERRBUF_SIZE];
 231         struct bpf_program fp;
 232         struct ethsock *sock;
 233         int err;
 234
 235         sock = malloc(sizeof(struct ethsock));
 236         if (!sock) {
 237                 perror("malloc");
 238                 return NULL;
 239         }
 240
 241 #ifdef NMRPFLASH_WINDOWS
 242         intf = intf_alias_to_wpcap(intf);
 243         if (!intf) {
 244                 return NULL;
 245         }
 246 #endif
 247
 248         buf[0] = '\0';
 249
 250         sock->pcap = pcap_open_live(intf, BUFSIZ, 1, 1, buf);
 251         if (!sock->pcap) {
 252                 fprintf(stderr, "%s.\n", buf);
 253                 goto cleanup_malloc;
 254         }
 255
 256         if (*buf) {
 257                 fprintf(stderr, "Warning: %s.\n", buf);
 258         }
 259
 260         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
 261                 fprintf(stderr, "%s is not an ethernet interface.\n",
 262                                 intf);
 263                 goto cleanup_pcap;
 264         }
 265
 266         if (!get_hwaddr(sock->hwaddr, intf)) {
 267                 fprintf(stderr, "Failed to get MAC address of interface.\n");
 268                 goto cleanup_malloc;
 269         }
 270
 271 #ifndef NMRPFLASH_WINDOWS
 272         sock->fd = pcap_get_selectable_fd(sock->pcap);
 273         if (sock->fd == -1) {
 274                 fprintf(stderr, "No selectable file descriptor available.\n");
 275                 goto cleanup_pcap;
 276         }
 277 #else
 278         sock->handle = pcap_getevent(sock->pcap);
 279         if (!sock->handle) {
 280                 fprintf(stderr, "No event handle available.\n");
 281                 goto cleanup_pcap;
 282         }
 283 #endif
 284
 285         snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
 286         err = pcap_compile(sock->pcap, &fp, buf, 0, 0);
 287         if (err) {
 288                 pcap_perror(sock->pcap, "pcap_compile");
 289                 goto cleanup_pcap;
 290         }
 291
 292         if ((err = pcap_setfilter(sock->pcap, &fp))) {
 293                 pcap_perror(sock->pcap, "pcap_setfilter");
 294                 goto cleanup_pcap;
 295         }
 296
 297         return sock;
 298
 299 cleanup_pcap:
 300         pcap_close(sock->pcap);
 301 cleanup_malloc:
 302         free(sock);
 303         return NULL;
 304 }
 305
 306 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 307 {
 308         struct pcap_pkthdr* hdr;
 309         const u_char *capbuf;
 310         int status;
 311 #ifndef NMRPFLASH_WINDOWS
 312         fd_set fds;
 313 #else
 314         DWORD ret;
 315 #endif
 316
 317 #ifndef NMRPFLASH_WINDOWS
 318         if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
 319                 FD_ZERO(&fds);
 320                 FD_SET(sock->fd, &fds);
 321
 322                 status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
 323                 if (status == -1) {
 324                         perror("select");
 325                         return -1;
 326                 } else if (status == 0) {
 327                         return 0;
 328                 }
 329         }
 330 #else
 331         if (sock->timeout) {
 332                 ret = WaitForSingleObject(sock->handle, sock->timeout);
 333                 if (ret == WAIT_TIMEOUT) {
 334                         return 0;
 335                 } else if (ret != WAIT_OBJECT_0) {
 336                         win_perror("WaitForSingleObject", ret);
 337                         return -1;
 338                 }
 339         }
 340 #endif
 341
 342         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
 343         switch (status) {
 344                 case 1:
 345                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
 346                         return hdr->caplen;
 347                 case 0:
 348                         return 0;
 349                 case -1:
 350                         pcap_perror(sock->pcap, "pcap_next_ex");
 351                         return -1;
 352                 default:
 353                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 354                         return -1;
 355         }
 356 }
 357
 358 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 359 {
 360 #ifdef NMRPFLASH_WINDOWS
 361         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 362                 return 0;
 363         } else {
 364                 pcap_perror(sock->pcap, "pcap_sendpacket");
 365                 return -1;
 366         }
 367 #else
 368         if (pcap_inject(sock->pcap, buf, len) == len) {
 369                 return 0;
 370         } else {
 371                 pcap_perror(sock->pcap, "pcap_inject");
 372                 return -1;
 373         }
 374 #endif
 375 }
 376
 377 int ethsock_close(struct ethsock *sock)
 378 {
 379         pcap_close(sock->pcap);
 380         free(sock);
 381         return 0;
 382 }
 383
 384 int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 385 {
 386 #ifndef NMRPFLASH_WINDOWS
 387         sock->timeout.tv_sec = msec / 1000;
 388         sock->timeout.tv_usec = (msec % 1000) * 1000;
 389 #else
 390         sock->timeout = msec;
 391 #endif
 392         return 0;
 393 }
 394
 395 static bool is_ethernet(const char *intf)
 396 {
 397         pcap_t *pcap;
 398         char errbuf[PCAP_ERRBUF_SIZE];
 399         bool ret = false;
 400
 401         if ((pcap = pcap_create(intf, errbuf))) {
 402                 if (pcap_activate(pcap) == 0) {
 403                         ret = (pcap_datalink(pcap) == DLT_EN10MB);
 404                 }
 405                 pcap_close(pcap);
 406         }
 407
 408         return ret;
 409 }
 410
 411 int ethsock_list_all(void)
 412 {
 413         pcap_if_t *devs, *dev;
 414         pcap_addr_t *addr;
 415         uint8_t hwaddr[6];
 416         unsigned dev_num = 0;
 417 #ifdef NMRPFLASH_WINDOWS
 418         const char *pretty;
 419 #endif
 420
 421         if (x_pcap_findalldevs(&devs) != 0) {
 422                 return -1;
 423         }
 424
 425         memset(hwaddr, 0, 6);
 426
 427         for (dev = devs; dev; dev = dev->next) {
 428                 if (!is_ethernet(dev->name)) {
 429                         if (verbosity > 1) {
 430                                 printf("%s  (not an ethernet device)\n",
 431                                                 dev->name);
 432                         }
 433                         continue;
 434                 }
 435
 436                 if (!get_hwaddr(hwaddr, dev->name)) {
 437                         if (verbosity > 1) {
 438                                 printf("%s  (failed to get hardware address)\n",
 439                                                 dev->name);
 440                         }
 441                         continue;
 442                 }
 443
 444 #ifndef NMRPFLASH_WINDOWS
 445                 printf("%s", dev->name);
 446 #else
 447                 printf(NMRPFLASH_ALIAS_PREFIX "%u", dev_num);
 448 #endif
 449
 450                 for (addr = dev->addresses; addr; addr = addr->next) {
 451                         if (addr->addr->sa_family == AF_INET) {
 452                                 printf("  %15s",
 453                                                 inet_ntoa(((struct sockaddr_in*)addr->addr)->sin_addr));
 454                                 break;
 455                         }
 456                 }
 457
 458                 if (!addr) {
 459                         printf("                 ");
 460                 }
 461
 462                 printf("  %02x:%02x:%02x:%02x:%02x:%02x", hwaddr[0], hwaddr[1],
 463                                 hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5]);
 464
 465 #ifdef NMRPFLASH_WINDOWS
 466                 if (verbosity) {
 467                         printf("  %s", dev->name);
 468                 }
 469
 470                 pretty = intf_get_pretty_name(dev->name);
 471                 if (pretty) {
 472                         printf("  (%s)", pretty);
 473                 } else if (dev->description) {
 474                         printf("  (%s)", dev->description);
 475                 }
 476
 477 #endif
 478                 printf("\n");
 479                 ++dev_num;
 480         }
 481
 482         if (!dev_num) {
 483                 printf("No suitable network interfaces found.\n");
 484         }
 485
 486         return 0;
 487 }