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