rawsock.c

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