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