ethsock.c

   1 #include <sys/socket.h>
   2 #include <sys/types.h>
   3 #include <net/if_dl.h>
   4 #include <stdbool.h>
   5 #include <ifaddrs.h>
   6 #include <string.h>
   7 #include <stdlib.h>
   8 #include <stdio.h>
   9 #include <pcap.h>
  10 #include "ethsock.h"
  11
  12 #ifndef MIN
  13 #define MIN(a, b) ((a) < (b) ? (a) : (b))
  14 #endif
  15
  16 struct ethsock
  17 {
  18         pcap_t *pcap;
  19         struct timeval timeout;
  20         int fd;
  21         uint8_t hwaddr[6];
  22 };
  23
  24 static bool ethsock_fill_hwaddr(struct ethsock *sock, const char *interface)
  25 {
  26         struct ifaddrs *ifas, *ifa;
  27         void *src;
  28         bool found;
  29
  30         if (getifaddrs(&ifas) != 0) {
  31                 perror("getifaddrs");
  32                 return false;
  33         }
  34
  35         found = false;
  36
  37         for (ifa = ifas; ifa; ifa = ifa->ifa_next) {
  38                 if (!strcmp(ifa->ifa_name, interface)) {
  39 #ifdef __linux__
  40                         if (ifa->ifa_addr->sa_family != AF_PACKET) {
  41                                 continue;
  42                         }
  43                         src = ((struct sockaddr_ll*)ifa->ifa_addr)->sll_addr;
  44 #else
  45                         if (ifa->ifa_addr->sa_family != AF_LINK) {
  46                                 continue;
  47                         }
  48                         src = LLADDR((struct sockaddr_dl*)ifa->ifa_addr);
  49 #endif
  50                         memcpy(sock->hwaddr, src, 6);
  51                         found = true;
  52                         break;
  53                 }
  54         }
  55
  56         if (!found) {
  57                 fprintf(stderr, "Failed to get MAC address of interface %s.\n", interface);
  58         }
  59
  60         freeifaddrs(ifas);
  61         return found;
  62 }
  63
  64 inline uint8_t *ethsock_get_hwaddr(struct ethsock *sock)
  65 {
  66         return sock->hwaddr;
  67 }
  68
  69 struct ethsock *ethsock_create(const char *interface, uint16_t protocol)
  70 {
  71         char buf[PCAP_ERRBUF_SIZE];
  72         struct bpf_program fp;
  73         struct ethsock *sock;
  74         int err;
  75
  76         sock = malloc(sizeof(struct ethsock));
  77         if (!sock) {
  78                 perror("malloc");
  79                 return NULL;
  80         }
  81
  82         if (!ethsock_fill_hwaddr(sock, interface)) {
  83                 goto cleanup_malloc;
  84         }
  85
  86         buf[0] = '\0';
  87
  88         sock->pcap = pcap_open_live(interface, BUFSIZ, 1, 1, buf);
  89         if (!sock->pcap) {
  90                 fprintf(stderr, "%s\n", buf);
  91                 goto cleanup_malloc;
  92         }
  93
  94         if (*buf) {
  95                 fprintf(stderr, "Warning: %s.\n", buf);
  96         }
  97
  98         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
  99                 fprintf(stderr, "Interface %s not supported.\n", interface);
 100                 goto cleanup_pcap;
 101         }
 102
 103         sock->fd = pcap_get_selectable_fd(sock->pcap);
 104         if (sock->fd == -1) {
 105                 fprintf(stderr, "No selectable file descriptor available.\n");
 106                 goto cleanup_pcap;
 107         }
 108
 109         snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
 110         err = pcap_compile(sock->pcap, &fp, buf, 0, PCAP_NETMASK_UNKNOWN);
 111         if (err) {
 112                 pcap_perror(sock->pcap, "pcap_compile");
 113                 goto cleanup_pcap;
 114         }
 115
 116         if ((err = pcap_setfilter(sock->pcap, &fp))) {
 117                 pcap_perror(sock->pcap, "pcap_setfilter");
 118                 goto cleanup_pcap;
 119         }
 120
 121         return sock;
 122
 123 cleanup_pcap:
 124         pcap_close(sock->pcap);
 125 cleanup_malloc:
 126         free(sock);
 127         return NULL;
 128 }
 129
 130 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
 131 {
 132         struct pcap_pkthdr* hdr;
 133         const u_char *capbuf;
 134         int status;
 135         fd_set fds;
 136
 137         if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
 138                 FD_ZERO(&fds);
 139                 FD_SET(sock->fd, &fds);
 140
 141                 status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
 142                 if (status == -1) {
 143                         perror("select");
 144                         return -1;
 145                 } else if (status == 0) {
 146                         return 1;
 147                 }
 148         }
 149
 150         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
 151         switch (status) {
 152                 case 1:
 153                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
 154                         return hdr->caplen;
 155                 case 0:
 156                         return 0;
 157                 case -1:
 158                         pcap_perror(sock->pcap, "pcap_next_ex");
 159                         return -1;
 160                 default:
 161                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 162                         return -1;
 163         }
 164 }
 165
 166 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 167 {
 168 #if defined(_WIN32) || defined(_WIN64)
 169         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 170                 return 0;
 171         } else {
 172                 pcap_perror(sock->pcap, "pcap_sendpacket");
 173                 return -1;
 174         }
 175 #else
 176         if (pcap_inject(sock->pcap, buf, len) == len) {
 177                 return 0;
 178         } else {
 179                 pcap_perror(sock->pcap, "pcap_inject");
 180                 return -1;
 181         }
 182 #endif
 183 }
 184
 185 int ethsock_close(struct ethsock *sock)
 186 {
 187         pcap_close(sock->pcap);
 188         free(sock);
 189         return 0;
 190 }
 191
 192 int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 193 {
 194         sock->timeout.tv_sec = msec / 1000;
 195         sock->timeout.tv_usec = (msec % 1000) * 1000;
 196         return 0;
 197 }