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