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