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