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 "nmrpd.h"
   7
   8 #if defined(NMRPFLASH_WINDOWS)
   9 #define NMRPFLASH_NETALIAS_PREFIX "net"
  10 #define WPCAP
  11 #include <pcap.h>
  12 #else
  13 #include <pcap.h>
  14 #include <ifaddrs.h>
  15 #if defined(NMRPFLASH_LINUX)
  16 #define NMRPFLASH_AF_PACKET AF_PACKET
  17 #include <linux/if_packet.h>
  18 #else
  19 #define NMRPFLASH_AF_PACKET AF_LINK
  20 #include <net/if_types.h>
  21 #endif
  22 #endif
  23
  24 struct ethsock
  25 {
  26         const char *intf;
  27         pcap_t *pcap;
  28 #ifndef NMRPFLASH_WINDOWS
  29         int fd;
  30 #else
  31         HANDLE handle;
  32         DWORD index;
  33 #endif
  34         unsigned timeout;
  35         uint8_t hwaddr[6];
  36 };
  37
  38 const char *mac_to_str(uint8_t *mac)
  39 {
  40         static char buf[18];
  41         snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x",
  42                         mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  43         return buf;
  44 }
  45
  46 static int x_pcap_findalldevs(pcap_if_t **devs)
  47 {
  48         char errbuf[PCAP_ERRBUF_SIZE];
  49         if (pcap_findalldevs(devs, errbuf) != 0) {
  50                 fprintf(stderr, "%s.\n", errbuf);
  51                 return -1;
  52         }
  53
  54         return 0;
  55 }
  56
  57 #ifndef NMRPFLASH_WINDOWS
  58 static inline bool sockaddr_get_hwaddr(struct sockaddr *sa, uint8_t *hwaddr)
  59 {
  60         void *src;
  61
  62         if (sa->sa_family != NMRPFLASH_AF_PACKET) {
  63                 return false;
  64         }
  65
  66 #ifndef NMRPFLASH_LINUX
  67         if (((struct sockaddr_dl*)sa)->sdl_type != IFT_ETHER) {
  68                 return false;
  69         }
  70         src = LLADDR((struct sockaddr_dl*)sa);
  71 #else
  72         src = ((struct sockaddr_ll*)sa)->sll_addr;
  73 #endif
  74
  75         memcpy(hwaddr, src, 6);
  76         return true;
  77 }
  78
  79 static bool get_intf_info(const char *intf, uint8_t *hwaddr, void *dummy)
  80 {
  81         struct ifaddrs *ifas, *ifa;
  82         bool found;
  83
  84         if (getifaddrs(&ifas) != 0) {
  85                 perror("getifaddrs");
  86                 return false;
  87         }
  88
  89         found = false;
  90
  91         for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
  92                 if (!strcmp(ifa->ifa_name, intf)) {
  93                         if (sockaddr_get_hwaddr(ifa->ifa_addr, hwaddr)) {
  94                                 found = true;
  95                                 break;
  96                         }
  97                 }
  98         }
  99
 100         freeifaddrs(ifas);
 101         return found;
 102 }
 103
 104 #else
 105
 106 void win_perror2(const char *msg, DWORD err)
 107 {
 108         char *buf = NULL;
 109         FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER |
 110                         FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
 111                         NULL, err, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
 112                         (LPTSTR)&buf, 0, NULL);
 113
 114         if (buf) {
 115                 /* FormatMessageA terminates buf with CRLF! */
 116                 fprintf(stderr, "%s: %s", msg, buf);
 117                 LocalFree(buf);
 118         } else {
 119                 fprintf(stderr, "%s: error %d\n", msg, (int)err);
 120         }
 121 }
 122
 123 static bool get_intf_info(const char *intf, uint8_t *hwaddr, DWORD *index)
 124 {
 125         PIP_ADAPTER_INFO adapters, adapter;
 126         DWORD ret;
 127         ULONG i, bufLen = 0;
 128         bool found = false;
 129
 130         if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
 131                 win_perror2("GetAdaptersInfo", ret);
 132                 return false;
 133         }
 134
 135         adapters = malloc(bufLen);
 136         if (!adapters) {
 137                 perror("malloc");
 138                 return false;
 139         }
 140
 141         if ((ret = GetAdaptersInfo(adapters, &bufLen) == NO_ERROR)) {
 142                 for (adapter = adapters; adapter; adapter = adapter->Next) {
 143                         if (adapter->Type != MIB_IF_TYPE_ETHERNET) {
 144                                 continue;
 145                         }
 146
 147                         /* Interface names from WinPcap are "\Device\NPF_{GUID}", while
 148                          * AdapterName from GetAdaptersInfo is just "{GUID}".*/
 149                         if (strstr(intf, adapter->AdapterName)) {
 150                                 if (adapter->AddressLength == 6) {
 151                                         memcpy(hwaddr, adapter->Address, 6);
 152                                         if (index) {
 153                                                 *index = adapter->Index;
 154                                         }
 155                                         found = true;
 156                                         break;
 157                                 }
 158                         }
 159                 }
 160         } else {
 161                 win_perror2("GetAdaptersInfo", ret);
 162         }
 163
 164         free(adapters);
 165         return found;
 166 }
 167
 168 static const char *intf_alias_to_wpcap(const char *intf)
 169 {
 170         static char buf[128];
 171         pcap_if_t *devs, *dev;
 172         unsigned i = 0, dev_num = 0;
 173
 174         if (intf[0] == '\\') {
 175                 return intf;
 176         } else if (sscanf(intf, NMRPFLASH_NETALIAS_PREFIX "%u", &dev_num) != 1) {
 177                 fprintf(stderr, "Invalid interface alias.\n");
 178                 return NULL;
 179         }
 180
 181         if (x_pcap_findalldevs(&devs) != 0) {
 182                 return NULL;
 183         }
 184
 185         for (dev = devs; dev; dev = dev->next, ++i) {
 186                 if (i == dev_num) {
 187                         if (verbosity) {
 188                                 printf("%s%u: %s\n", NMRPFLASH_NETALIAS_PREFIX, i, dev->name);
 189                         }
 190                         strncpy(buf, dev->name, sizeof(buf) - 1);
 191                         buf[sizeof(buf) - 1] = '\0';
 192                         break;
 193                 }
 194         }
 195
 196         pcap_freealldevs(devs);
 197
 198         if (!dev) {
 199                 fprintf(stderr, "Interface alias not found.\n");
 200                 return NULL;
 201         }
 202
 203         return buf;
 204 }
 205
 206 static const char *intf_get_pretty_name(const char *intf)
 207 {
 208         static char buf[512];
 209         char *guid;
 210         HKEY hkey;
 211         LONG err;
 212         DWORD len;
 213
 214         guid = strstr(intf, "NPF_{");
 215         if (!guid) {
 216                 return NULL;
 217         }
 218
 219         guid += 4;
 220
 221         snprintf(buf, sizeof(buf),
 222                         "System\\CurrentControlSet\\Control\\Network\\"
 223                         "{4D36E972-E325-11CE-BFC1-08002BE10318}\\"
 224                         "%s\\Connection", guid);
 225         err = RegOpenKeyExA(HKEY_LOCAL_MACHINE, buf, 0, KEY_READ, &hkey);
 226         if (err != ERROR_SUCCESS) {
 227                 if (verbosity > 1) {
 228                         win_perror2("RegOpenKeyExA", err);
 229                 }
 230                 return NULL;
 231         }
 232
 233         len = sizeof(buf);
 234         err = RegQueryValueExA(hkey, "Name", NULL, NULL, (LPBYTE)buf, &len);
 235         if (err == ERROR_SUCCESS) {
 236                 intf = buf;
 237         } else {
 238                 if (verbosity > 1) {
 239                         win_perror2("RegQueryValueExA", err);
 240                 }
 241                 intf = NULL;
 242         }
 243
 244         RegCloseKey(hkey);
 245         return intf;
 246 }
 247 #endif
 248
 249 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
 250 {
 251         return sock->hwaddr;
 252 }
 253
 254 struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
 255 {
 256         char buf[PCAP_ERRBUF_SIZE];
 257         struct bpf_program fp;
 258         struct ethsock *sock;
 259         int err;
 260
 261         sock = malloc(sizeof(struct ethsock));
 262         if (!sock) {
 263                 perror("malloc");
 264                 return NULL;
 265         }
 266
 267 #ifdef NMRPFLASH_WINDOWS
 268         intf = intf_alias_to_wpcap(intf);
 269         if (!intf) {
 270                 return NULL;
 271         }
 272 #endif
 273
 274         buf[0] = '\0';
 275
 276         sock->intf = intf;
 277         sock->pcap = pcap_open_live(sock->intf, BUFSIZ, 1, 1, buf);
 278         if (!sock->pcap) {
 279                 fprintf(stderr, "%s.\n", buf);
 280                 goto cleanup_malloc;
 281         }
 282
 283         if (*buf) {
 284                 fprintf(stderr, "Warning: %s.\n", buf);
 285         }
 286
 287         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
 288                 fprintf(stderr, "%s is not an ethernet interface.\n",
 289                                 intf);
 290                 goto cleanup_pcap;
 291         }
 292
 293 #ifndef NMRPFLASH_WINDOWS
 294         err = !get_intf_info(intf, sock->hwaddr, NULL);
 295 #else
 296         err = !get_intf_info(intf, sock->hwaddr, &sock->index);
 297 #endif
 298         if (err) {
 299                 fprintf(stderr, "Failed to get interface info.\n");
 300                 goto cleanup_malloc;
 301         }
 302
 303 #ifndef NMRPFLASH_WINDOWS
 304         sock->fd = pcap_get_selectable_fd(sock->pcap);
 305         if (sock->fd == -1) {
 306                 pcap_perror(sock->pcap, "pcap_get_selectable_fd");
 307                 goto cleanup_pcap;
 308         }
 309 #else
 310         sock->handle = pcap_getevent(sock->pcap);
 311         if (!sock->handle) {
 312                 pcap_perror(sock->pcap, "pcap_getevent");
 313                 goto cleanup_pcap;
 314         }
 315
 316         err = pcap_setmintocopy(sock->pcap, 1);
 317         if (err) {
 318                 pcap_perror(sock->pcap, "pcap_setmintocopy");
 319                 goto cleanup_pcap;
 320         }
 321 #endif
 322
 323         snprintf(buf, sizeof(buf), "ether proto 0x%04x and not ether src %s",
 324                         protocol, mac_to_str(sock->hwaddr));
 325
 326         err = pcap_compile(sock->pcap, &fp, buf, 0, 0);
 327         if (err) {
 328                 pcap_perror(sock->pcap, "pcap_compile");
 329                 goto cleanup_pcap;
 330         }
 331
 332         err = pcap_setfilter(sock->pcap, &fp);
 333         pcap_freecode(&fp);
 334
 335         if (err) {
 336                 pcap_perror(sock->pcap, "pcap_setfilter");
 337                 goto cleanup_pcap;
 338         }
 339
 340         return sock;
 341
 342 cleanup_pcap:
 343         pcap_close(sock->pcap);
 344 cleanup_malloc:
 345         free(sock);
 346         return NULL;
 347 }
 348
 349 int select_fd(int fd, unsigned timeout)
 350 {
 351         struct timeval tv;
 352         int status;
 353         fd_set fds;
 354
 355         FD_ZERO(&fds);
 356         FD_SET(fd, &fds);
 357
 358         tv.tv_sec = timeout / 1000;
 359         tv.tv_usec = 1000 * (timeout % 1000);
 360
 361         status = select(fd + 1, &fds, NULL, NULL, &tv);
 362         if (status < 0) {
 363                 sock_perror("select");
 364         }
 365
 366         return status;
 367 }
 368
 369 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 370 {
 371         struct pcap_pkthdr* hdr;
 372         const u_char *capbuf;
 373         int status;
 374 #ifdef NMRPFLASH_WINDOWS
 375         DWORD ret;
 376
 377         if (sock->timeout) {
 378                 ret = WaitForSingleObject(sock->handle, sock->timeout);
 379                 if (ret == WAIT_TIMEOUT) {
 380                         return 0;
 381                 } else if (ret != WAIT_OBJECT_0) {
 382                         win_perror2("WaitForSingleObject", ret);
 383                         return -1;
 384                 }
 385         }
 386 #else
 387         if (sock->timeout) {
 388                 status = select_fd(sock->fd, sock->timeout);
 389                 if (status < 0) {
 390                         return -1;
 391                 } else if (status == 0) {
 392                         return 0;
 393                 }
 394         }
 395 #endif
 396
 397         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
 398         switch (status) {
 399                 case 1:
 400                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
 401                         return hdr->caplen;
 402                 case 0:
 403                         return 0;
 404                 case -1:
 405                         pcap_perror(sock->pcap, "pcap_next_ex");
 406                         return -1;
 407                 default:
 408                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 409                         return -1;
 410         }
 411 }
 412
 413 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 414 {
 415 #ifdef NMRPFLASH_WINDOWS
 416         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 417                 return 0;
 418         } else {
 419                 pcap_perror(sock->pcap, "pcap_sendpacket");
 420                 return -1;
 421         }
 422 #else
 423         if (pcap_inject(sock->pcap, buf, len) == len) {
 424                 return 0;
 425         } else {
 426                 pcap_perror(sock->pcap, "pcap_inject");
 427                 return -1;
 428         }
 429 #endif
 430 }
 431
 432 int ethsock_close(struct ethsock *sock)
 433 {
 434         pcap_close(sock->pcap);
 435         free(sock);
 436         return 0;
 437 }
 438
 439 inline int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 440 {
 441         sock->timeout = msec;
 442         return 0;
 443 }
 444
 445 #ifndef NMRPFLASH_WINDOWS
 446 int ethsock_arp_add(struct ethsock *sock, uint8_t *hwaddr, struct in_addr *ipaddr)
 447 {
 448         return 0;
 449 }
 450
 451 int ethsock_arp_del(struct ethsock *sock, uint8_t *hwaddr, struct in_addr *ipaddr)
 452 {
 453         return 0;
 454 }
 455 #else
 456 static int ethsock_arp(struct ethsock *sock, uint8_t *hwaddr, struct in_addr *ipaddr, int add)
 457 {
 458         DWORD ret;
 459         MIB_IPNETROW arp = {
 460                 .dwIndex = sock->index,
 461                 .dwPhysAddrLen = 6,
 462                 .dwAddr = ipaddr->s_addr,
 463                 .dwType = MIB_IPNET_TYPE_STATIC
 464         };
 465
 466         memcpy(arp.bPhysAddr, hwaddr, 6);
 467
 468         if (add) {
 469                 ret = CreateIpNetEntry(&arp);
 470                 if (ret != NO_ERROR) {
 471                         win_perror2("CreateIpNetEntry", ret);
 472                         return -1;
 473                 }
 474         } else {
 475                 DeleteIpNetEntry(&arp);
 476         }
 477
 478         return 0;
 479 }
 480
 481 int ethsock_arp_add(struct ethsock *sock, uint8_t *hwaddr, struct in_addr *ipaddr)
 482 {
 483         ethsock_arp_del(sock, hwaddr, ipaddr);
 484         return ethsock_arp(sock, hwaddr, ipaddr, 1);
 485 }
 486
 487 int ethsock_arp_del(struct ethsock *sock, uint8_t *hwaddr, struct in_addr *ipaddr)
 488 {
 489         return ethsock_arp(sock, hwaddr, ipaddr, 0);
 490 }
 491 #endif
 492
 493 static bool get_hwaddr_from_pcap(const pcap_if_t *dev, uint8_t *hwaddr)
 494 {
 495 #ifndef NMRPFLASH_WINDOWS
 496         pcap_addr_t *addr;
 497         int i;
 498
 499         for (addr = dev->addresses; addr; addr = addr->next) {
 500                 if (verbosity > 1) {
 501                         printf("%s: sa_family=%d, sa_data={ ", dev->name,
 502                                         addr->addr->sa_family);
 503                         for (i = 0; i != sizeof(addr->addr->sa_data); ++i) {
 504                                 printf("%02x ", addr->addr->sa_data[i] & 0xff);
 505                         }
 506                         printf("}\n");
 507                 }
 508
 509                 if (sockaddr_get_hwaddr(addr->addr, hwaddr)) {
 510                         return true;
 511                 }
 512         }
 513 #endif
 514
 515         return get_intf_info(dev->name, hwaddr, NULL);
 516 }
 517
 518 int ethsock_list_all(void)
 519 {
 520         pcap_if_t *devs, *dev;
 521         pcap_addr_t *addr;
 522         uint8_t hwaddr[6];
 523         unsigned dev_num = 0, dev_ok = 0;
 524 #ifdef NMRPFLASH_WINDOWS
 525         const char *pretty;
 526 #endif
 527
 528         if (x_pcap_findalldevs(&devs) != 0) {
 529                 return -1;
 530         }
 531
 532         memset(hwaddr, 0, 6);
 533
 534         for (dev = devs; dev; dev = dev->next, ++dev_num) {
 535                 if (dev->flags & PCAP_IF_LOOPBACK) {
 536                         if (verbosity) {
 537                                 printf("%-15s  (loopback device)\n", dev->name);
 538                         }
 539                         continue;
 540                 }
 541
 542                 if (!get_hwaddr_from_pcap(dev, hwaddr)) {
 543                         if (verbosity) {
 544                                 printf("%-15s  (not an ethernet device)\n",
 545                                                 dev->name);
 546                         }
 547                         continue;
 548                 }
 549
 550 #ifndef NMRPFLASH_WINDOWS
 551                 printf("%-15s", dev->name);
 552 #else
 553                 /* Call this here so *_perror() calls don't happen within a line */
 554                 pretty = intf_get_pretty_name(dev->name);
 555
 556                 if (!verbosity) {
 557                         printf("%s%u", NMRPFLASH_NETALIAS_PREFIX, dev_num);
 558                 } else {
 559                         printf("%s", dev->name);
 560                 }
 561 #endif
 562
 563                 for (addr = dev->addresses; addr; addr = addr->next) {
 564                         if (addr->addr->sa_family == AF_INET) {
 565                                 printf("  %-15s",
 566                                                 inet_ntoa(((struct sockaddr_in*)addr->addr)->sin_addr));
 567                                 break;
 568                         }
 569                 }
 570
 571                 if (!addr) {
 572                         printf("  %-15s", "0.0.0.0");
 573                 }
 574
 575                 printf("  %s", mac_to_str(hwaddr));
 576
 577 #ifdef NMRPFLASH_WINDOWS
 578                 if (pretty) {
 579                         printf("  (%s)", pretty);
 580                 } else if (dev->description) {
 581                         printf("  (%s)", dev->description);
 582                 }
 583
 584 #endif
 585                 printf("\n");
 586                 ++dev_ok;
 587         }
 588
 589         if (!dev_ok) {
 590                 printf("No suitable network interfaces found.\n");
 591         }
 592
 593         return 0;
 594 }
 595
 596 int ethsock_for_each_ip(struct ethsock *sock, ethsock_ip_callback_t callback,
 597                 void *arg)
 598 {
 599         struct ethsock_ip_callback_args args;
 600         pcap_if_t *devs, *dev;
 601         pcap_addr_t *addr;
 602         int status = 0;
 603
 604         if (x_pcap_findalldevs(&devs) != 0) {
 605                 return -1;
 606         }
 607
 608         args.arg = arg;
 609
 610         for (dev = devs; dev; dev = dev->next) {
 611                 if (strcmp(sock->intf, dev->name)) {
 612                         continue;
 613                 }
 614
 615                 for (addr = dev->addresses; addr; addr = addr->next) {
 616                         if (addr->addr->sa_family == AF_INET) {
 617                                 args.ipaddr = &((struct sockaddr_in*)addr->addr)->sin_addr;
 618                                 args.ipmask = &((struct sockaddr_in*)addr->netmask)->sin_addr;
 619
 620                                 status = callback(&args);
 621                                 if (status <= 0) {
 622                                         break;
 623                                 }
 624                         }
 625                 }
 626
 627                 break;
 628         }
 629
 630         pcap_freealldevs(devs);
 631
 632         return status <= 0 ? status : 0;
 633 }