svn commit: r215649 - in head/sys: conf dev/usb dev/usb/controller modules/usb/usb

Weongyo Jeong weongyo at FreeBSD.org
Mon Nov 22 01:11:28 UTC 2010


Author: weongyo
Date: Mon Nov 22 01:11:28 2010
New Revision: 215649
URL: http://svn.freebsd.org/changeset/base/215649

Log:
  Adds a USB packet filter feature to the stack that it could capture
  packets which go through each USB host controllers.  Its implementations
  are almost based on BPF code and very similar with it except it's
  little bit customized for USB packet only.  The userland program
  usbdump(8) would be committed soon.
  
  Discussed with:	hps, thompsa, yongari

Added:
  head/sys/dev/usb/usb_pf.c   (contents, props changed)
  head/sys/dev/usb/usb_pf.h   (contents, props changed)
Modified:
  head/sys/conf/files
  head/sys/dev/usb/controller/usb_controller.c
  head/sys/dev/usb/usb_bus.h
  head/sys/dev/usb/usb_controller.h
  head/sys/dev/usb/usb_transfer.c
  head/sys/modules/usb/usb/Makefile

Modified: head/sys/conf/files
==============================================================================
--- head/sys/conf/files	Mon Nov 22 00:44:43 2010	(r215648)
+++ head/sys/conf/files	Mon Nov 22 01:11:28 2010	(r215649)
@@ -1779,6 +1779,7 @@ dev/usb/usb_lookup.c		optional usb
 dev/usb/usb_mbuf.c		optional usb
 dev/usb/usb_msctest.c		optional usb
 dev/usb/usb_parse.c		optional usb
+dev/usb/usb_pf.c		optional usb
 dev/usb/usb_process.c		optional usb
 dev/usb/usb_request.c		optional usb
 dev/usb/usb_transfer.c		optional usb

Modified: head/sys/dev/usb/controller/usb_controller.c
==============================================================================
--- head/sys/dev/usb/controller/usb_controller.c	Mon Nov 22 00:44:43 2010	(r215648)
+++ head/sys/dev/usb/controller/usb_controller.c	Mon Nov 22 01:11:28 2010	(r215649)
@@ -61,6 +61,7 @@
 
 #include <dev/usb/usb_controller.h>
 #include <dev/usb/usb_bus.h>
+#include <dev/usb/usb_pf.h>
 
 /* function prototypes  */
 
