Update README.md
[oweals/nmrpflash.git] / ethsock.c
index 65f1b6161587374307befc80d86980110dd75ed4..d754653ca11e48ab49e0e7da02315516b725d105 100644 (file)
--- a/ethsock.c
+++ b/ethsock.c
@@ -3,30 +3,21 @@
 #include <string.h>
 #include <stdlib.h>
 #include <stdio.h>
-#include <pcap.h>
-#include "ethsock.h"
-
-#if defined(_WIN32) || defined(_WIN64)
-#define NMRPFLASH_WINDOWS
-#elif defined(__linux__)
-#define NMRPFLASH_LINUX
-#elif defined(__APPLE__) && defined(__MACH__)
-#define NMRPFLASH_OSX
-#elif defined(__unix__)
-#define NMRPFLASH_UNIX
-#warning "nmrp-flash is not fully supported on your operating system"
-#endif
+#include "nmrpd.h"
 
 #if defined(NMRPFLASH_WINDOWS)
-#include <winsock2.h>
-#include <iphlpapi.h>
+#define NMRPFLASH_NETALIAS_PREFIX "net"
+#define WPCAP
+#include <pcap.h>
 #else
-#include <sys/socket.h>
+#include <pcap.h>
 #include <ifaddrs.h>
 #if defined(NMRPFLASH_LINUX)
+#define NMRPFLASH_AF_PACKET AF_PACKET
 #include <linux/if_packet.h>
-#elif defined (NMRPFLASH_OSX)
-#include <net/if_dl.h>
+#else
+#define NMRPFLASH_AF_PACKET AF_LINK
+#include <net/if_types.h>
 #endif
 #endif
 
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
 #endif
 
+
 struct ethsock
 {
        pcap_t *pcap;
 #ifndef NMRPFLASH_WINDOWS
-       struct timeval timeout;
        int fd;
 #else
-       DWORD timeout;
        HANDLE handle;
 #endif
+       unsigned timeout;
        uint8_t hwaddr[6];
 };
 
+const char *mac_to_str(uint8_t *mac)
+{
+       static char buf[18];
+       snprintf(buf, sizeof(buf), "%02x:%02x:%02x:%02x:%02x:%02x",
+                       mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+       return buf;
+}
+
+static int x_pcap_findalldevs(pcap_if_t **devs)
+{
+       char errbuf[PCAP_ERRBUF_SIZE];
+       if (pcap_findalldevs(devs, errbuf) != 0) {
+               fprintf(stderr, "%s.\n", errbuf);
+               return -1;
+       }
+
+       return 0;
+}
+
 #ifndef NMRPFLASH_WINDOWS
-static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
+static inline bool sockaddr_get_hwaddr(struct sockaddr *sa, uint8_t *hwaddr)
 {
-       struct ifaddrs *ifas, *ifa;
        void *src;
+
+       if (sa->sa_family != NMRPFLASH_AF_PACKET) {
+               return false;
+       }
+
+#ifndef NMRPFLASH_LINUX
+       if (((struct sockaddr_dl*)addr)->sdl_type != IFT_ETHER) {
+               return false;
+       }
+       src = LLADDR((struct sockaddr_dl*)sa);
+#else
+       src = ((struct sockaddr_ll*)sa)->sll_addr;
+#endif
+
+       memcpy(hwaddr, src, 6);
+       return true;
+}
+
+static bool get_hwaddr_from_intf(const char *intf, uint8_t *hwaddr)
+{
+       struct ifaddrs *ifas, *ifa;
        bool found;
 
        if (getifaddrs(&ifas) != 0) {
@@ -63,28 +93,37 @@ static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
 
        for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
                if (!strcmp(ifa->ifa_name, intf)) {
-#ifdef NMRPFLASH_LINUX
-                       if (ifa->ifa_addr->sa_family != AF_PACKET) {
-                               continue;
+                       if (sockaddr_get_hwaddr(ifa->ifa_addr, hwaddr)) {
+                               found = true;
+                               break;
                        }
-                       src = ((struct sockaddr_ll*)ifa->ifa_addr)->sll_addr;
-#else
-                       if (ifa->ifa_addr->sa_family != AF_LINK) {
-                               continue;
-                       }
-                       src = LLADDR((struct sockaddr_dl*)ifa->ifa_addr);
-#endif
-                       memcpy(hwaddr, src, 6);
-                       found = true;
-                       break;
                }
        }
 
        freeifaddrs(ifas);
        return found;
 }
+
 #else
-static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
+
+void win_perror2(const char *msg, DWORD err)
+{
+       char *buf = NULL;
+       FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER |
+                       FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
+                       NULL, err, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+                       (LPTSTR)&buf, 0, NULL);
+
+       if (buf) {
+               /* FormatMessageA terminates buf with CRLF! */
+               fprintf(stderr, "%s: %s", msg, buf);
+               LocalFree(buf);
+       } else {
+               fprintf(stderr, "%s: error %d\n", msg, (int)err);
+       }
+}
+
+static bool get_hwaddr_from_intf(const char *intf, uint8_t *hwaddr)
 {
        PIP_ADAPTER_INFO adapters, adapter;
        DWORD ret;
@@ -92,7 +131,7 @@ static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
        bool found = false;
 
        if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
-               fprintf(stderr, "GetAdaptersInfo: error %d.\n", (int)ret);
+               win_perror2("GetAdaptersInfo", ret);
                return false;
        }
 
@@ -108,7 +147,9 @@ static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
                                continue;
                        }
 
