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