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