drivers/usb/mon/mon_bin.c

   1 /*
   2  * The USB Monitor, inspired by Dave Harding's USBMon.
   3  *
   4  * This is a binary format reader.
   5  *
   6  * Copyright (C) 2006 Paolo Abeni (paolo.abeni@email.it)
   7  * Copyright (C) 2006,2007 Pete Zaitcev (zaitcev@redhat.com)
   8  */
   9
  10 #include <linux/kernel.h>
  11 #include <linux/types.h>
  12 #include <linux/fs.h>
  13 #include <linux/cdev.h>
  14 #include <linux/usb.h>
  15 #include <linux/poll.h>
  16 #include <linux/compat.h>
  17 #include <linux/mm.h>
  18
  19 #include <asm/uaccess.h>
  20
  21 #include "usb_mon.h"
  22
  23 /*
  24  * Defined by USB 2.0 clause 9.3, table 9.2.
  25  */
  26 #define SETUP_LEN  8
  27
  28 /* ioctl macros */
  29 #define MON_IOC_MAGIC 0x92
  30
  31 #define MON_IOCQ_URB_LEN _IO(MON_IOC_MAGIC, 1)
  32 /* #2 used to be MON_IOCX_URB, removed before it got into Linus tree */
  33 #define MON_IOCG_STATS _IOR(MON_IOC_MAGIC, 3, struct mon_bin_stats)
  34 #define MON_IOCT_RING_SIZE _IO(MON_IOC_MAGIC, 4)
  35 #define MON_IOCQ_RING_SIZE _IO(MON_IOC_MAGIC, 5)
  36 #define MON_IOCX_GET   _IOW(MON_IOC_MAGIC, 6, struct mon_bin_get)
  37 #define MON_IOCX_MFETCH _IOWR(MON_IOC_MAGIC, 7, struct mon_bin_mfetch)
  38 #define MON_IOCH_MFLUSH _IO(MON_IOC_MAGIC, 8)
  39 #ifdef CONFIG_COMPAT
  40 #define MON_IOCX_GET32 _IOW(MON_IOC_MAGIC, 6, struct mon_bin_get32)
  41 #define MON_IOCX_MFETCH32 _IOWR(MON_IOC_MAGIC, 7, struct mon_bin_mfetch32)
  42 #endif
  43
  44 /*
  45  * Some architectures have enormous basic pages (16KB for ia64, 64KB for ppc).
  46  * But it's all right. Just use a simple way to make sure the chunk is never
  47  * smaller than a page.
  48  *
  49  * N.B. An application does not know our chunk size.
  50  *
  51  * Woops, get_zeroed_page() returns a single page. I guess we're stuck with
  52  * page-sized chunks for the time being.
  53  */
  54 #define CHUNK_SIZE   PAGE_SIZE
  55 #define CHUNK_ALIGN(x)   (((x)+CHUNK_SIZE-1) & ~(CHUNK_SIZE-1))
  56
  57 /*
  58  * The magic limit was calculated so that it allows the monitoring
  59  * application to pick data once in two ticks. This way, another application,
  60  * which presumably drives the bus, gets to hog CPU, yet we collect our data.
  61  * If HZ is 100, a 480 mbit/s bus drives 614 KB every jiffy. USB has an
  62  * enormous overhead built into the bus protocol, so we need about 1000 KB.
  63  *
  64  * This is still too much for most cases, where we just snoop a few
  65  * descriptor fetches for enumeration. So, the default is a "reasonable"
  66  * amount for systems with HZ=250 and incomplete bus saturation.
  67  *
  68  * XXX What about multi-megabyte URBs which take minutes to transfer?
  69  */
  70 #define BUFF_MAX  CHUNK_ALIGN(1200*1024)
  71 #define BUFF_DFL   CHUNK_ALIGN(300*1024)
  72 #define BUFF_MIN     CHUNK_ALIGN(8*1024)
  73
  74 /*
  75  * The per-event API header (2 per URB).
  76  *
  77  * This structure is seen in userland as defined by the documentation.
  78  */
  79 struct mon_bin_hdr {
  80         u64 id;                 /* URB ID - from submission to callback */
  81         unsigned char type;     /* Same as in text API; extensible. */
  82         unsigned char xfer_type;        /* ISO, Intr, Control, Bulk */
  83         unsigned char epnum;    /* Endpoint number and transfer direction */
  84         unsigned char devnum;   /* Device address */
  85         unsigned short busnum;  /* Bus number */
  86         char flag_setup;
  87         char flag_data;
  88         s64 ts_sec;             /* gettimeofday */
  89         s32 ts_usec;            /* gettimeofday */
  90         int status;
  91         unsigned int len_urb;   /* Length of data (submitted or actual) */
  92         unsigned int len_cap;   /* Delivered length */
  93         unsigned char setup[SETUP_LEN]; /* Only for Control S-type */
  94 };
  95
  96 /* per file statistic */
  97 struct mon_bin_stats {
  98         u32 queued;
  99         u32 dropped;
 100 };
 101
 102 struct mon_bin_get {
 103         struct mon_bin_hdr __user *hdr; /* Only 48 bytes, not 64. */
 104         void __user *data;
 105         size_t alloc;           /* Length of data (can be zero) */
 106 };
 107
 108 struct mon_bin_mfetch {
 109         u32 __user *offvec;     /* Vector of events fetched */
 110         u32 nfetch;             /* Number of events to fetch (out: fetched) */
 111         u32 nflush;             /* Number of events to flush */
 112 };
 113
 114 #ifdef CONFIG_COMPAT
 115 struct mon_bin_get32 {
 116         u32 hdr32;
 117         u32 data32;
 118         u32 alloc32;
 119 };
 120
 121 struct mon_bin_mfetch32 {
 122         u32 offvec32;
 123         u32 nfetch32;
 124         u32 nflush32;
 125 };
 126 #endif
 127
 128 /* Having these two values same prevents wrapping of the mon_bin_hdr */
 129 #define PKT_ALIGN   64
 130 #define PKT_SIZE    64
 131
 132 /* max number of USB bus supported */
 133 #define MON_BIN_MAX_MINOR 128
 134
 135 /*
 136  * The buffer: map of used pages.
 137  */
 138 struct mon_pgmap {
 139         struct page *pg;
 140         unsigned char *ptr;     /* XXX just use page_to_virt everywhere? */
 141 };
 142
 143 /*
 144  * This gets associated with an open file struct.
 145  */
 146 struct mon_reader_bin {
 147         /* The buffer: one per open. */
 148         spinlock_t b_lock;              /* Protect b_cnt, b_in */
 149         unsigned int b_size;            /* Current size of the buffer - bytes */
 150         unsigned int b_cnt;             /* Bytes used */
 151         unsigned int b_in, b_out;       /* Offsets into buffer - bytes */
 152         unsigned int b_read;            /* Amount of read data in curr. pkt. */
 153         struct mon_pgmap *b_vec;        /* The map array */
 154         wait_queue_head_t b_wait;       /* Wait for data here */
 155
 156         struct mutex fetch_lock;        /* Protect b_read, b_out */
 157         int mmap_active;
 158
 159         /* A list of these is needed for "bus 0". Some time later. */
 160         struct mon_reader r;
 161
 162         /* Stats */
 163         unsigned int cnt_lost;
 164 };
 165
 166 static inline struct mon_bin_hdr *MON_OFF2HDR(const struct mon_reader_bin *rp,
 167     unsigned int offset)
 168 {
 169         return (struct mon_bin_hdr *)
 170             (rp->b_vec[offset / CHUNK_SIZE].ptr + offset % CHUNK_SIZE);
 171 }
 172
 173 #define MON_RING_EMPTY(rp)      ((rp)->b_cnt == 0)
 174
 175 static struct class *mon_bin_class;
 176 static dev_t mon_bin_dev0;
 177 static struct cdev mon_bin_cdev;
 178
 179 static void mon_buff_area_fill(const struct mon_reader_bin *rp,
 180     unsigned int offset, unsigned int size);
 181 static int mon_bin_wait_event(struct file *file, struct mon_reader_bin *rp);
 182 static int mon_alloc_buff(struct mon_pgmap *map, int npages);
 183 static void mon_free_buff(struct mon_pgmap *map, int npages);
 184
 185 /*
 186  * This is a "chunked memcpy". It does not manipulate any counters.
 187  * But it returns the new offset for repeated application.
 188  */
 189 unsigned int mon_copy_to_buff(const struct mon_reader_bin *this,
 190     unsigned int off, const unsigned char *from, unsigned int length)
 191 {
 192         unsigned int step_len;
 193         unsigned char *buf;
 194         unsigned int in_page;
 195
 196         while (length) {
 197                 /*
 198                  * Determine step_len.
 199                  */
 200                 step_len = length;
 201                 in_page = CHUNK_SIZE - (off & (CHUNK_SIZE-1));
 202                 if (in_page < step_len)
 203                         step_len = in_page;
 204
 205                 /*
 206                  * Copy data and advance pointers.
 207                  */
 208                 buf = this->b_vec[off / CHUNK_SIZE].ptr + off % CHUNK_SIZE;
 209                 memcpy(buf, from, step_len);
 210                 if ((off += step_len) >= this->b_size) off = 0;
 211                 from += step_len;
 212                 length -= step_len;
 213         }
 214         return off;
 215 }
 216
 217 /*
 218  * This is a little worse than the above because it's "chunked copy_to_user".
 219  * The return value is an error code, not an offset.
 220  */
 221 static int copy_from_buf(const struct mon_reader_bin *this, unsigned int off,
 222     char __user *to, int length)
 223 {
 224         unsigned int step_len;
 225         unsigned char *buf;
 226         unsigned int in_page;
 227
 228         while (length) {
 229                 /*
 230                  * Determine step_len.
 231                  */
 232                 step_len = length;
 233                 in_page = CHUNK_SIZE - (off & (CHUNK_SIZE-1));
 234                 if (in_page < step_len)
 235                         step_len = in_page;
 236
 237                 /*
 238                  * Copy data and advance pointers.
 239                  */
 240                 buf = this->b_vec[off / CHUNK_SIZE].ptr + off % CHUNK_SIZE;
 241                 if (copy_to_user(to, buf, step_len))
 242                         return -EINVAL;
 243                 if ((off += step_len) >= this->b_size) off = 0;
 244                 to += step_len;
 245                 length -= step_len;
 246         }
 247         return 0;
 248 }
 249
 250 /*
 251  * Allocate an (aligned) area in the buffer.
 252  * This is called under b_lock.
 253  * Returns ~0 on failure.
 254  */
 255 static unsigned int mon_buff_area_alloc(struct mon_reader_bin *rp,
 256     unsigned int size)
 257 {
 258         unsigned int offset;
 259
 260         size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 261         if (rp->b_cnt + size > rp->b_size)
 262                 return ~0;
 263         offset = rp->b_in;
 264         rp->b_cnt += size;
 265         if ((rp->b_in += size) >= rp->b_size)
 266                 rp->b_in -= rp->b_size;
 267         return offset;
 268 }
 269
 270 /*
 271  * This is the same thing as mon_buff_area_alloc, only it does not allow
 272  * buffers to wrap. This is needed by applications which pass references
 273  * into mmap-ed buffers up their stacks (libpcap can do that).
 274  *
 275  * Currently, we always have the header stuck with the data, although
 276  * it is not strictly speaking necessary.
 277  *
 278  * When a buffer would wrap, we place a filler packet to mark the space.
 279  */
 280 static unsigned int mon_buff_area_alloc_contiguous(struct mon_reader_bin *rp,
 281     unsigned int size)
 282 {
 283         unsigned int offset;
 284         unsigned int fill_size;
 285
 286         size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 287         if (rp->b_cnt + size > rp->b_size)
 288                 return ~0;
 289         if (rp->b_in + size > rp->b_size) {
 290                 /*
 291                  * This would wrap. Find if we still have space after
 292                  * skipping to the end of the buffer. If we do, place
 293                  * a filler packet and allocate a new packet.
 294                  */
 295                 fill_size = rp->b_size - rp->b_in;
 296                 if (rp->b_cnt + size + fill_size > rp->b_size)
 297                         return ~0;
 298                 mon_buff_area_fill(rp, rp->b_in, fill_size);
 299
 300                 offset = 0;
 301                 rp->b_in = size;
 302                 rp->b_cnt += size + fill_size;
 303         } else if (rp->b_in + size == rp->b_size) {
 304                 offset = rp->b_in;
 305                 rp->b_in = 0;
 306                 rp->b_cnt += size;
 307         } else {
 308                 offset = rp->b_in;
 309                 rp->b_in += size;
 310                 rp->b_cnt += size;
 311         }
 312         return offset;
 313 }
 314
 315 /*
 316  * Return a few (kilo-)bytes to the head of the buffer.
 317  * This is used if a DMA fetch fails.
 318  */
 319 static void mon_buff_area_shrink(struct mon_reader_bin *rp, unsigned int size)
 320 {
 321
 322         size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 323         rp->b_cnt -= size;
 324         if (rp->b_in < size)
 325                 rp->b_in += rp->b_size;
 326         rp->b_in -= size;
 327 }
 328
 329 /*
 330  * This has to be called under both b_lock and fetch_lock, because
 331  * it accesses both b_cnt and b_out.
 332  */
 333 static void mon_buff_area_free(struct mon_reader_bin *rp, unsigned int size)
 334 {
 335
 336         size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 337         rp->b_cnt -= size;
 338         if ((rp->b_out += size) >= rp->b_size)
 339                 rp->b_out -= rp->b_size;
 340 }
 341
 342 static void mon_buff_area_fill(const struct mon_reader_bin *rp,
 343     unsigned int offset, unsigned int size)
 344 {
 345         struct mon_bin_hdr *ep;
 346
 347         ep = MON_OFF2HDR(rp, offset);
 348         memset(ep, 0, PKT_SIZE);
 349         ep->type = '@';
 350         ep->len_cap = size - PKT_SIZE;
 351 }
 352
 353 static inline char mon_bin_get_setup(unsigned char *setupb,
 354     const struct urb *urb, char ev_type)
 355 {
 356
 357         if (!usb_pipecontrol(urb->pipe) || ev_type != 'S')
 358                 return '-';
 359
 360         if (urb->dev->bus->uses_dma &&
 361             (urb->transfer_flags & URB_NO_SETUP_DMA_MAP)) {
 362                 return mon_dmapeek(setupb, urb->setup_dma, SETUP_LEN);
 363         }
 364         if (urb->setup_packet == NULL)
 365                 return 'Z';
 366
 367         memcpy(setupb, urb->setup_packet, SETUP_LEN);
 368         return 0;
 369 }
 370
 371 static char mon_bin_get_data(const struct mon_reader_bin *rp,
 372     unsigned int offset, struct urb *urb, unsigned int length)
 373 {
 374
 375         if (urb->dev->bus->uses_dma &&
 376             (urb->transfer_flags & URB_NO_TRANSFER_DMA_MAP)) {
 377                 mon_dmapeek_vec(rp, offset, urb->transfer_dma, length);
 378                 return 0;
 379         }
 380
 381         if (urb->transfer_buffer == NULL)
 382                 return 'Z';
 383
 384         mon_copy_to_buff(rp, offset, urb->transfer_buffer, length);
 385         return 0;
 386 }
 387
 388 static void mon_bin_event(struct mon_reader_bin *rp, struct urb *urb,
 389     char ev_type)
 390 {
 391         unsigned long flags;
 392         struct timeval ts;
 393         unsigned int urb_length;
 394         unsigned int offset;
 395         unsigned int length;
 396         struct mon_bin_hdr *ep;
 397         char data_tag = 0;
 398
 399         do_gettimeofday(&ts);
 400
 401         spin_lock_irqsave(&rp->b_lock, flags);
 402
 403         /*
 404          * Find the maximum allowable length, then allocate space.
 405          */
 406         urb_length = (ev_type == 'S') ?
 407             urb->transfer_buffer_length : urb->actual_length;
 408         length = urb_length;
 409
 410         if (length >= rp->b_size/5)
 411                 length = rp->b_size/5;
 412
 413         if (usb_pipein(urb->pipe)) {
 414                 if (ev_type == 'S') {
 415                         length = 0;
 416                         data_tag = '<';
 417                 }
 418         } else {
 419                 if (ev_type == 'C') {
 420                         length = 0;
 421                         data_tag = '>';
 422                 }
 423         }
 424
 425         if (rp->mmap_active)
 426                 offset = mon_buff_area_alloc_contiguous(rp, length + PKT_SIZE);
 427         else
 428                 offset = mon_buff_area_alloc(rp, length + PKT_SIZE);
 429         if (offset == ~0) {
 430                 rp->cnt_lost++;
 431                 spin_unlock_irqrestore(&rp->b_lock, flags);
 432                 return;
 433         }
 434
 435         ep = MON_OFF2HDR(rp, offset);
 436         if ((offset += PKT_SIZE) >= rp->b_size) offset = 0;
 437
 438         /*
 439          * Fill the allocated area.
 440          */
 441         memset(ep, 0, PKT_SIZE);
 442         ep->type = ev_type;
 443         ep->xfer_type = usb_pipetype(urb->pipe);
 444         /* We use the fact that usb_pipein() returns 0x80 */
 445         ep->epnum = usb_pipeendpoint(urb->pipe) | usb_pipein(urb->pipe);
 446         ep->devnum = usb_pipedevice(urb->pipe);
 447         ep->busnum = urb->dev->bus->busnum;
 448         ep->id = (unsigned long) urb;
 449         ep->ts_sec = ts.tv_sec;
 450         ep->ts_usec = ts.tv_usec;
 451         ep->status = urb->status;
 452         ep->len_urb = urb_length;
 453         ep->len_cap = length;
 454
 455         ep->flag_setup = mon_bin_get_setup(ep->setup, urb, ev_type);
 456         if (length != 0) {
 457                 ep->flag_data = mon_bin_get_data(rp, offset, urb, length);
 458                 if (ep->flag_data != 0) {       /* Yes, it's 0x00, not '0' */
 459                         ep->len_cap = 0;
 460                         mon_buff_area_shrink(rp, length);
 461                 }
 462         } else {
 463                 ep->flag_data = data_tag;
 464         }
 465
 466         spin_unlock_irqrestore(&rp->b_lock, flags);
 467
 468         wake_up(&rp->b_wait);
 469 }
 470
 471 static void mon_bin_submit(void *data, struct urb *urb)
 472 {
 473         struct mon_reader_bin *rp = data;
 474         mon_bin_event(rp, urb, 'S');
 475 }
 476
 477 static void mon_bin_complete(void *data, struct urb *urb)
 478 {
 479         struct mon_reader_bin *rp = data;
 480         mon_bin_event(rp, urb, 'C');
 481 }
 482
 483 static void mon_bin_error(void *data, struct urb *urb, int error)
 484 {
 485         struct mon_reader_bin *rp = data;
 486         unsigned long flags;
 487         unsigned int offset;
 488         struct mon_bin_hdr *ep;
 489
 490         spin_lock_irqsave(&rp->b_lock, flags);
 491
 492         offset = mon_buff_area_alloc(rp, PKT_SIZE);
 493         if (offset == ~0) {
 494                 /* Not incrementing cnt_lost. Just because. */
 495                 spin_unlock_irqrestore(&rp->b_lock, flags);
 496                 return;
 497         }
 498
 499         ep = MON_OFF2HDR(rp, offset);
 500
 501         memset(ep, 0, PKT_SIZE);
 502         ep->type = 'E';
 503         ep->xfer_type = usb_pipetype(urb->pipe);
 504         /* We use the fact that usb_pipein() returns 0x80 */
 505         ep->epnum = usb_pipeendpoint(urb->pipe) | usb_pipein(urb->pipe);
 506         ep->devnum = usb_pipedevice(urb->pipe);
 507         ep->busnum = urb->dev->bus->busnum;
 508         ep->id = (unsigned long) urb;
 509         ep->status = error;
 510
 511         ep->flag_setup = '-';
 512         ep->flag_data = 'E';
 513
 514         spin_unlock_irqrestore(&rp->b_lock, flags);
 515
 516         wake_up(&rp->b_wait);
 517 }
 518
 519 static int mon_bin_open(struct inode *inode, struct file *file)
 520 {
 521         struct mon_bus *mbus;
 522         struct mon_reader_bin *rp;
 523         size_t size;
 524         int rc;
 525
 526         mutex_lock(&mon_lock);
 527         if ((mbus = mon_bus_lookup(iminor(inode))) == NULL) {
 528                 mutex_unlock(&mon_lock);
 529                 return -ENODEV;
 530         }
 531         if (mbus != &mon_bus0 && mbus->u_bus == NULL) {
 532                 printk(KERN_ERR TAG ": consistency error on open\n");
 533                 mutex_unlock(&mon_lock);
 534                 return -ENODEV;
 535         }
 536
 537         rp = kzalloc(sizeof(struct mon_reader_bin), GFP_KERNEL);
 538         if (rp == NULL) {
 539                 rc = -ENOMEM;
 540                 goto err_alloc;
 541         }
 542         spin_lock_init(&rp->b_lock);
 543         init_waitqueue_head(&rp->b_wait);
 544         mutex_init(&rp->fetch_lock);
 545
 546         rp->b_size = BUFF_DFL;
 547
 548         size = sizeof(struct mon_pgmap) * (rp->b_size/CHUNK_SIZE);
 549         if ((rp->b_vec = kzalloc(size, GFP_KERNEL)) == NULL) {
 550                 rc = -ENOMEM;
 551                 goto err_allocvec;
 552         }
 553
 554         if ((rc = mon_alloc_buff(rp->b_vec, rp->b_size/CHUNK_SIZE)) < 0)
 555                 goto err_allocbuff;
 556
 557         rp->r.m_bus = mbus;
 558         rp->r.r_data = rp;
 559         rp->r.rnf_submit = mon_bin_submit;
 560         rp->r.rnf_error = mon_bin_error;
 561         rp->r.rnf_complete = mon_bin_complete;
 562
 563         mon_reader_add(mbus, &rp->r);
 564
 565         file->private_data = rp;
 566         mutex_unlock(&mon_lock);
 567         return 0;
 568
 569 err_allocbuff:
 570         kfree(rp->b_vec);
 571 err_allocvec:
 572         kfree(rp);
 573 err_alloc:
 574         mutex_unlock(&mon_lock);
 575         return rc;
 576 }
 577
 578 /*
 579  * Extract an event from buffer and copy it to user space.
 580  * Wait if there is no event ready.
 581  * Returns zero or error.
 582  */
 583 static int mon_bin_get_event(struct file *file, struct mon_reader_bin *rp,
 584     struct mon_bin_hdr __user *hdr, void __user *data, unsigned int nbytes)
 585 {
 586         unsigned long flags;
 587         struct mon_bin_hdr *ep;
 588         size_t step_len;
 589         unsigned int offset;
 590         int rc;
 591
 592         mutex_lock(&rp->fetch_lock);
 593
 594         if ((rc = mon_bin_wait_event(file, rp)) < 0) {
 595                 mutex_unlock(&rp->fetch_lock);
 596                 return rc;
 597         }
 598
 599         ep = MON_OFF2HDR(rp, rp->b_out);
 600
 601         if (copy_to_user(hdr, ep, sizeof(struct mon_bin_hdr))) {
 602                 mutex_unlock(&rp->fetch_lock);
 603                 return -EFAULT;
 604         }
 605
 606         step_len = min(ep->len_cap, nbytes);
 607         if ((offset = rp->b_out + PKT_SIZE) >= rp->b_size) offset = 0;
 608
 609         if (copy_from_buf(rp, offset, data, step_len)) {
 610                 mutex_unlock(&rp->fetch_lock);
 611                 return -EFAULT;
 612         }
 613
 614         spin_lock_irqsave(&rp->b_lock, flags);
 615         mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
 616         spin_unlock_irqrestore(&rp->b_lock, flags);
 617         rp->b_read = 0;
 618
 619         mutex_unlock(&rp->fetch_lock);
 620         return 0;
 621 }
 622
 623 static int mon_bin_release(struct inode *inode, struct file *file)
 624 {
 625         struct mon_reader_bin *rp = file->private_data;
 626         struct mon_bus* mbus = rp->r.m_bus;
 627
 628         mutex_lock(&mon_lock);
 629
 630         if (mbus->nreaders <= 0) {
 631                 printk(KERN_ERR TAG ": consistency error on close\n");
 632                 mutex_unlock(&mon_lock);
 633                 return 0;
 634         }
 635         mon_reader_del(mbus, &rp->r);
 636
 637         mon_free_buff(rp->b_vec, rp->b_size/CHUNK_SIZE);
 638         kfree(rp->b_vec);
 639         kfree(rp);
 640
 641         mutex_unlock(&mon_lock);
 642         return 0;
 643 }
 644
 645 static ssize_t mon_bin_read(struct file *file, char __user *buf,
 646     size_t nbytes, loff_t *ppos)
 647 {
 648         struct mon_reader_bin *rp = file->private_data;
 649         unsigned long flags;
 650         struct mon_bin_hdr *ep;
 651         unsigned int offset;
 652         size_t step_len;
 653         char *ptr;
 654         ssize_t done = 0;
 655         int rc;
 656
 657         mutex_lock(&rp->fetch_lock);
 658
 659         if ((rc = mon_bin_wait_event(file, rp)) < 0) {
 660                 mutex_unlock(&rp->fetch_lock);
 661                 return rc;
 662         }
 663
 664         ep = MON_OFF2HDR(rp, rp->b_out);
 665
 666         if (rp->b_read < sizeof(struct mon_bin_hdr)) {
 667                 step_len = min(nbytes, sizeof(struct mon_bin_hdr) - rp->b_read);
 668                 ptr = ((char *)ep) + rp->b_read;
 669                 if (step_len && copy_to_user(buf, ptr, step_len)) {
 670                         mutex_unlock(&rp->fetch_lock);
 671                         return -EFAULT;
 672                 }
 673                 nbytes -= step_len;
 674                 buf += step_len;
 675                 rp->b_read += step_len;
 676                 done += step_len;
 677         }
 678
 679         if (rp->b_read >= sizeof(struct mon_bin_hdr)) {
 680                 step_len = min(nbytes, (size_t)ep->len_cap);
 681                 offset = rp->b_out + PKT_SIZE;
 682                 offset += rp->b_read - sizeof(struct mon_bin_hdr);
 683                 if (offset >= rp->b_size)
 684                         offset -= rp->b_size;
 685                 if (copy_from_buf(rp, offset, buf, step_len)) {
 686                         mutex_unlock(&rp->fetch_lock);
 687                         return -EFAULT;
 688                 }
 689                 nbytes -= step_len;
 690                 buf += step_len;
 691                 rp->b_read += step_len;
 692                 done += step_len;
 693         }
 694
 695         /*
 696          * Check if whole packet was read, and if so, jump to the next one.
 697          */
 698         if (rp->b_read >= sizeof(struct mon_bin_hdr) + ep->len_cap) {
 699                 spin_lock_irqsave(&rp->b_lock, flags);
 700                 mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
 701                 spin_unlock_irqrestore(&rp->b_lock, flags);
 702                 rp->b_read = 0;
 703         }
 704
 705         mutex_unlock(&rp->fetch_lock);
 706         return done;
 707 }
 708
 709 /*
 710  * Remove at most nevents from chunked buffer.
 711  * Returns the number of removed events.
 712  */
 713 static int mon_bin_flush(struct mon_reader_bin *rp, unsigned nevents)
 714 {
 715         unsigned long flags;
 716         struct mon_bin_hdr *ep;
 717         int i;
 718
 719         mutex_lock(&rp->fetch_lock);
 720         spin_lock_irqsave(&rp->b_lock, flags);
 721         for (i = 0; i < nevents; ++i) {
 722                 if (MON_RING_EMPTY(rp))
 723                         break;
 724
 725                 ep = MON_OFF2HDR(rp, rp->b_out);
 726                 mon_buff_area_free(rp, PKT_SIZE + ep->len_cap);
 727         }
 728         spin_unlock_irqrestore(&rp->b_lock, flags);
 729         rp->b_read = 0;
 730         mutex_unlock(&rp->fetch_lock);
 731         return i;
 732 }
 733
 734 /*
 735  * Fetch at most max event offsets into the buffer and put them into vec.
 736  * The events are usually freed later with mon_bin_flush.
 737  * Return the effective number of events fetched.
 738  */
 739 static int mon_bin_fetch(struct file *file, struct mon_reader_bin *rp,
 740     u32 __user *vec, unsigned int max)
 741 {
 742         unsigned int cur_out;
 743         unsigned int bytes, avail;
 744         unsigned int size;
 745         unsigned int nevents;
 746         struct mon_bin_hdr *ep;
 747         unsigned long flags;
 748         int rc;
 749
 750         mutex_lock(&rp->fetch_lock);
 751
 752         if ((rc = mon_bin_wait_event(file, rp)) < 0) {
 753                 mutex_unlock(&rp->fetch_lock);
 754                 return rc;
 755         }
 756
 757         spin_lock_irqsave(&rp->b_lock, flags);
 758         avail = rp->b_cnt;
 759         spin_unlock_irqrestore(&rp->b_lock, flags);
 760
 761         cur_out = rp->b_out;
 762         nevents = 0;
 763         bytes = 0;
 764         while (bytes < avail) {
 765                 if (nevents >= max)
 766                         break;
 767
 768                 ep = MON_OFF2HDR(rp, cur_out);
 769                 if (put_user(cur_out, &vec[nevents])) {
 770                         mutex_unlock(&rp->fetch_lock);
 771                         return -EFAULT;
 772                 }
 773
 774                 nevents++;
 775                 size = ep->len_cap + PKT_SIZE;
 776                 size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 777                 if ((cur_out += size) >= rp->b_size)
 778                         cur_out -= rp->b_size;
 779                 bytes += size;
 780         }
 781
 782         mutex_unlock(&rp->fetch_lock);
 783         return nevents;
 784 }
 785
 786 /*
 787  * Count events. This is almost the same as the above mon_bin_fetch,
 788  * only we do not store offsets into user vector, and we have no limit.
 789  */
 790 static int mon_bin_queued(struct mon_reader_bin *rp)
 791 {
 792         unsigned int cur_out;
 793         unsigned int bytes, avail;
 794         unsigned int size;
 795         unsigned int nevents;
 796         struct mon_bin_hdr *ep;
 797         unsigned long flags;
 798
 799         mutex_lock(&rp->fetch_lock);
 800
 801         spin_lock_irqsave(&rp->b_lock, flags);
 802         avail = rp->b_cnt;
 803         spin_unlock_irqrestore(&rp->b_lock, flags);
 804
 805         cur_out = rp->b_out;
 806         nevents = 0;
 807         bytes = 0;
 808         while (bytes < avail) {
 809                 ep = MON_OFF2HDR(rp, cur_out);
 810
 811                 nevents++;
 812                 size = ep->len_cap + PKT_SIZE;
 813                 size = (size + PKT_ALIGN-1) & ~(PKT_ALIGN-1);
 814                 if ((cur_out += size) >= rp->b_size)
 815                         cur_out -= rp->b_size;
 816                 bytes += size;
 817         }
 818
 819         mutex_unlock(&rp->fetch_lock);
 820         return nevents;
 821 }
 822
 823 /*
 824  */
 825 static int mon_bin_ioctl(struct inode *inode, struct file *file,
 826     unsigned int cmd, unsigned long arg)
 827 {
 828         struct mon_reader_bin *rp = file->private_data;
 829         // struct mon_bus* mbus = rp->r.m_bus;
 830         int ret = 0;
 831         struct mon_bin_hdr *ep;
 832         unsigned long flags;
 833
 834         switch (cmd) {
 835
 836         case MON_IOCQ_URB_LEN:
 837                 /*
 838                  * N.B. This only returns the size of data, without the header.
 839                  */
 840                 spin_lock_irqsave(&rp->b_lock, flags);
 841                 if (!MON_RING_EMPTY(rp)) {
 842                         ep = MON_OFF2HDR(rp, rp->b_out);
 843                         ret = ep->len_cap;
 844                 }
 845                 spin_unlock_irqrestore(&rp->b_lock, flags);
 846                 break;
 847
 848         case MON_IOCQ_RING_SIZE:
 849                 ret = rp->b_size;
 850                 break;
 851
 852         case MON_IOCT_RING_SIZE:
 853                 /*
 854                  * Changing the buffer size will flush it's contents; the new
 855                  * buffer is allocated before releasing the old one to be sure
 856                  * the device will stay functional also in case of memory
 857                  * pressure.
 858                  */
 859                 {
 860                 int size;
 861                 struct mon_pgmap *vec;
 862
 863                 if (arg < BUFF_MIN || arg > BUFF_MAX)
 864                         return -EINVAL;
 865
 866                 size = CHUNK_ALIGN(arg);
 867                 if ((vec = kzalloc(sizeof(struct mon_pgmap) * (size/CHUNK_SIZE),
 868                     GFP_KERNEL)) == NULL) {
 869                         ret = -ENOMEM;
 870                         break;
 871                 }
 872
 873                 ret = mon_alloc_buff(vec, size/CHUNK_SIZE);
 874                 if (ret < 0) {
 875                         kfree(vec);
 876                         break;
 877                 }
 878
 879                 mutex_lock(&rp->fetch_lock);
 880                 spin_lock_irqsave(&rp->b_lock, flags);
 881                 mon_free_buff(rp->b_vec, size/CHUNK_SIZE);
 882                 kfree(rp->b_vec);
 883                 rp->b_vec  = vec;
 884                 rp->b_size = size;
 885                 rp->b_read = rp->b_in = rp->b_out = rp->b_cnt = 0;
 886                 rp->cnt_lost = 0;
 887                 spin_unlock_irqrestore(&rp->b_lock, flags);
 888                 mutex_unlock(&rp->fetch_lock);
 889                 }
 890                 break;
 891
 892         case MON_IOCH_MFLUSH:
 893                 ret = mon_bin_flush(rp, arg);
 894                 break;
 895
 896         case MON_IOCX_GET:
 897                 {
 898                 struct mon_bin_get getb;
 899
 900                 if (copy_from_user(&getb, (void __user *)arg,
 901                                             sizeof(struct mon_bin_get)))
 902                         return -EFAULT;
 903
 904                 if (getb.alloc > 0x10000000)    /* Want to cast to u32 */
 905                         return -EINVAL;
 906                 ret = mon_bin_get_event(file, rp,
 907                           getb.hdr, getb.data, (unsigned int)getb.alloc);
 908                 }
 909                 break;
 910
 911 #ifdef CONFIG_COMPAT
 912         case MON_IOCX_GET32: {
 913                 struct mon_bin_get32 getb;
 914
 915                 if (copy_from_user(&getb, (void __user *)arg,
 916                                             sizeof(struct mon_bin_get32)))
 917                         return -EFAULT;
 918
 919                 ret = mon_bin_get_event(file, rp,
 920                     compat_ptr(getb.hdr32), compat_ptr(getb.data32),
 921                     getb.alloc32);
 922                 }
 923                 break;
 924 #endif
 925
 926         case MON_IOCX_MFETCH:
 927                 {
 928                 struct mon_bin_mfetch mfetch;
 929                 struct mon_bin_mfetch __user *uptr;
 930
 931                 uptr = (struct mon_bin_mfetch __user *)arg;
 932
 933                 if (copy_from_user(&mfetch, uptr, sizeof(mfetch)))
 934                         return -EFAULT;
 935
 936                 if (mfetch.nflush) {
 937                         ret = mon_bin_flush(rp, mfetch.nflush);
 938                         if (ret < 0)
 939                                 return ret;
 940                         if (put_user(ret, &uptr->nflush))
 941                                 return -EFAULT;
 942                 }
 943                 ret = mon_bin_fetch(file, rp, mfetch.offvec, mfetch.nfetch);
 944                 if (ret < 0)
 945                         return ret;
 946                 if (put_user(ret, &uptr->nfetch))
 947                         return -EFAULT;
 948                 ret = 0;
 949                 }
 950                 break;
 951
 952 #ifdef CONFIG_COMPAT
 953         case MON_IOCX_MFETCH32:
 954                 {
 955                 struct mon_bin_mfetch32 mfetch;
 956                 struct mon_bin_mfetch32 __user *uptr;
 957
 958                 uptr = (struct mon_bin_mfetch32 __user *) compat_ptr(arg);
 959
 960                 if (copy_from_user(&mfetch, uptr, sizeof(mfetch)))
 961                         return -EFAULT;
 962
 963                 if (mfetch.nflush32) {
 964                         ret = mon_bin_flush(rp, mfetch.nflush32);
 965                         if (ret < 0)
 966                                 return ret;
 967                         if (put_user(ret, &uptr->nflush32))
 968                                 return -EFAULT;
 969                 }
 970                 ret = mon_bin_fetch(file, rp, compat_ptr(mfetch.offvec32),
 971                     mfetch.nfetch32);
 972                 if (ret < 0)
 973                         return ret;
 974                 if (put_user(ret, &uptr->nfetch32))
 975                         return -EFAULT;
 976                 ret = 0;
 977                 }
 978                 break;
 979 #endif
 980
 981         case MON_IOCG_STATS: {
 982                 struct mon_bin_stats __user *sp;
 983                 unsigned int nevents;
 984                 unsigned int ndropped;
 985
 986                 spin_lock_irqsave(&rp->b_lock, flags);
 987                 ndropped = rp->cnt_lost;
 988                 rp->cnt_lost = 0;
 989                 spin_unlock_irqrestore(&rp->b_lock, flags);
 990                 nevents = mon_bin_queued(rp);
 991
 992                 sp = (struct mon_bin_stats __user *)arg;
 993                 if (put_user(rp->cnt_lost, &sp->dropped))
 994                         return -EFAULT;
 995                 if (put_user(nevents, &sp->queued))
 996                         return -EFAULT;
 997
 998                 }
 999                 break;
1000
1001         default:
1002                 return -ENOTTY;
1003         }
1004
1005         return ret;
1006 }
1007
1008 static unsigned int
1009 mon_bin_poll(struct file *file, struct poll_table_struct *wait)
1010 {
1011         struct mon_reader_bin *rp = file->private_data;
1012         unsigned int mask = 0;
1013         unsigned long flags;
1014
1015         if (file->f_mode & FMODE_READ)
1016                 poll_wait(file, &rp->b_wait, wait);
1017
1018         spin_lock_irqsave(&rp->b_lock, flags);
1019         if (!MON_RING_EMPTY(rp))
1020                 mask |= POLLIN | POLLRDNORM;    /* readable */
1021         spin_unlock_irqrestore(&rp->b_lock, flags);
1022         return mask;
1023 }
1024
1025 /*
1026  * open and close: just keep track of how many times the device is
1027  * mapped, to use the proper memory allocation function.
1028  */
1029 static void mon_bin_vma_open(struct vm_area_struct *vma)
1030 {
1031         struct mon_reader_bin *rp = vma->vm_private_data;
1032         rp->mmap_active++;
1033 }
1034
1035 static void mon_bin_vma_close(struct vm_area_struct *vma)
1036 {
1037         struct mon_reader_bin *rp = vma->vm_private_data;
1038         rp->mmap_active--;
1039 }
1040
1041 /*
1042  * Map ring pages to user space.
1043  */
1044 struct page *mon_bin_vma_nopage(struct vm_area_struct *vma,
1045                                 unsigned long address, int *type)
1046 {
1047         struct mon_reader_bin *rp = vma->vm_private_data;
1048         unsigned long offset, chunk_idx;
1049         struct page *pageptr;
1050
1051         offset = (address - vma->vm_start) + (vma->vm_pgoff << PAGE_SHIFT);
1052         if (offset >= rp->b_size)
1053                 return NOPAGE_SIGBUS;
1054         chunk_idx = offset / CHUNK_SIZE;
1055         pageptr = rp->b_vec[chunk_idx].pg;
1056         get_page(pageptr);
1057         if (type)
1058                 *type = VM_FAULT_MINOR;
1059         return pageptr;
1060 }
1061
1062 struct vm_operations_struct mon_bin_vm_ops = {
1063         .open =     mon_bin_vma_open,
1064         .close =    mon_bin_vma_close,
1065         .nopage =   mon_bin_vma_nopage,
1066 };
1067
1068 int mon_bin_mmap(struct file *filp, struct vm_area_struct *vma)
1069 {
1070         /* don't do anything here: "nopage" will set up page table entries */
1071         vma->vm_ops = &mon_bin_vm_ops;
1072         vma->vm_flags |= VM_RESERVED;
1073         vma->vm_private_data = filp->private_data;
1074         mon_bin_vma_open(vma);
1075         return 0;
1076 }
1077
1078 struct file_operations mon_fops_binary = {
1079         .owner =        THIS_MODULE,
1080         .open =         mon_bin_open,
1081         .llseek =       no_llseek,
1082         .read =         mon_bin_read,
1083         /* .write =     mon_text_write, */
1084         .poll =         mon_bin_poll,
1085         .ioctl =        mon_bin_ioctl,
1086         .release =      mon_bin_release,
1087 };
1088
1089 static int mon_bin_wait_event(struct file *file, struct mon_reader_bin *rp)
1090 {
1091         DECLARE_WAITQUEUE(waita, current);
1092         unsigned long flags;
1093
1094         add_wait_queue(&rp->b_wait, &waita);
1095         set_current_state(TASK_INTERRUPTIBLE);
1096
1097         spin_lock_irqsave(&rp->b_lock, flags);
1098         while (MON_RING_EMPTY(rp)) {
1099                 spin_unlock_irqrestore(&rp->b_lock, flags);
1100
1101                 if (file->f_flags & O_NONBLOCK) {
1102                         set_current_state(TASK_RUNNING);
1103                         remove_wait_queue(&rp->b_wait, &waita);
1104                         return -EWOULDBLOCK; /* Same as EAGAIN in Linux */
1105                 }
1106                 schedule();
1107                 if (signal_pending(current)) {
1108                         remove_wait_queue(&rp->b_wait, &waita);
1109                         return -EINTR;
1110                 }
1111                 set_current_state(TASK_INTERRUPTIBLE);
1112
1113                 spin_lock_irqsave(&rp->b_lock, flags);
1114         }
1115         spin_unlock_irqrestore(&rp->b_lock, flags);
1116
1117         set_current_state(TASK_RUNNING);
1118         remove_wait_queue(&rp->b_wait, &waita);
1119         return 0;
1120 }
1121
1122 static int mon_alloc_buff(struct mon_pgmap *map, int npages)
1123 {
1124         int n;
1125         unsigned long vaddr;
1126
1127         for (n = 0; n < npages; n++) {
1128                 vaddr = get_zeroed_page(GFP_KERNEL);
1129                 if (vaddr == 0) {
1130                         while (n-- != 0)
1131                                 free_page((unsigned long) map[n].ptr);
1132                         return -ENOMEM;
1133                 }
1134                 map[n].ptr = (unsigned char *) vaddr;
1135                 map[n].pg = virt_to_page(vaddr);
1136         }
1137         return 0;
1138 }
1139
1140 static void mon_free_buff(struct mon_pgmap *map, int npages)
1141 {
1142         int n;
1143
1144         for (n = 0; n < npages; n++)
1145                 free_page((unsigned long) map[n].ptr);
1146 }
1147
1148 int mon_bin_add(struct mon_bus *mbus, const struct usb_bus *ubus)
1149 {
1150         struct device *dev;
1151         unsigned minor = ubus? ubus->busnum: 0;
1152
1153         if (minor >= MON_BIN_MAX_MINOR)
1154                 return 0;
1155
1156         dev = device_create(mon_bin_class, ubus? ubus->controller: NULL,
1157                         MKDEV(MAJOR(mon_bin_dev0), minor), "usbmon%d", minor);
1158         if (IS_ERR(dev))
1159                 return 0;
1160
1161         mbus->classdev = dev;
1162         return 1;
1163 }
1164
1165 void mon_bin_del(struct mon_bus *mbus)
1166 {
1167         device_destroy(mon_bin_class, mbus->classdev->devt);
1168 }
1169
1170 int __init mon_bin_init(void)
1171 {
1172         int rc;
1173
1174         mon_bin_class = class_create(THIS_MODULE, "usbmon");
1175         if (IS_ERR(mon_bin_class)) {
1176                 rc = PTR_ERR(mon_bin_class);
1177                 goto err_class;
1178         }
1179
1180         rc = alloc_chrdev_region(&mon_bin_dev0, 0, MON_BIN_MAX_MINOR, "usbmon");
1181         if (rc < 0)
1182                 goto err_dev;
1183
1184         cdev_init(&mon_bin_cdev, &mon_fops_binary);
1185         mon_bin_cdev.owner = THIS_MODULE;
1186
1187         rc = cdev_add(&mon_bin_cdev, mon_bin_dev0, MON_BIN_MAX_MINOR);
1188         if (rc < 0)
1189                 goto err_add;
1190
1191         return 0;
1192
1193 err_add:
1194         unregister_chrdev_region(mon_bin_dev0, MON_BIN_MAX_MINOR);
1195 err_dev:
1196         class_destroy(mon_bin_class);
1197 err_class:
1198         return rc;
1199 }
1200
1201 void mon_bin_exit(void)
1202 {
1203         cdev_del(&mon_bin_cdev);
1204         unregister_chrdev_region(mon_bin_dev0, MON_BIN_MAX_MINOR);
1205         class_destroy(mon_bin_class);
1206 }