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