-                       if (!strcmp(adapter->AdapterName, intf)) {
+                       /* Interface names from WinPcap are "\Device\NPF_{GUID}", while
+                        * AdapterName from GetAdaptersInfo is just "{GUID}".*/
+                       if (strstr(intf, adapter->AdapterName)) {
                                if (adapter->AddressLength == 6) {
                                        for (i = 0; i != 6; ++i) {
                                                hwaddr[i] = adapter->Address[i];
@@ -120,14 +161,93 @@ static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
                        }
                }
        } else {
-               fprintf(stderr, "GetAdaptersInfo: error %d.\n", (int)ret);
+               win_perror2("GetAdaptersInfo", ret);
        }
 
        free(adapters);
        return found;
 }
-#endif
 
+static const char *intf_alias_to_wpcap(const char *intf)
+{
+       static char buf[128];
+       pcap_if_t *devs, *dev;
+       unsigned i = 0, dev_num = 0;
+
+       if (intf[0] == '\\') {
+               return intf;
+       } else if (sscanf(intf, NMRPFLASH_NETALIAS_PREFIX "%u", &dev_num) != 1) {
+               fprintf(stderr, "Invalid interface alias.\n");
+               return NULL;
+       }
+
+       if (x_pcap_findalldevs(&devs) != 0) {
+               return NULL;
+       }
+
+       for (dev = devs; dev; dev = dev->next, ++i) {
+               if (i == dev_num) {
+                       if (verbosity) {
+                               printf("%s%u: %s\n", NMRPFLASH_NETALIAS_PREFIX, i, dev->name);
+                       }
+                       strncpy(buf, dev->name, sizeof(buf) - 1);
+                       buf[sizeof(buf) - 1] = '\0';
+                       break;
+               }
+       }
+
+       pcap_freealldevs(devs);
+
+       if (!dev) {
+               fprintf(stderr, "Interface alias not found.\n");
+               return NULL;
+       }
+
+       return buf;
+}
+
+static const char *intf_get_pretty_name(const char *intf)
+{
+       static char buf[512];
+       char *guid;
+       HKEY hkey;
+       LONG err;
+       DWORD len;
+
+       guid = strstr(intf, "NPF_{");
+       if (!guid) {
+               return NULL;
+       }
+
+       guid += 4;
+
+       snprintf(buf, sizeof(buf),
+                       "System\\CurrentControlSet\\Control\\Network\\"
+                       "{4D36E972-E325-11CE-BFC1-08002BE10318}\\"
+                       "%s\\Connection", guid);
+       err = RegOpenKeyExA(HKEY_LOCAL_MACHINE, buf, 0, KEY_READ, &hkey);
+       if (err != ERROR_SUCCESS) {
+               if (verbosity > 1) {
+                       win_perror2("RegOpenKeyExA", err);
+               }
+               return NULL;
+       }
+
+       len = sizeof(buf);
+       err = RegQueryValueExA(hkey, "Name", NULL, NULL, (LPBYTE)buf, &len);
+       if (err == ERROR_SUCCESS) {
+               intf = buf;
+       } else {
+               if (verbosity > 1) {
+                       win_perror2("RegQueryValueExA", err);
+               }
+               intf = NULL;
+       }
+
+       RegCloseKey(hkey);
+       return intf;
+}
+#endif
 
 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
 {
@@ -147,6 +267,13 @@ struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
                return NULL;
        }
 
