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