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