@@ -547,6 +548,8 @@ usb_bus_mem_alloc_all(struct usb_bus *bu
 
 	TAILQ_INIT(&bus->intr_q.head);
 
+	usbpf_attach(bus, &bus->uif);
+
 #if USB_HAVE_BUSDMA
 	usb_dma_tag_setup(bus->dma_parent_tag, bus->dma_tags,
 	    dmat, &bus->bus_mtx, NULL, 32, USB_BUS_DMA_TAG_MAX);
@@ -594,5 +597,34 @@ usb_bus_mem_free_all(struct usb_bus *bus
 	usb_dma_tag_unsetup(bus->dma_parent_tag);
 #endif
 
+	usbpf_detach(bus);
+
 	mtx_destroy(&bus->bus_mtx);
 }
+
+struct usb_bus *
+usb_bus_find(const char *name)
+{
+	struct usb_bus *ubus;
+	devclass_t dc;
+	device_t *devlist;
+	int devcount, error, i;
+	const char *nameunit;
+
+	dc = devclass_find("usbus");
+	if (dc == NULL)
+		return (NULL);
+	error = devclass_get_devices(dc, &devlist, &devcount);
+	if (error != 0)
+		return (NULL);
+	for (i = 0; i < devcount; i++) {
+		nameunit = device_get_nameunit(devlist[i]);
+		if (!strncmp(name, nameunit, strlen(nameunit))) {
+			ubus = device_get_ivars(devlist[i]);
+			free(devlist, M_TEMP);
+			return (ubus);
+		}
+	}
+	free(devlist, M_TEMP);
+	return (NULL);
+}

Modified: head/sys/dev/usb/usb_bus.h
==============================================================================
--- head/sys/dev/usb/usb_bus.h	Mon Nov 22 00:44:43 2010	(r215648)
+++ head/sys/dev/usb/usb_bus.h	Mon Nov 22 01:11:28 2010	(r215649)
@@ -86,6 +86,8 @@ struct usb_bus {
 	struct usb_bus_methods *methods;	/* filled by HC driver */
 	struct usb_device **devices;
 
+	struct usbpf_if *uif;	/* USB Packet Filter */
+
 	usb_power_mask_t hw_power_state;	/* see USB_HW_POWER_XXX */
 	usb_size_t uframe_usage[USB_HS_MICRO_FRAMES_MAX];
 

Modified: head/sys/dev/usb/usb_controller.h
==============================================================================
--- head/sys/dev/usb/usb_controller.h	Mon Nov 22 00:44:43 2010	(r215648)
+++ head/sys/dev/usb/usb_controller.h	Mon Nov 22 01:11:28 2010	(r215649)
@@ -221,5 +221,6 @@ void	usb_bus_mem_free_all(struct usb_bus
 uint16_t usb_isoc_time_expand(struct usb_bus *bus, uint16_t isoc_time_curr);
 uint16_t usbd_fs_isoc_schedule_isoc_time_expand(struct usb_device *udev, struct usb_fs_isoc_schedule **pp_start, struct usb_fs_isoc_schedule **pp_end, uint16_t isoc_time);
 uint8_t	usbd_fs_isoc_schedule_alloc(struct usb_fs_isoc_schedule *fss, uint8_t *pstart, uint16_t len);
+struct usb_bus *usb_bus_find(const char *name);
 
 #endif					/* _USB_CONTROLLER_H_ */

Added: head/sys/dev/usb/usb_pf.c
==============================================================================
--- /dev/null	00:00:00 1970	(empty, because file is newly added)
+++ head/sys/dev/usb/usb_pf.c	Mon Nov 22 01:11:28 2010	(r215649)
@@ -0,0 +1,1862 @@
+/*-
+ * Copyright (c) 1990, 1991, 1993
+ *	The Regents of the University of California.  All rights reserved.
+ *
+ * This code is derived from the Stanford/CMU enet packet filter,
+ * (net/enet.c) distributed as part of 4.3BSD, and code contributed
+ * to Berkeley by Steven McCanne and Van Jacobson both of Lawrence
+ * Berkeley Laboratory.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 4. Neither the name of the University nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+#include <sys/param.h>
+#include <sys/kernel.h>
+#include <sys/bus.h>
+#include <sys/fcntl.h>
+#include <sys/malloc.h>
+#include <sys/proc.h>
+#include <sys/socket.h>
+#include <sys/sockio.h>
+#include <net/if.h>
+
+#include <dev/usb/usb.h>
+#include <dev/usb/usbdi.h>
+#include <dev/usb/usb_busdma.h>
+#include <dev/usb/usb_controller.h>
+#include <dev/usb/usb_core.h>
+#include <dev/usb/usb_process.h>
+#include <dev/usb/usb_device.h>
+#include <dev/usb/usb_bus.h>
+#include <dev/usb/usb_pf.h>
+#include <dev/usb/usb_transfer.h>
+
+/*
+ * All usbpf implementations are extracted from bpf(9) APIs and it's
+ * specialized for USB packet filtering between the driver and the host
+ * controller.
+ */
+
+MALLOC_DEFINE(M_USBPF, "USBPktFilter", "USB Packet Filter");
+
+/*
+ * Rotate the packet buffers in descriptor ud.  Move the store buffer into the
+ * hold slot, and the free buffer ino the store slot.  Zero the length of the
+ * new store buffer.  Descriptor lock should be held.
+ */
+#define	USBPF_ROTATE_BUFFERS(ud)	do {				\
+	(ud)->ud_hbuf = (ud)->ud_sbuf;					\
+	(ud)->ud_hlen = (ud)->ud_slen;					\
+	(ud)->ud_sbuf = (ud)->ud_fbuf;					\
+	(ud)->ud_slen = 0;						\
+	(ud)->ud_fbuf = NULL;						\
+	usbpf_bufheld(ud);						\
+} while (0)
+
+#ifndef __i386__
+#define	USBPF_ALIGN
+#endif
+
+#ifndef USBPF_ALIGN
+#define	USBPF_EXTRACT_SHORT(p)	((u_int16_t)ntohs(*(u_int16_t *)p))
+#define	USBPF_EXTRACT_LONG(p)	(ntohl(*(u_int32_t *)p))
+#else
+#define	USBPF_EXTRACT_SHORT(p)						\
+	((u_int16_t)							\
+	    ((u_int16_t)*((u_char *)p+0)<<8|				\
+		(u_int16_t)*((u_char *)p+1)<<0))
+#define	USBPF_EXTRACT_LONG(p)						\
+	((u_int32_t)*((u_char *)p+0)<<24|				\
+	    (u_int32_t)*((u_char *)p+1)<<16|				\
+	    (u_int32_t)*((u_char *)p+2)<<8|				\
+	    (u_int32_t)*((u_char *)p+3)<<0)
+#endif
+
+/*
+ * Number of scratch memory words (for USBPF_LD|USBPF_MEM and USBPF_ST).
+ */
+#define	USBPF_MEMWORDS		 16
+
+/* Values for ud_state */
+#define	USBPF_IDLE		0	/* no select in progress */
+#define	USBPF_WAITING		1	/* waiting for read timeout in select */
+#define	USBPF_TIMED_OUT		2	/* read timeout has expired in select */
+
+#define	PRIUSB			26	/* interruptible */
+
+/* Frame directions */
+enum usbpf_direction {
+	USBPF_D_IN,	/* See incoming frames */
+	USBPF_D_INOUT,	/* See incoming and outgoing frames */
+	USBPF_D_OUT	/* See outgoing frames */
+};
+
+static void	usbpf_append_bytes(struct usbpf_d *, caddr_t, u_int, void *,
+		    u_int);
+static void	usbpf_attachd(struct usbpf_d *, struct usbpf_if *);
+static void	usbpf_detachd(struct usbpf_d *);
+static int	usbpf_canfreebuf(struct usbpf_d *);
+static void	usbpf_buf_reclaimed(struct usbpf_d *);
+static int	usbpf_canwritebuf(struct usbpf_d *);
+
+static	d_open_t	usbpf_open;
+static	d_read_t	usbpf_read;
+static	d_write_t	usbpf_write;
+static	d_ioctl_t	usbpf_ioctl;
+static	d_poll_t	usbpf_poll;
+static	d_kqfilter_t	usbpf_kqfilter;
+
+static struct cdevsw usbpf_cdevsw = {
+	.d_version =	D_VERSION,
+	.d_open =	usbpf_open,
+	.d_read =	usbpf_read,
+	.d_write =	usbpf_write,
+	.d_ioctl =	usbpf_ioctl,
+	.d_poll =	usbpf_poll,
+	.d_name =	"usbpf",
+	.d_kqfilter =	usbpf_kqfilter,
+};
+
+static LIST_HEAD(, usbpf_if)	usbpf_iflist;
+static struct mtx	usbpf_mtx;		/* global lock */
+static int usbpf_uifd_cnt;
+
+static int usbpf_bufsize = 4096;
+#define	USBPF_MINBUFSIZE 32
+#define	USBPF_MAXBUFSIZE 0x80000
+static int usbpf_maxbufsize = USBPF_MAXBUFSIZE;
+#define	USBPF_MAXINSNS 512
+static int usbpf_maxinsns = USBPF_MAXINSNS;
+
+static void
+usbpf_buffer_init(struct usbpf_d *ud)
+{
+
+	ud->ud_bufsize = usbpf_bufsize;
+}
+
+/*
+ * Free USBPF kernel buffers on device close.
+ */
+static void
+usbpf_buffer_free(struct usbpf_d *ud)
+{
+
+	if (ud->ud_sbuf != NULL)
+		free(ud->ud_sbuf, M_USBPF);
+	if (ud->ud_hbuf != NULL)
+		free(ud->ud_hbuf, M_USBPF);
+	if (ud->ud_fbuf != NULL)
+		free(ud->ud_fbuf, M_USBPF);
+
+#ifdef INVARIANTS
+	ud->ud_sbuf = ud->ud_hbuf = ud->ud_fbuf = (caddr_t)~0;
+#endif
+}
+
+static void
+usbpf_buffer_alloc(struct usbpf_d *ud)
+{
+
+	KASSERT(ud->ud_fbuf == NULL, ("%s: ud_fbuf != NULL", __func__));
+	KASSERT(ud->ud_sbuf == NULL, ("%s: ud_sbuf != NULL", __func__));
+	KASSERT(ud->ud_hbuf == NULL, ("%s: ud_hbuf != NULL", __func__));
+
+	ud->ud_fbuf = (caddr_t)malloc(ud->ud_bufsize, M_USBPF, M_WAITOK);
+	ud->ud_sbuf = (caddr_t)malloc(ud->ud_bufsize, M_USBPF, M_WAITOK);
+	ud->ud_hbuf = NULL;
+	ud->ud_slen = 0;
+	ud->ud_hlen = 0;
+}
+
+/*
+ * Copy buffer storage to user space in read().
+ */
+static int
+usbpf_buffer_uiomove(struct usbpf_d *ud, caddr_t buf, u_int len,
+    struct uio *uio)
+{
+
+	return (uiomove(buf, len, uio));
+}
+
+/*
+ * Simple data copy to the current kernel buffer.
+ */
+static void
+usbpf_buffer_append_bytes(struct usbpf_d *ud, caddr_t buf, u_int offset,
+    void *src, u_int len)
+{
+	u_char *src_bytes;
+
+	src_bytes = (u_char *)src;
+	bcopy(src_bytes, buf + offset, len);
+}
+
+/*
+ * Allocate or resize buffers.
+ */
+static int
+usbpf_buffer_ioctl_sblen(struct usbpf_d *ud, u_int *i)
+{
+	u_int size;
+
+	USBPFD_LOCK(ud);
+	if (ud->ud_bif != NULL) {
+		USBPFD_UNLOCK(ud);
+		return (EINVAL);
+	}
+	size = *i;
+	if (size > usbpf_maxbufsize)
+		*i = size = usbpf_maxbufsize;
+	else if (size < USBPF_MINBUFSIZE)
+		*i = size = USBPF_MINBUFSIZE;
+	ud->ud_bufsize = size;
+	USBPFD_UNLOCK(ud);
+	return (0);
+}
+
+static const u_short	usbpf_code_map[] = {
+	0x10ff,	/* 0x00-0x0f: 1111111100001000 */
+	0x3070,	/* 0x10-0x1f: 0000111000001100 */
+	0x3131,	/* 0x20-0x2f: 1000110010001100 */
+	0x3031,	/* 0x30-0x3f: 1000110000001100 */
+	0x3131,	/* 0x40-0x4f: 1000110010001100 */
+	0x1011,	/* 0x50-0x5f: 1000100000001000 */
+	0x1013,	/* 0x60-0x6f: 1100100000001000 */
+	0x1010,	/* 0x70-0x7f: 0000100000001000 */
+	0x0093,	/* 0x80-0x8f: 1100100100000000 */
+	0x0000,	/* 0x90-0x9f: 0000000000000000 */
+	0x0000,	/* 0xa0-0xaf: 0000000000000000 */
+	0x0002,	/* 0xb0-0xbf: 0100000000000000 */
+	0x0000,	/* 0xc0-0xcf: 0000000000000000 */
+	0x0000,	/* 0xd0-0xdf: 0000000000000000 */
+	0x0000,	/* 0xe0-0xef: 0000000000000000 */
+	0x0000	/* 0xf0-0xff: 0000000000000000 */
+};
+
+#define	USBPF_VALIDATE_CODE(c)	\
+    ((c) <= 0xff && (usbpf_code_map[(c) >> 4] & (1 << ((c) & 0xf))) != 0)
+
+/*
+ * Return true if the 'fcode' is a valid filter program.
+ * The constraints are that each jump be forward and to a valid
+ * code.  The code must terminate with either an accept or reject.
+ *
+ * The kernel needs to be able to verify an application's filter code.
+ * Otherwise, a bogus program could easily crash the system.
+ */
+static int
+usbpf_validate(const struct usbpf_insn *f, int len)
+{
+	register int i;
+	register const struct usbpf_insn *p;
+
+	/* Do not accept negative length filter. */
+	if (len < 0)
+		return (0);
+
+	/* An empty filter means accept all. */
+	if (len == 0)
+		return (1);
+
+	for (i = 0; i < len; ++i) {
+		p = &f[i];
+		/*
+		 * Check that the code is valid.
+		 */
+		if (!USBPF_VALIDATE_CODE(p->code))
+			return (0);
+		/*
+		 * Check that that jumps are forward, and within
+		 * the code block.
+		 */
+		if (USBPF_CLASS(p->code) == USBPF_JMP) {
+			register u_int offset;
+
+			if (p->code == (USBPF_JMP|USBPF_JA))
+				offset = p->k;
+			else
+				offset = p->jt > p->jf ? p->jt : p->jf;
+			if (offset >= (u_int)(len - i) - 1)
+				return (0);
+			continue;
+		}
+		/*
+		 * Check that memory operations use valid addresses.
+		 */
+		if (p->code == USBPF_ST || p->code == USBPF_STX ||
+		    p->code == (USBPF_LD|USBPF_MEM) ||
+		    p->code == (USBPF_LDX|USBPF_MEM)) {
+			if (p->k >= USBPF_MEMWORDS)
+				return (0);
+			continue;
+		}
+		/*
+		 * Check for constant division by 0.
+		 */
+		if (p->code == (USBPF_ALU|USBPF_DIV|USBPF_K) && p->k == 0)
+			return (0);
+	}
+	return (USBPF_CLASS(f[len - 1].code) == USBPF_RET);
+}
+
+#ifdef _KERNEL
+#define	MINDEX(m, k) \
+{ \
+	register int len = m->m_len; \
+ \
+	while (k >= len) { \
+		k -= len; \
+		m = m->m_next; \
+		if (m == 0) \
+			return (0); \
+		len = m->m_len; \
+	} \
+}
+
+static u_int16_t	m_xhalf(struct mbuf *m, usbpf_u_int32 k, int *err);
+static u_int32_t	m_xword(struct mbuf *m, usbpf_u_int32 k, int *err);
+
+static u_int32_t
+m_xword(struct mbuf *m, usbpf_u_int32 k, int *err)
+{
+	size_t len;
+	u_char *cp, *np;
+	struct mbuf *m0;
+
+	len = m->m_len;
+	while (k >= len) {
+		k -= len;
+		m = m->m_next;
+		if (m == 0)
+			goto bad;
+		len = m->m_len;
+	}
+	cp = mtod(m, u_char *) + k;
+	if (len - k >= 4) {
+		*err = 0;
+		return (USBPF_EXTRACT_LONG(cp));
+	}
+	m0 = m->m_next;
+	if (m0 == 0 || m0->m_len + len - k < 4)
+		goto bad;
+	*err = 0;
+	np = mtod(m0, u_char *);
+	switch (len - k) {
+	case 1:
+		return (((u_int32_t)cp[0] << 24) |
+		    ((u_int32_t)np[0] << 16) |
+		    ((u_int32_t)np[1] << 8)  |
+		    (u_int32_t)np[2]);
+
+	case 2:
+		return (((u_int32_t)cp[0] << 24) |
+		    ((u_int32_t)cp[1] << 16) |
+		    ((u_int32_t)np[0] << 8) |
+		    (u_int32_t)np[1]);
+
+	default:
+		return (((u_int32_t)cp[0] << 24) |
+		    ((u_int32_t)cp[1] << 16) |
+		    ((u_int32_t)cp[2] << 8) |
+		    (u_int32_t)np[0]);
+	}
+    bad:
+	*err = 1;
+	return (0);
+}
+
+static u_int16_t
+m_xhalf(struct mbuf *m, usbpf_u_int32 k, int *err)
+{
+	size_t len;
+	u_char *cp;
+	struct mbuf *m0;
+
+	len = m->m_len;
+	while (k >= len) {
+		k -= len;
+		m = m->m_next;
+		if (m == 0)
+			goto bad;
+		len = m->m_len;
+	}
+	cp = mtod(m, u_char *) + k;
+	if (len - k >= 2) {
+		*err = 0;
+		return (USBPF_EXTRACT_SHORT(cp));
+	}
+	m0 = m->m_next;
+	if (m0 == 0)
+		goto bad;
+	*err = 0;
+	return ((cp[0] << 8) | mtod(m0, u_char *)[0]);
+ bad:
+	*err = 1;
+	return (0);
+}
+#endif
+
+/*
+ * Execute the filter program starting at pc on the packet p
+ * wirelen is the length of the original packet
+ * buflen is the amount of data present
+ */
+static u_int
+usbpf_filter(const struct usbpf_insn *pc, u_char *p, u_int wirelen,
+    u_int buflen)
+{
+	u_int32_t A = 0, X = 0;
+	usbpf_u_int32 k;
+	u_int32_t mem[USBPF_MEMWORDS];
+
+	/*
+	 * XXX temporarily the filter system is disabled because currently it
+	 * could not handle the some machine code properly that leads to
+	 * kernel crash by invalid usage.
+	 */
+	return ((u_int)-1);
+
+	if (pc == NULL)
+		/*
+		 * No filter means accept all.
+		 */
+		return ((u_int)-1);
+
+	--pc;
+	while (1) {
+		++pc;
+		switch (pc->code) {
+		default:
+#ifdef _KERNEL
+			return (0);
+#else
+			abort();
+#endif
+
+		case USBPF_RET|USBPF_K:
+			return ((u_int)pc->k);
+
+		case USBPF_RET|USBPF_A:
+			return ((u_int)A);
+
+		case USBPF_LD|USBPF_W|USBPF_ABS:
+			k = pc->k;
+			if (k > buflen || sizeof(int32_t) > buflen - k) {
+#ifdef _KERNEL
+				int merr;
+
+				if (buflen != 0)
+					return (0);
+				A = m_xword((struct mbuf *)p, k, &merr);
+				if (merr != 0)
+					return (0);
+				continue;
+#else
+				return (0);
+#endif
+			}
+#ifdef USBPF_ALIGN
+			if (((intptr_t)(p + k) & 3) != 0)
+				A = USBPF_EXTRACT_LONG(&p[k]);
+			else
+#endif
+				A = ntohl(*(int32_t *)(p + k));
+			continue;
+
+		case USBPF_LD|USBPF_H|USBPF_ABS:
+			k = pc->k;
+			if (k > buflen || sizeof(int16_t) > buflen - k) {
+#ifdef _KERNEL
+				int merr;
+
+				if (buflen != 0)
+					return (0);
+				A = m_xhalf((struct mbuf *)p, k, &merr);
+				continue;
+#else
+				return (0);
+#endif
+			}
+			A = USBPF_EXTRACT_SHORT(&p[k]);
+			continue;
+
+		case USBPF_LD|USBPF_B|USBPF_ABS:
+			k = pc->k;
+			if (k >= buflen) {
+#ifdef _KERNEL
+				struct mbuf *m;
+
+				if (buflen != 0)
+					return (0);
+				m = (struct mbuf *)p;
+				MINDEX(m, k);
+				A = mtod(m, u_char *)[k];
+				continue;
+#else
+				return (0);
+#endif
+			}
+			A = p[k];
+			continue;
+
+		case USBPF_LD|USBPF_W|USBPF_LEN:
+			A = wirelen;
+			continue;
+
+		case USBPF_LDX|USBPF_W|USBPF_LEN:
+			X = wirelen;
+			continue;
+
+		case USBPF_LD|USBPF_W|USBPF_IND:
+			k = X + pc->k;
+			if (pc->k > buflen || X > buflen - pc->k ||
+			    sizeof(int32_t) > buflen - k) {
+#ifdef _KERNEL
+				int merr;
+
+				if (buflen != 0)
+					return (0);
+				A = m_xword((struct mbuf *)p, k, &merr);
+				if (merr != 0)
+					return (0);
+				continue;
+#else
+				return (0);
+#endif
+			}
+#ifdef USBPF_ALIGN
+			if (((intptr_t)(p + k) & 3) != 0)
+				A = USBPF_EXTRACT_LONG(&p[k]);
+			else
+#endif
+				A = ntohl(*(int32_t *)(p + k));
+			continue;
+
+		case USBPF_LD|USBPF_H|USBPF_IND:
+			k = X + pc->k;
+			if (X > buflen || pc->k > buflen - X ||
+			    sizeof(int16_t) > buflen - k) {
+#ifdef _KERNEL
+				int merr;
+
+				if (buflen != 0)
+					return (0);
+				A = m_xhalf((struct mbuf *)p, k, &merr);
+				if (merr != 0)
+					return (0);
+				continue;
+#else
+				return (0);
+#endif
+			}
+			A = USBPF_EXTRACT_SHORT(&p[k]);
+			continue;
+
+		case USBPF_LD|USBPF_B|USBPF_IND:
+			k = X + pc->k;
+			if (pc->k >= buflen || X >= buflen - pc->k) {
+#ifdef _KERNEL
+				struct mbuf *m;
+
+				if (buflen != 0)
+					return (0);
+				m = (struct mbuf *)p;
+				MINDEX(m, k);
+				A = mtod(m, u_char *)[k];
+				continue;
+#else
+				return (0);
+#endif
+			}
+			A = p[k];
+			continue;
+
+		case USBPF_LDX|USBPF_MSH|USBPF_B:
+			k = pc->k;
+			if (k >= buflen) {
+#ifdef _KERNEL
+				register struct mbuf *m;
+
+				if (buflen != 0)
+					return (0);
+				m = (struct mbuf *)p;
+				MINDEX(m, k);
+				X = (mtod(m, u_char *)[k] & 0xf) << 2;
+				continue;
+#else
+				return (0);
+#endif
+			}
+			X = (p[pc->k] & 0xf) << 2;
+			continue;
+
+		case USBPF_LD|USBPF_IMM:
+			A = pc->k;
+			continue;
+
+		case USBPF_LDX|USBPF_IMM:
+			X = pc->k;
+			continue;
+
+		case USBPF_LD|USBPF_MEM:
+			A = mem[pc->k];
+			continue;
+
+		case USBPF_LDX|USBPF_MEM:
+			X = mem[pc->k];
+			continue;
+
+		case USBPF_ST:
+			mem[pc->k] = A;
+			continue;
+
+		case USBPF_STX:
+			mem[pc->k] = X;
+			continue;
+
+		case USBPF_JMP|USBPF_JA:
+			pc += pc->k;
+			continue;
+
+		case USBPF_JMP|USBPF_JGT|USBPF_K:
+			pc += (A > pc->k) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JGE|USBPF_K:
+			pc += (A >= pc->k) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JEQ|USBPF_K:
+			pc += (A == pc->k) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JSET|USBPF_K:
+			pc += (A & pc->k) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JGT|USBPF_X:
+			pc += (A > X) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JGE|USBPF_X:
+			pc += (A >= X) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JEQ|USBPF_X:
+			pc += (A == X) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_JMP|USBPF_JSET|USBPF_X:
+			pc += (A & X) ? pc->jt : pc->jf;
+			continue;
+
+		case USBPF_ALU|USBPF_ADD|USBPF_X:
+			A += X;
+			continue;
+
+		case USBPF_ALU|USBPF_SUB|USBPF_X:
+			A -= X;
+			continue;
+
+		case USBPF_ALU|USBPF_MUL|USBPF_X:
+			A *= X;
+			continue;
+
+		case USBPF_ALU|USBPF_DIV|USBPF_X:
+			if (X == 0)
+				return (0);
+			A /= X;
+			continue;
+
+		case USBPF_ALU|USBPF_AND|USBPF_X:
+			A &= X;
+			continue;
+
+		case USBPF_ALU|USBPF_OR|USBPF_X:
+			A |= X;
+			continue;
+
+		case USBPF_ALU|USBPF_LSH|USBPF_X:
+			A <<= X;
+			continue;
+
+		case USBPF_ALU|USBPF_RSH|USBPF_X:
+			A >>= X;
+			continue;
+
+		case USBPF_ALU|USBPF_ADD|USBPF_K:
+			A += pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_SUB|USBPF_K:
+			A -= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_MUL|USBPF_K:
+			A *= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_DIV|USBPF_K:
+			A /= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_AND|USBPF_K:
+			A &= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_OR|USBPF_K:
+			A |= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_LSH|USBPF_K:
+			A <<= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_RSH|USBPF_K:
+			A >>= pc->k;
+			continue;
+
+		case USBPF_ALU|USBPF_NEG:
+			A = -A;
+			continue;
+
+		case USBPF_MISC|USBPF_TAX:
+			X = A;
+			continue;
+
+		case USBPF_MISC|USBPF_TXA:
+			A = X;
+			continue;
+		}
+	}
+}
+
+static void
+usbpf_free(struct usbpf_d *ud)
+{
+
+	switch (ud->ud_bufmode) {
+	case USBPF_BUFMODE_BUFFER:
+		return (usbpf_buffer_free(ud));
+	default:
+		panic("usbpf_buf_free");
+	}
+}
+
+/*
+ * Notify the buffer model that a buffer has moved into the hold position.
+ */
+static void
+usbpf_bufheld(struct usbpf_d *ud)
+{
+
+	USBPFD_LOCK_ASSERT(ud);
+}
+
+/*
+ * Free buffers currently in use by a descriptor.
+ * Called on close.
+ */
+static void
+usbpf_freed(struct usbpf_d *ud)
+{
+
+	/*
+	 * We don't need to lock out interrupts since this descriptor has
+	 * been detached from its interface and it yet hasn't been marked
+	 * free.
+	 */
+	usbpf_free(ud);
+	if (ud->ud_rfilter != NULL)
+		free((caddr_t)ud->ud_rfilter, M_USBPF);
+	if (ud->ud_wfilter != NULL)
+		free((caddr_t)ud->ud_wfilter, M_USBPF);
+	mtx_destroy(&ud->ud_mtx);
+}
+
+/*
+ * Close the descriptor by detaching it from its interface,
+ * deallocating its buffers, and marking it free.
+ */
+static void
+usbpf_dtor(void *data)
+{
+	struct usbpf_d *ud = data;
+
+	USBPFD_LOCK(ud);
+	if (ud->ud_state == USBPF_WAITING)
+		callout_stop(&ud->ud_callout);
+	ud->ud_state = USBPF_IDLE;
+	USBPFD_UNLOCK(ud);
+	funsetown(&ud->ud_sigio);
+	mtx_lock(&usbpf_mtx);
+	if (ud->ud_bif)
+		usbpf_detachd(ud);
+	mtx_unlock(&usbpf_mtx);
+	selwakeuppri(&ud->ud_sel, PRIUSB);
+	knlist_destroy(&ud->ud_sel.si_note);
+	callout_drain(&ud->ud_callout);
+	usbpf_freed(ud);
+	free(ud, M_USBPF);
+}
+
+/*
+ * Open device.  Returns ENXIO for illegal minor device number,
+ * EBUSY if file is open by another process.
+ */
+/* ARGSUSED */
+static	int
+usbpf_open(struct cdev *dev, int flags, int fmt, struct thread *td)
+{
+	struct usbpf_d *ud;
+	int error;
+
+	ud = malloc(sizeof(*ud), M_USBPF, M_WAITOK | M_ZERO);
+	error = devfs_set_cdevpriv(ud, usbpf_dtor);
+	if (error != 0) {
+		free(ud, M_USBPF);
+		return (error);
+	}
+
+	usbpf_buffer_init(ud);
+	ud->ud_bufmode = USBPF_BUFMODE_BUFFER;
+	ud->ud_sig = SIGIO;
+	ud->ud_direction = USBPF_D_INOUT;
+	ud->ud_pid = td->td_proc->p_pid;
+	mtx_init(&ud->ud_mtx, devtoname(dev), "usbpf cdev lock", MTX_DEF);
+	callout_init_mtx(&ud->ud_callout, &ud->ud_mtx, 0);
+	knlist_init_mtx(&ud->ud_sel.si_note, &ud->ud_mtx);
+
+	return (0);
+}
+
+static int
+usbpf_uiomove(struct usbpf_d *ud, caddr_t buf, u_int len, struct uio *uio)
+{
+
+	if (ud->ud_bufmode != USBPF_BUFMODE_BUFFER)
+		return (EOPNOTSUPP);
+	return (usbpf_buffer_uiomove(ud, buf, len, uio));
+}
+
+/*
+ *  usbpf_read - read next chunk of packets from buffers
+ */
+static	int
+usbpf_read(struct cdev *dev, struct uio *uio, int ioflag)
+{
+	struct usbpf_d *ud;
+	int error;
+	int non_block;
+	int timed_out;
+
+	error = devfs_get_cdevpriv((void **)&ud);
+	if (error != 0)
+		return (error);
+
+	/*
+	 * Restrict application to use a buffer the same size as
+	 * as kernel buffers.
+	 */
+	if (uio->uio_resid != ud->ud_bufsize)
+		return (EINVAL);
+
+	non_block = ((ioflag & O_NONBLOCK) != 0);
+
+	USBPFD_LOCK(ud);
+	ud->ud_pid = curthread->td_proc->p_pid;
+	if (ud->ud_bufmode != USBPF_BUFMODE_BUFFER) {
+		USBPFD_UNLOCK(ud);
+		return (EOPNOTSUPP);
+	}
+	if (ud->ud_state == USBPF_WAITING)
+		callout_stop(&ud->ud_callout);
+	timed_out = (ud->ud_state == USBPF_TIMED_OUT);
+	ud->ud_state = USBPF_IDLE;
+	/*
+	 * If the hold buffer is empty, then do a timed sleep, which
+	 * ends when the timeout expires or when enough packets
+	 * have arrived to fill the store buffer.
+	 */
+	while (ud->ud_hbuf == NULL) {
+		if (ud->ud_slen != 0) {
+			/*
+			 * A packet(s) either arrived since the previous
+			 * read or arrived while we were asleep.
+			 */
+			if (ud->ud_immediate || non_block || timed_out) {

*** DIFF OUTPUT TRUNCATED AT 1000 LINES ***


More information about the svn-src-all mailing list