+#ifdef NMRPFLASH_WINDOWS
+       intf = intf_alias_to_wpcap(intf);
+       if (!intf) {
+               return NULL;
+       }
+#endif
+
        buf[0] = '\0';
 
        sock->pcap = pcap_open_live(intf, BUFSIZ, 1, 1, buf);
@@ -165,7 +292,7 @@ struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
                goto cleanup_pcap;
        }
 
-       if (!get_hwaddr(sock->hwaddr, intf)) {
+       if (!get_hwaddr_from_intf(intf, sock->hwaddr)) {
                fprintf(stderr, "Failed to get MAC address of interface.\n");
                goto cleanup_malloc;
        }
@@ -182,16 +309,27 @@ struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
                fprintf(stderr, "No event handle available.\n");
                goto cleanup_pcap;
        }
+
+       err = pcap_setmintocopy(sock->pcap, 1);
+       if (err) {
+               pcap_perror(sock->pcap, "pcap_setmintocopy");
+               goto cleanup_pcap;
+       }
 #endif
 
-       snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
+       snprintf(buf, sizeof(buf), "ether proto 0x%04x and not ether src %s",
+                       protocol, mac_to_str(sock->hwaddr));
+
        err = pcap_compile(sock->pcap, &fp, buf, 0, 0);
        if (err) {
                pcap_perror(sock->pcap, "pcap_compile");
                goto cleanup_pcap;
        }
 
-       if ((err = pcap_setfilter(sock->pcap, &fp))) {
+       err = pcap_setfilter(sock->pcap, &fp);
+       pcap_freecode(&fp);
+
+       if (err) {
                pcap_perror(sock->pcap, "pcap_setfilter");
                goto cleanup_pcap;
        }
@@ -205,39 +343,51 @@ cleanup_malloc:
        return NULL;
 }
 
+int select_fd(int fd, unsigned timeout)
+{
+       struct timeval tv;
+       int status;
+       fd_set fds;
+
+       FD_ZERO(&fds);
+       FD_SET(fd, &fds);
+
+       tv.tv_sec = timeout / 1000;
+       tv.tv_usec = 1000 * (timeout % 1000);
+
+       status = select(fd + 1, &fds, NULL, NULL, &tv);
+       if (status < 0) {
+               sock_perror("select");
+       }
+
+       return status;
+}
+
 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 {
        struct pcap_pkthdr* hdr;
        const u_char *capbuf;
        int status;
-#ifndef NMRPFLASH_WINDOWS
-       fd_set fds;
-#else
+#ifdef NMRPFLASH_WINDOWS
        DWORD ret;
-#endif
 
-#ifndef NMRPFLASH_WINDOWS
-       if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
-               FD_ZERO(&fds);
-               FD_SET(sock->fd, &fds);
-
-               status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
-               if (status == -1) {
-                       perror("select");
-                       return -1;
-               } else if (status == 0) {
-                       return 0;
-               }
-       }
-#else
        if (sock->timeout) {
                ret = WaitForSingleObject(sock->handle, sock->timeout);
                if (ret == WAIT_TIMEOUT) {
                        return 0;
                } else if (ret != WAIT_OBJECT_0) {
-                       fprintf(stderr, "WaitForSingleObject: returned %d\n", (int)ret);
+                       win_perror2("WaitForSingleObject", ret);
+                       return -1;
+               }
+       }
+#else
+       if (sock->timeout) {
+               status = select_fd(sock->fd, sock->timeout);
+               if (status < 0) {
                        return -1;
-               }               
+               } else if (status == 0) {
+                       return 0;
+               }
        }
 #endif
 
@@ -283,67 +433,110 @@ int ethsock_close(struct ethsock *sock)
        return 0;
 }
 
-int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
+inline int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 {
-#ifndef NMRPFLASH_WINDOWS
-       sock->timeout.tv_sec = msec / 1000;
-       sock->timeout.tv_usec = (msec % 1000) * 1000;
-#else
        sock->timeout = msec;
-#endif
        return 0;
 }
 
