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