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