-static bool is_ethernet(const char *intf)
+static bool get_hwaddr_from_pcap(const pcap_if_t *dev, uint8_t *hwaddr)
 {
-       pcap_t *pcap;
-       char errbuf[PCAP_ERRBUF_SIZE];
-       bool ret = false;
+#ifndef NMRPFLASH_WINDOWS
+       pcap_addr_t *addr;
+       int i;
+
+       for (addr = dev->addresses; addr; addr = addr->next) {
+               if (verbosity > 1) {
+                       printf("%s: sa_family=%d, sa_data={ ", dev->name,
+                                       addr->addr->sa_family);
+                       for (i = 0; i != sizeof(addr->addr->sa_data); ++i) {
+                               printf("%02x ", addr->addr->sa_data[i] & 0xff);
+                       }
+                       printf("}\n");
+               }
 
-       if ((pcap = pcap_create(intf, errbuf))) {
-               if (pcap_activate(pcap) == 0) {
-                       ret = (pcap_datalink(pcap) == DLT_EN10MB);
+               if (sockaddr_get_hwaddr(addr->addr, hwaddr)) {
+                       return true;
                }
-               pcap_close(pcap);
        }
+#endif
 
-       return ret;
+       return get_hwaddr_from_intf(dev->name, hwaddr);
 }
 
 int ethsock_list_all(void)
 {
        pcap_if_t *devs, *dev;
+       pcap_addr_t *addr;
        uint8_t hwaddr[6];
-       char errbuf[PCAP_ERRBUF_SIZE];
+       unsigned dev_num = 0, dev_ok = 0;
+#ifdef NMRPFLASH_WINDOWS
+       const char *pretty;
+#endif
 
-       if (pcap_findalldevs(&devs, errbuf) != 0) {
-               fprintf(stderr, "%s.\n", errbuf);
+       if (x_pcap_findalldevs(&devs) != 0) {
                return -1;
        }
 
        memset(hwaddr, 0, 6);
 
-       for (dev = devs; dev; dev = dev->next) {
+       for (dev = devs; dev; dev = dev->next, ++dev_num) {
                if (dev->flags & PCAP_IF_LOOPBACK) {
+                       if (verbosity) {
+                               printf("%-15s  (loopback device)\n", dev->name);
+                       }
                        continue;
                }
 
-               if (!is_ethernet(dev->name)) {
+               if (!get_hwaddr_from_pcap(dev, hwaddr)) {
+                       if (verbosity) {
+                               printf("%-15s  (not an ethernet device)\n",
+                                               dev->name);
+                       }
                        continue;
                }
 
-               if (!get_hwaddr(hwaddr, dev->name)) {
-                       continue;
+#ifndef NMRPFLASH_WINDOWS
+               printf("%-15s", dev->name);
+#else
+               /* Call this here so *_perror() calls don't happen within a line */
+               pretty = intf_get_pretty_name(dev->name);
+
+               if (!verbosity) {
+                       printf("%s%u", NMRPFLASH_NETALIAS_PREFIX, dev_num);
+               } else {
+                       printf("%s", dev->name);
                }
+#endif
 
-               printf("%s  %02x:%02x:%02x:%02x:%02x:%02x", dev->name, hwaddr[0],
-                               hwaddr[1], hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5]);
+               for (addr = dev->addresses; addr; addr = addr->next) {
+                       if (addr->addr->sa_family == AF_INET) {
+                               printf("  %-15s",
+                                               inet_ntoa(((struct sockaddr_in*)addr->addr)->sin_addr));
+                               break;
+                       }
+               }
 
-               if (dev->description) {
-                       printf("    (%s)\n", dev->description);
-               } else {
-                       printf("\n");
+               if (!addr) {
+                       printf("  %-15s", "0.0.0.0");
+               }
+
+               printf("  %s", mac_to_str(hwaddr));
+
+#ifdef NMRPFLASH_WINDOWS
+               if (pretty) {
+                       printf("  (%s)", pretty);
+               } else if (dev->description) {
+                       printf("  (%s)", dev->description);
                }
+
+#endif
+               printf("\n");
+               ++dev_ok;
+       }
+
+       if (!dev_ok) {
+               printf("No suitable network interfaces found.\n");
        }
 
        return 0;