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