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