src/pcap_sg.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, version 2.
   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 <unistd.h>
  14
  15 #include "pcap.h"
  16 #include "xmalloc.h"
  17 #include "xio.h"
  18 #include "xutils.h"
  19 #include "locking.h"
  20 #include "built_in.h"
  21
  22 #define IOVSIZ          1000
  23 #define ALLSIZ          (PAGE_SIZE * 3)
  24 #define ALLSIZ_2K       (PAGE_SIZE * 3)  // 12K max
  25 #define ALLSIZ_JUMBO    (PAGE_SIZE * 16) // 64K max
  26
  27 static struct iovec iov[IOVSIZ];
  28 static unsigned long c = 0;
  29 static struct spinlock lock;
  30 static ssize_t iov_used;
  31
  32 static int pcap_sg_pull_file_header(int fd, uint32_t *linktype)
  33 {
  34         ssize_t ret;
  35         struct pcap_filehdr hdr;
  36
  37         ret = read(fd, &hdr, sizeof(hdr));
  38         if (unlikely(ret != sizeof(hdr)))
  39                 return -EIO;
  40
  41         pcap_validate_header(&hdr);
  42
  43         *linktype = hdr.linktype;
  44
  45         return 0;
  46 }
  47
  48 static int pcap_sg_push_file_header(int fd, uint32_t linktype)
  49 {
  50         ssize_t ret;
  51         struct pcap_filehdr hdr;
  52
  53         fmemset(&hdr, 0, sizeof(hdr));
  54         pcap_prepare_header(&hdr, linktype, 0, PCAP_DEFAULT_SNAPSHOT_LEN);
  55
  56         ret = write_or_die(fd, &hdr, sizeof(hdr));
  57         if (unlikely(ret != sizeof(hdr))) {
  58                 whine("Failed to write pkt file header!\n");
  59                 return -EIO;
  60         }
  61
  62         return 0;
  63 }
  64
  65 static int pcap_sg_prepare_writing_pcap(int fd)
  66 {
  67         set_ioprio_rt();
  68         return 0;
  69 }
  70
  71 static ssize_t pcap_sg_write_pcap_pkt(int fd, struct pcap_pkthdr *hdr,
  72                                       uint8_t *packet, size_t len)
  73 {
  74         ssize_t ret;
  75
  76         spinlock_lock(&lock);
  77
  78         if (unlikely(c == IOVSIZ)) {
  79                 ret = writev(fd, iov, IOVSIZ);
  80                 if (ret < 0)
  81                         panic("writev I/O error!\n");
  82
  83                 c = 0;
  84         }
  85
  86         iov[c].iov_len = 0;
  87         fmemcpy(iov[c].iov_base, hdr, sizeof(*hdr));
  88
  89         iov[c].iov_len += sizeof(*hdr);
  90         fmemcpy(iov[c].iov_base + iov[c].iov_len, packet, len);
  91
  92         iov[c].iov_len += len;
  93         ret = iov[c].iov_len;
  94
  95         c++;
  96
  97         spinlock_unlock(&lock);
  98
  99         return ret;
 100 }
 101
 102 static int pcap_sg_prepare_reading_pcap(int fd)
 103 {
 104         set_ioprio_rt();
 105
 106         spinlock_lock(&lock);
 107         if (readv(fd, iov, IOVSIZ) <= 0)
 108                 return -EIO;
 109
 110         iov_used = 0;
 111         c = 0;
 112         spinlock_unlock(&lock);
 113
 114         return 0;
 115 }
 116
 117 static ssize_t pcap_sg_read_pcap_pkt(int fd, struct pcap_pkthdr *hdr,
 118                                      uint8_t *packet, size_t len)
 119 {
 120         ssize_t ret = 0;
 121
 122         /* In contrast to writing, reading gets really ugly ... */
 123         spinlock_lock(&lock);
 124
 125         if (likely(iov[c].iov_len - iov_used >= sizeof(*hdr))) {
 126                 fmemcpy(hdr, iov[c].iov_base + iov_used, sizeof(*hdr));
 127                 iov_used += sizeof(*hdr);
 128         } else {
 129                 size_t offset = 0;
 130                 ssize_t remainder;
 131
 132                 offset = iov[c].iov_len - iov_used;
 133                 remainder = sizeof(*hdr) - offset;
 134                 if (remainder < 0)
 135                         remainder = 0;
 136
 137                 bug_on(offset + remainder != sizeof(*hdr));
 138
 139                 fmemcpy(hdr, iov[c].iov_base + iov_used, offset);
 140
 141                 iov_used = 0;
 142                 c++;
 143
 144                 if (c == IOVSIZ) {
 145                         /* We need to refetch! */
 146                         c = 0;
 147                         if (readv(fd, iov, IOVSIZ) <= 0) {
 148                                 ret = -EIO;
 149                                 goto out_err;
 150                         }
 151                 }
 152
 153                 /* Now we copy the remainder and go on with business ... */
 154                 fmemcpy((uint8_t *) hdr + offset,
 155                         iov[c].iov_base + iov_used, remainder);
 156                 iov_used += remainder;
 157         }
 158
 159         /* header read completed */
 160
 161         if (unlikely(hdr->caplen == 0 || hdr->caplen > len)) {
 162                 ret = -EINVAL; /* Bogus packet */
 163                 goto out_err;
 164         }
 165
 166         /* now we read data ... */
 167
 168         if (likely(iov[c].iov_len - iov_used >= hdr->caplen)) {
 169                 fmemcpy(packet, iov[c].iov_base + iov_used, hdr->caplen);
 170                 iov_used += hdr->caplen;
 171         } else {
 172                 size_t offset = 0;
 173                 ssize_t remainder;
 174
 175                 offset = iov[c].iov_len - iov_used;
 176                 remainder = hdr->caplen - offset;
 177                 if (remainder < 0)
 178                         remainder = 0;
 179
 180                 bug_on(offset + remainder != hdr->caplen);
 181
 182                 fmemcpy(packet, iov[c].iov_base + iov_used, offset);
 183
 184                 iov_used = 0;
 185                 c++;
 186
 187                 if (c == IOVSIZ) {
 188                         /* We need to refetch! */
 189                         c = 0;
 190                         if (readv(fd, iov, IOVSIZ) <= 0) {
 191                                 ret = -EIO;
 192                                 goto out_err;
 193                         }
 194                 }
 195
 196                 /* Now we copy the remainder and go on with business ... */
 197                 fmemcpy(packet + offset, iov[c].iov_base + iov_used, remainder);
 198                 iov_used += remainder;
 199         }
 200
 201         spinlock_unlock(&lock);
 202
 203         return sizeof(*hdr) + hdr->caplen;
 204
 205 out_err:
 206         spinlock_unlock(&lock);
 207         return ret;
 208 }
 209
 210 static void pcap_sg_fsync_pcap(int fd)
 211 {
 212         ssize_t ret;
 213
 214         spinlock_lock(&lock);
 215         ret = writev(fd, iov, c);
 216         if (ret < 0)
 217                 panic("writev I/O error!\n");
 218
 219         c = 0;
 220
 221         fdatasync(fd);
 222         spinlock_unlock(&lock);
 223 }
 224
 225 struct pcap_file_ops pcap_sg_ops __read_mostly = {
 226         .name = "scatter-gather",
 227         .pull_file_header = pcap_sg_pull_file_header,
 228         .push_file_header = pcap_sg_push_file_header,
 229         .write_pcap_pkt = pcap_sg_write_pcap_pkt,
 230         .prepare_reading_pcap =  pcap_sg_prepare_reading_pcap,
 231         .prepare_writing_pcap =  pcap_sg_prepare_writing_pcap,
 232         .read_pcap_pkt = pcap_sg_read_pcap_pkt,
 233         .fsync_pcap = pcap_sg_fsync_pcap,
 234 };
 235
 236 int init_pcap_sg(int jumbo_support)
 237 {
 238         unsigned long i;
 239         size_t allocsz = 0;
 240
 241         c = 0;
 242
 243         fmemset(iov, 0, sizeof(iov));
 244
 245         if (jumbo_support)
 246                 allocsz = ALLSIZ_JUMBO;
 247         else
 248                 allocsz = ALLSIZ_2K;
 249
 250         for (i = 0; i < IOVSIZ; ++i) {
 251                 iov[i].iov_base = xzmalloc_aligned(allocsz, 64);
 252                 iov[i].iov_len = allocsz;
 253         }
 254
 255         spinlock_init(&lock);
 256
 257         return pcap_ops_group_register(&pcap_sg_ops, PCAP_OPS_SG);
 258 }
 259
 260 void cleanup_pcap_sg(void)
 261 {
 262         unsigned long i;
 263
 264         spinlock_destroy(&lock);
 265
 266         for (i = 0; i < IOVSIZ; ++i)
 267                 xfree(iov[i].iov_base);
 268
 269         pcap_ops_group_unregister(PCAP_OPS_SG);
 270 }