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