src/sg_pcap.c

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * By Daniel Borkmann <daniel@netsniff-ng.org>
   4  * Copyright 2011 Daniel Borkmann.
   5  * Subject to the GPL.
   6  */
   7
   8 #include <stdio.h>
   9 #include <sys/types.h>
  10 #include <sys/stat.h>
  11 #include <fcntl.h>
  12 #include <sys/uio.h>
  13 #include <assert.h>
  14
  15 #include "pcap.h"
  16 #include "tlsf.h"
  17 #include "write_or_die.h"
  18 #include "opt_memcpy.h"
  19 #include "locking.h"
  20
  21 #define IOVSIZ   1000
  22 #define ALLSIZ   9100
  23
  24 static struct iovec iov[IOVSIZ];
  25 static unsigned long c = 0;
  26 static struct spinlock lock;
  27 static ssize_t avail, used, iov_used;
  28
  29 static int sg_pcap_pull_file_header(int fd)
  30 {
  31         ssize_t ret;
  32         struct pcap_filehdr hdr;
  33
  34         ret = read(fd, &hdr, sizeof(hdr));
  35         if (unlikely(ret != sizeof(hdr)))
  36                 return -EIO;
  37         pcap_validate_header_maybe_die(&hdr);
  38
  39         return 0;
  40 }
  41
  42 static int sg_pcap_push_file_header(int fd)
  43 {
  44         ssize_t ret;
  45         struct pcap_filehdr hdr;
  46
  47         memset(&hdr, 0, sizeof(hdr));
  48         pcap_prepare_header(&hdr, LINKTYPE_EN10MB, 0,
  49                             PCAP_DEFAULT_SNAPSHOT_LEN);
  50         ret = write_or_die(fd, &hdr, sizeof(hdr));
  51         if (unlikely(ret != sizeof(hdr))) {
  52                 whine("Failed to write pkt file header!\n");
  53                 return -EIO;
  54         }
  55
  56         return 0;
  57 }
  58
  59 static ssize_t sg_pcap_write_pcap_pkt(int fd, struct pcap_pkthdr *hdr,
  60                                       uint8_t *packet, size_t len)
  61 {
  62         ssize_t ret;
  63         spinlock_lock(&lock);
  64         if (c == IOVSIZ) {
  65                 ret = writev(fd, iov, IOVSIZ);
  66                 if (ret < 0)
  67                         panic("writev I/O error!\n");
  68                 c = 0;
  69         }
  70         iov[c].iov_len = 0;
  71         __memcpy_small(iov[c].iov_base, hdr, sizeof(*hdr));
  72         iov[c].iov_len += sizeof(*hdr);
  73         __memcpy(iov[c].iov_base + iov[c].iov_len, packet, len);
  74         iov[c].iov_len += len;
  75         ret = iov[c].iov_len;
  76         c++;
  77         spinlock_unlock(&lock);
  78         return ret;
  79 }
  80
  81 static int sg_pcap_prepare_reading_pcap(int fd)
  82 {
  83         spinlock_lock(&lock);
  84         avail = readv(fd, iov, IOVSIZ);
  85         if (avail <= 0)
  86                 return -EIO;
  87         used = iov_used = 0;
  88         c = 0;
  89         spinlock_unlock(&lock);
  90         return 0;
  91 }
  92
  93 static ssize_t sg_pcap_read_pcap_pkt(int fd, struct pcap_pkthdr *hdr,
  94                                      uint8_t *packet, size_t len)
  95 {
  96         /* In contrast to writing, reading gets really ugly ... */
  97         spinlock_lock(&lock);
  98         if (likely(avail - used >= sizeof(*hdr) &&
  99                    iov[c].iov_len - iov_used >= sizeof(*hdr))) {
 100                 __memcpy_small(hdr, iov[c].iov_base + iov_used, sizeof(*hdr));
 101                 iov_used += sizeof(*hdr);
 102                 used += sizeof(*hdr);
 103         } else {
 104                 size_t remainder, offset = 0;
 105                 if (avail - used < sizeof(*hdr))
 106                         return -ENOMEM;
 107                 offset = iov[c].iov_len - iov_used;
 108                 remainder = sizeof(*hdr) - offset;
 109                 assert(offset + remainder == sizeof(*hdr));
 110                 __memcpy_small(hdr, iov[c].iov_base + iov_used, offset);
 111                 used += offset;
 112                 iov_used = 0;
 113                 c++;
 114                 if (c == IOVSIZ) {
 115                         /* We need to refetch! */
 116                         c = 0;
 117                         avail = readv(fd, iov, IOVSIZ);
 118                         if (avail < 0)
 119                                 return -EIO;
 120                         used = 0;
 121                 }
 122                 /* Now we copy the remainder and go on with business ... */
 123                 __memcpy_small(hdr, iov[c].iov_base + iov_used, remainder);
 124                 iov_used += remainder;
 125                 used += remainder;
 126         }
 127         if (likely(avail - used >= hdr->len &&
 128                    iov[c].iov_len - iov_used >= hdr->len)) {
 129                 __memcpy(packet, iov[c].iov_base + iov_used, hdr->len);
 130                 iov_used += hdr->len;
 131                 used += hdr->len;
 132         } else {
 133                 size_t remainder, offset = 0;
 134                 if (avail - used < hdr->len)
 135                         return -ENOMEM;
 136                 offset = iov[c].iov_len - iov_used;
 137                 remainder = hdr->len - offset;
 138                 assert(offset + remainder == hdr->len);
 139                 __memcpy(packet, iov[c].iov_base + iov_used, offset);
 140                 used += offset;
 141                 iov_used = 0;
 142                 c++;
 143                 if (c == IOVSIZ) {
 144                         /* We need to refetch! */
 145                         c = 0;
 146                         avail = readv(fd, iov, IOVSIZ);
 147                         if (avail < 0)
 148                                 return -EIO;
 149                         used = 0;
 150                 }
 151                 /* Now we copy the remainder and go on with business ... */
 152                 __memcpy(packet, iov[c].iov_base + iov_used, remainder);
 153                 iov_used += remainder;
 154                 used += remainder;
 155         }
 156         spinlock_unlock(&lock);
 157         if (unlikely(hdr->len == 0))
 158                 return -EINVAL; /* Bogus packet */
 159         return sizeof(*hdr) + hdr->len;
 160 }
 161
 162 static void sg_pcap_fsync_pcap(int fd)
 163 {
 164         ssize_t ret;
 165         spinlock_lock(&lock);
 166         ret = writev(fd, iov, c);
 167         if (ret < 0)
 168                 panic("writev I/O error!\n");
 169         c = 0;
 170         spinlock_unlock(&lock);
 171 }
 172
 173 struct pcap_file_ops sg_pcap_ops __read_mostly = {
 174         .name = "SCATTER/GATHER",
 175         .pull_file_header = sg_pcap_pull_file_header,
 176         .push_file_header = sg_pcap_push_file_header,
 177         .write_pcap_pkt = sg_pcap_write_pcap_pkt,
 178         .read_pcap_pkt = sg_pcap_read_pcap_pkt,
 179         .prepare_reading_pcap =  sg_pcap_prepare_reading_pcap,
 180         .fsync_pcap = sg_pcap_fsync_pcap,
 181 };
 182
 183 int init_sg_pcap(void)
 184 {
 185         unsigned long i;
 186         c = 0;
 187         memset(iov, 0, sizeof(iov));
 188         for (i = 0; i < IOVSIZ; ++i) {
 189                 iov[i].iov_base = xtlsf_malloc(ALLSIZ);
 190                 iov[i].iov_len = ALLSIZ;
 191         }
 192         spinlock_init(&lock);
 193         return pcap_ops_group_register(&sg_pcap_ops, PCAP_OPS_SG);
 194 }
 195
 196 void cleanup_sg_pcap(void)
 197 {
 198         unsigned long i;
 199         spinlock_destroy(&lock);
 200         for (i = 0; i < IOVSIZ; ++i)
 201                 xtlsf_free(iov[i].iov_base);
 202         pcap_ops_group_unregister(PCAP_OPS_SG);
 203 }
 204