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 <pcap.h>
   7 #include "ethsock.h"
   8
   9 #if defined(_WIN32) || defined(_WIN64)
  10 #define NMRPFLASH_WINDOWS
  11 #elif defined(__linux__)
  12 #define NMRPFLASH_LINUX
  13 #elif defined(__APPLE__) && defined(__MACH__)
  14 #define NMRPFLASH_OSX
  15 #elif defined(__unix__)
  16 #define NMRPFLASH_UNIX
  17 #warning "nmrp-flash is not fully supported on your operating system"
  18 #endif
  19
  20 #if defined(NMRPFLASH_WINDOWS)
  21 #include <winsock2.h>
  22 #include <iphlpapi.h>
  23 #else
  24 #include <sys/socket.h>
  25 #include <ifaddrs.h>
  26 #if defined(NMRPFLASH_LINUX)
  27 #include <linux/if_packet.h>
  28 #elif defined (NMRPFLASH_OSX)
  29 #include <net/if_dl.h>
  30 #endif
  31 #endif
  32
  33 #ifndef MIN
  34 #define MIN(a, b) ((a) < (b) ? (a) : (b))
  35 #endif
  36
  37 struct ethsock
  38 {
  39         pcap_t *pcap;
  40         struct timeval timeout;
  41         int fd;
  42         uint8_t hwaddr[6];
  43 };
  44
  45 #ifndef NMRPFLASH_WINDOWS
  46 static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
  47 {
  48         struct ifaddrs *ifas, *ifa;
  49         void *src;
  50         bool found;
  51
  52         if (getifaddrs(&ifas) != 0) {
  53                 perror("getifaddrs");
  54                 return false;
  55         }
  56
  57         found = false;
  58
  59         for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
  60                 if (!strcmp(ifa->ifa_name, intf)) {
  61 #ifdef NMRPFLASH_LINUX
  62                         if (ifa->ifa_addr->sa_family != AF_PACKET) {
  63                                 continue;
  64                         }
  65                         src = ((struct sockaddr_ll*)ifa->ifa_addr)->sll_addr;
  66 #else
  67                         if (ifa->ifa_addr->sa_family != AF_LINK) {
  68                                 continue;
  69                         }
  70                         src = LLADDR((struct sockaddr_dl*)ifa->ifa_addr);
  71 #endif
  72                         memcpy(hwaddr, src, 6);
  73                         found = true;
  74                         break;
  75                 }
  76         }
  77
  78         freeifaddrs(ifas);
  79         return found;
  80 }
  81 #else
  82 static bool get_hwaddr(uint8_t *hwaddr, const char *intf)
  83 {
  84         PIP_ADAPTER_INFO adapters, adapter;
  85         DWORD ret;
  86         ULONG i, bufLen = 0;
  87         bool found = false;
  88
  89         if ((ret = GetAdaptersInfo(NULL, &bufLen)) != ERROR_BUFFER_OVERFLOW) {
  90                 fprintf(stderr, "GetAdaptersInfo: error %d.\n", ret);
  91                 return false;
  92         }
  93
  94         adapters = malloc(bufLen);
  95         if (!adapters) {
  96                 perror("malloc");
  97                 return false;
  98         }
  99
 100         if ((ret = GetAdaptersInfo(adapters, bufLen) == NO_ERROR)) {
 101                 for (adapter = adapters; adapter; adapter = adapter->Next) {
 102                         if (adapter->Type != MIB_IF_TYPE_ETHERNET) {
 103                                 continue;
 104                         }
 105
 106                         if (!strcmp(adapter->AdapterName, intf)) {
 107                                 if (adapter->AddressLength == 6) {
 108                                         for (i = 0; i != 6; ++i) {
 109                                                 hwaddr[i] = adapter->Address[i];
 110                                         }
 111
 112                                         found = true;
 113                                         break;
 114                                 }
 115                         }
 116                 }
 117         } else {
 118                 fprintf(stderr, "GetAdaptersInfo: error %d.\n", ret);
 119         }
 120
 121         free(adapters);
 122         return found;
 123 }
 124 #endif
 125
 126
 127 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
 128 {
 129         return sock->hwaddr;
 130 }
 131
 132 struct ethsock *ethsock_create(const char *intf, uint16_t protocol)
 133 {
 134         char buf[PCAP_ERRBUF_SIZE];
 135         struct bpf_program fp;
 136         struct ethsock *sock;
 137         int err;
 138
 139         sock = malloc(sizeof(struct ethsock));
 140         if (!sock) {
 141                 perror("malloc");
 142                 return NULL;
 143         }
 144
 145         buf[0] = '\0';
 146
 147         sock->pcap = pcap_open_live(intf, BUFSIZ, 1, 1, buf);
 148         if (!sock->pcap) {
 149                 fprintf(stderr, "%s.\n", buf);
 150                 goto cleanup_malloc;
 151         }
 152
 153         if (*buf) {
 154                 fprintf(stderr, "Warning: %s.\n", buf);
 155         }
 156
 157         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
 158                 fprintf(stderr, "%s is not an ethernet interface.\n",
 159                                 intf);
 160                 goto cleanup_pcap;
 161         }
 162
 163         if (!get_hwaddr(sock->hwaddr, intf)) {
 164                 fprintf(stderr, "Failed to get MAC address of interface.\n");
 165                 goto cleanup_malloc;
 166         }
 167
 168         sock->fd = pcap_get_selectable_fd(sock->pcap);
 169         if (sock->fd == -1) {
 170                 fprintf(stderr, "No selectable file descriptor available.\n");
 171                 goto cleanup_pcap;
 172         }
 173
 174         snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
 175         err = pcap_compile(sock->pcap, &fp, buf, 0, PCAP_NETMASK_UNKNOWN);
 176         if (err) {
 177                 pcap_perror(sock->pcap, "pcap_compile");
 178                 goto cleanup_pcap;
 179         }
 180
 181         if ((err = pcap_setfilter(sock->pcap, &fp))) {
 182                 pcap_perror(sock->pcap, "pcap_setfilter");
 183                 goto cleanup_pcap;
 184         }
 185
 186         return sock;
 187
 188 cleanup_pcap:
 189         pcap_close(sock->pcap);
 190 cleanup_malloc:
 191         free(sock);
 192         return NULL;
 193 }
 194
 195 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 196 {
 197         struct pcap_pkthdr* hdr;
 198         const u_char *capbuf;
 199         int status;
 200         fd_set fds;
 201
 202         if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
 203                 FD_ZERO(&fds);
 204                 FD_SET(sock->fd, &fds);
 205
 206                 status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
 207                 if (status == -1) {
 208                         perror("select");
 209                         return -1;
 210                 } else if (status == 0) {
 211                         return 0;
 212                 }
 213         }
 214
 215         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
 216         switch (status) {
 217                 case 1:
 218                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
 219                         return hdr->caplen;
 220                 case 0:
 221                         return 0;
 222                 case -1:
 223                         pcap_perror(sock->pcap, "pcap_next_ex");
 224                         return -1;
 225                 default:
 226                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 227                         return -1;
 228         }
 229 }
 230
 231 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 232 {
 233 #ifdef NMRPFLASH_WINDOWS
 234         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 235                 return 0;
 236         } else {
 237                 pcap_perror(sock->pcap, "pcap_sendpacket");
 238                 return -1;
 239         }
 240 #else
 241         if (pcap_inject(sock->pcap, buf, len) == len) {
 242                 return 0;
 243         } else {
 244                 pcap_perror(sock->pcap, "pcap_inject");
 245                 return -1;
 246         }
 247 #endif
 248 }
 249
 250 int ethsock_close(struct ethsock *sock)
 251 {
 252         pcap_close(sock->pcap);
 253         free(sock);
 254         return 0;
 255 }
 256
 257 int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 258 {
 259         sock->timeout.tv_sec = msec / 1000;
 260         sock->timeout.tv_usec = (msec % 1000) * 1000;
 261         return 0;
 262 }
 263
 264 static bool is_ethernet(const char *intf)
 265 {
 266         pcap_t *pcap;
 267         char errbuf[PCAP_ERRBUF_SIZE];
 268         bool ret = false;
 269
 270         if ((pcap = pcap_create(intf, errbuf))) {
 271                 if (pcap_activate(pcap) == 0) {
 272                         ret = (pcap_datalink(pcap) == DLT_EN10MB);
 273                 }
 274                 pcap_close(pcap);
 275         }
 276
 277         return ret;
 278 }
 279
 280 int ethsock_list_all(void)
 281 {
 282         pcap_if_t *devs, *dev;
 283         uint8_t hwaddr[6];
 284         char errbuf[PCAP_ERRBUF_SIZE];
 285
 286         if (pcap_findalldevs(&devs, errbuf) != 0) {
 287                 fprintf(stderr, "%s.\n", errbuf);
 288                 return -1;
 289         }
 290
 291         memset(hwaddr, 0, 6);
 292
 293         for (dev = devs; dev; dev = dev->next) {
 294                 if (dev->flags & PCAP_IF_LOOPBACK) {
 295                         continue;
 296                 }
 297
 298                 if (!is_ethernet(dev->name)) {
 299                         continue;
 300                 }
 301
 302                 if (!get_hwaddr(hwaddr, dev->name)) {
 303                         continue;
 304                 }
 305
 306                 printf("%s  %02x:%02x:%02x:%02x:%02x:%02x", dev->name, hwaddr[0],
 307                                 hwaddr[1], hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5]);
 308
 309                 if (dev->description) {
 310                         printf("    (%s)\n", dev->description);
 311                 } else {
 312                         printf("\n");
 313                 }
 314         }
 315
 316         return 0;
 317 }