svn commit: r213677 - in user/weongyo/usb/sys/dev/usb: . controller
Weongyo Jeong
weongyo at FreeBSD.org
Mon Oct 11 00:59:47 UTC 2010
Author: weongyo
Date: Mon Oct 11 00:59:46 2010
New Revision: 213677
URL: http://svn.freebsd.org/changeset/base/213677
Log:
Commits a experimental USB packet filter (aka usbpf) to capture USB
packets between the driver and the host controller. It's for USB
debugging and monitoring. Most code are extracted from bpf(9) interfaces
and customized only for USB packets. As next step the userland
application, usbdump(1), would be coming. Reviews welcome.
Added:
user/weongyo/usb/sys/dev/usb/usb_pf.c (contents, props changed)
user/weongyo/usb/sys/dev/usb/usb_pf.h (contents, props changed)
Modified:
user/weongyo/usb/sys/dev/usb/controller/usb_controller.c
user/weongyo/usb/sys/dev/usb/usb_bus.h
user/weongyo/usb/sys/dev/usb/usb_controller.h
Modified: user/weongyo/usb/sys/dev/usb/controller/usb_controller.c
==============================================================================
--- user/weongyo/usb/sys/dev/usb/controller/usb_controller.c Sun Oct 10 22:11:38 2010 (r213676)
+++ user/weongyo/usb/sys/dev/usb/controller/usb_controller.c Mon Oct 11 00:59:46 2010 (r213677)
@@ -60,6 +60,7 @@
#include <dev/usb/usb_controller.h>
#include <dev/usb/usb_bus.h>
+#include <dev/usb/usb_pf.h>
/* function prototypes */
@@ -540,6 +541,9 @@ usb_bus_struct_init(struct usb_bus *bus,
/* get all DMA memory */
if (usb_bus_mem_alloc_all(bus, USB_GET_DMA_TAG(dev)))
return (ENOMEM);
+
+ usbpf_attach(bus, &bus->uif);
+
return (0);
}
@@ -547,6 +551,35 @@ void
usb_bus_struct_fini(struct usb_bus *bus)
{
+ usbpf_detach(bus);
+
usb_bus_mem_free_all(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: user/weongyo/usb/sys/dev/usb/usb_bus.h
==============================================================================
--- user/weongyo/usb/sys/dev/usb/usb_bus.h Sun Oct 10 22:11:38 2010 (r213676)
+++ user/weongyo/usb/sys/dev/usb/usb_bus.h Mon Oct 11 00:59:46 2010 (r213677)
@@ -80,6 +80,8 @@ struct usb_bus {
struct usb_bus_methods *methods; /* filled by HC driver */
struct usb_device **devices;
+ struct usbpf_if *uif; /* USB Packet Filter */
+
void (*busmem_func)(struct usb_bus *, usb_bus_mem_callback_t *);
usb_power_mask_t hw_power_state; /* see USB_HW_POWER_XXX */
Modified: user/weongyo/usb/sys/dev/usb/usb_controller.h
==============================================================================
--- user/weongyo/usb/sys/dev/usb/usb_controller.h Sun Oct 10 22:11:38 2010 (r213676)
+++ user/weongyo/usb/sys/dev/usb/usb_controller.h Mon Oct 11 00:59:46 2010 (r213677)
@@ -199,6 +199,7 @@ int usb_bus_struct_init(struct usb_bus
void (*busmem_func)(struct usb_bus *,
usb_bus_mem_callback_t *));
void usb_bus_struct_fini(struct usb_bus *bus);
+struct usb_bus *usb_bus_find(const char *name);
void usb_bus_mem_flush_all(struct usb_bus *bus);
uint16_t usb_isoc_time_expand(struct usb_bus *bus,
uint16_t isoc_time_curr);
Added: user/weongyo/usb/sys/dev/usb/usb_pf.c
==============================================================================
--- /dev/null 00:00:00 1970 (empty, because file is newly added)
+++ user/weongyo/usb/sys/dev/usb/usb_pf.c Mon Oct 11 00:59:46 2010 (r213677)
@@ -0,0 +1,1719 @@
+/*-
+ * Copyright (c) 2010
+ * 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/socket.h>
+#include <sys/sockio.h>
+#include <sys/fcntl.h>
+#include <sys/proc.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_device.h>
+#include <dev/usb/usb_bus.h>
+#include <dev/usb/usb_pf.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
+
+/*
+ * The instruction encodings.
+ */
+
+/* instruction classes */
+#define USBPF_CLASS(code) ((code) & 0x07)
+#define USBPF_LD 0x00
+#define USBPF_LDX 0x01
+#define USBPF_ST 0x02
+#define USBPF_STX 0x03
+#define USBPF_ALU 0x04
+#define USBPF_JMP 0x05
+#define USBPF_RET 0x06
+#define USBPF_MISC 0x07
+
+/* ld/ldx fields */
+#define USBPF_SIZE(code) ((code) & 0x18)
+#define USBPF_W 0x00
+#define USBPF_H 0x08
+#define USBPF_B 0x10
+#define USBPF_MODE(code) ((code) & 0xe0)
+#define USBPF_IMM 0x00
+#define USBPF_ABS 0x20
+#define USBPF_IND 0x40
+#define USBPF_MEM 0x60
+#define USBPF_LEN 0x80
+#define USBPF_MSH 0xa0
+
+/* alu/jmp fields */
+#define USBPF_OP(code) ((code) & 0xf0)
+#define USBPF_ADD 0x00
+#define USBPF_SUB 0x10
+#define USBPF_MUL 0x20
+#define USBPF_DIV 0x30
+#define USBPF_OR 0x40
+#define USBPF_AND 0x50
+#define USBPF_LSH 0x60
+#define USBPF_RSH 0x70
+#define USBPF_NEG 0x80
+#define USBPF_JA 0x00
+#define USBPF_JEQ 0x10
+#define USBPF_JGT 0x20
+#define USBPF_JGE 0x30
+#define USBPF_JSET 0x40
+#define USBPF_SRC(code) ((code) & 0x08)
+#define USBPF_K 0x00
+#define USBPF_X 0x08
+
+/* ret - USBPF_K and USBPF_X also apply */
+#define USBPF_RVAL(code) ((code) & 0x18)
+#define USBPF_A 0x10
+
+/* misc */
+#define USBPF_MISCOP(code) ((code) & 0xf8)
+#define USBPF_TAX 0x00
+#define USBPF_TXA 0x80
+
+/*
+ * 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, ("usbpf_buffer_alloc: ud_fbuf != NULL"));
+ KASSERT(ud->ud_sbuf == NULL, ("usbpf_buffer_alloc: ud_sbuf != NULL"));
+ KASSERT(ud->ud_hbuf == NULL, ("usbpf_buffer_alloc: ud_hbuf != NULL"));
+
+ 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];
+
+ 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;
*** DIFF OUTPUT TRUNCATED AT 1000 LINES ***
More information about the svn-src-user
mailing list