ethsock.c

   1 #include <string.h>
   2 #include <stdlib.h>
   3 #include <stdio.h>
   4 #include <pcap.h>
   5 #include "ethsock.h"
   6
   7 #ifndef MIN
   8 #define MIN(a, b) ((a) < (b) ? (a) : (b))
   9 #endif
  10
  11 struct ethsock
  12 {
  13         pcap_t *pcap;
  14         struct timeval timeout;
  15         int fd;
  16 };
  17
  18 struct ethsock *ethsock_create(const char *interface, uint16_t protocol)
  19 {
  20         char buf[PCAP_ERRBUF_SIZE];
  21         struct bpf_program fp;
  22         struct ethsock *sock;
  23         int err;
  24
  25         sock = malloc(sizeof(struct ethsock));
  26         if (!sock) {
  27                 perror("malloc");
  28                 return NULL;
  29         }
  30
  31         buf[0] = '\0';
  32
  33         sock->pcap = pcap_open_live(interface, BUFSIZ, 1, 1, buf);
  34         if (!sock->pcap) {
  35                 fprintf(stderr, "pcap_open_live: %s\n", buf);
  36                 goto cleanup_malloc;
  37         }
  38
  39         if (*buf) {
  40                 fprintf(stderr, "Warning: %s.\n", buf);
  41         }
  42
  43         if (pcap_datalink(sock->pcap) != DLT_EN10MB) {
  44                 fprintf(stderr, "Interface %s not supported.\n", interface);
  45                 goto cleanup_pcap;
  46         }
  47
  48         sock->fd = pcap_get_selectable_fd(sock->pcap);
  49         if (sock->fd == -1) {
  50                 fprintf(stderr, "No selectable file descriptor available.\n");
  51                 goto cleanup_pcap;
  52         }
  53
  54         snprintf(buf, sizeof(buf), "ether proto %04x", protocol);
  55         err = pcap_compile(sock->pcap, &fp, buf, 0, PCAP_NETMASK_UNKNOWN);
  56         if (err) {
  57                 pcap_perror(sock->pcap, "pcap_compile");
  58                 goto cleanup_pcap;
  59         }
  60
  61         if ((err = pcap_setfilter(sock->pcap, &fp))) {
  62                 pcap_perror(sock->pcap, "pcap_setfilter");
  63                 goto cleanup_pcap;
  64         }
  65
  66         return sock;
  67
  68 cleanup_pcap:
  69         pcap_close(sock->pcap);
  70 cleanup_malloc:
  71         free(sock);
  72         return NULL;
  73 }
  74
  75 ssize_t ethsock_recv(struct ethsock *sock, void *buf, size_t len)
  76 {
  77         struct pcap_pkthdr* hdr;
  78         const u_char *capbuf;
  79         int status;
  80         fd_set fds;
  81
  82         if (sock->timeout.tv_sec || sock->timeout.tv_usec) {
  83                 FD_ZERO(&fds);
  84                 FD_SET(sock->fd, &fds);
  85
  86                 status = select(sock->fd + 1, &fds, NULL, NULL, &sock->timeout);
  87                 if (status == -1) {
  88                         perror("select");
  89                         return -1;
  90                 } else if (status == 0) {
  91                         return 1;
  92                 }
  93         }
  94
  95         status = pcap_next_ex(sock->pcap, &hdr, &capbuf);
  96         switch (status) {
  97                 case 1:
  98                         memcpy(buf, capbuf, MIN(len, hdr->caplen));
  99                         return hdr->caplen;
 100                 case 0:
 101                         return 0;
 102                 case -1:
 103                         pcap_perror(sock->pcap, "pcap_next_ex");
 104                         return -1;
 105                 default:
 106                         fprintf(stderr, "pcap_next_ex: returned %d.\n", status);
 107                         return -1;
 108         }
 109 }
 110
 111 int ethsock_send(struct ethsock *sock, void *buf, size_t len)
 112 {
 113 #if defined(_WIN32) || defined(_WIN64)
 114         if (pcap_sendpacket(sock->pcap, buf, len) == 0) {
 115                 return 0;
 116         } else {
 117                 pcap_perror(sock->pcap, "pcap_sendpacket");
 118                 return -1;
 119         }
 120 #else
 121         if (pcap_inject(sock->pcap, buf, len) == len) {
 122                 return 0;
 123         } else {
 124                 pcap_perror(sock->pcap, "pcap_inject");
 125                 return -1;
 126         }
 127 #endif
 128 }
 129
 130 int ethsock_close(struct ethsock *sock)
 131 {
 132         pcap_close(sock->pcap);
 133         free(sock);
 134         return 0;
 135 }
 136
 137 int ethsock_set_timeout(struct ethsock *sock, unsigned msec)
 138 {
 139         sock->timeout.tv_sec = msec / 1000;
 140         sock->timeout.tv_usec = (msec % 1000) * 1000;
 141         return 0;
 142 }