Pull netlink into release branch
[linux-2.6] / 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 }