ethsock.c

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