- Improvements to USB PF solution
- Add more fields for USB device and host mode - Add more information to USB PF header so that decoding can easily be done by software analyzer tools like Wireshark. - Optimise usbdump to display USB streams in text format more efficiently. - Software using USB PF must be recompiled after this commit, due to structure changes. MFC after: 7 days Approved by: thompsa (mentor)
This commit is contained in:
Hans Petter Selasky
parent
90574b0a79
commit
31a1169186
Notes:
svn2git
2020-12-20 02:59:44 +00:00
svn path=/head/; revision=220301
@ -45,6 +45,7 @@ __FBSDID("$FreeBSD$");
|
||||
#include <net/if.h>
|
||||
#include <net/if_types.h>
|
||||
#include <net/bpf.h>
|
||||
#include <sys/sysctl.h>
|
||||
|
||||
#include <dev/usb/usb.h>
|
||||
#include <dev/usb/usbdi.h>
|
||||
@ -57,28 +58,44 @@ __FBSDID("$FreeBSD$");
|
||||
#include <dev/usb/usb_pf.h>
|
||||
#include <dev/usb/usb_transfer.h>
|
||||
|
||||
static int usb_no_pf;
|
||||
|
||||
SYSCTL_INT(_hw_usb, OID_AUTO, no_pf, CTLFLAG_RW,
|
||||
&usb_no_pf, 0, "Set to disable USB packet filtering");
|
||||
|
||||
TUNABLE_INT("hw.usb.no_pf", &usb_no_pf);
|
||||
|
||||
void
|
||||
usbpf_attach(struct usb_bus *ubus)
|
||||
{
|
||||
struct ifnet *ifp;
|
||||
|
||||
if (usb_no_pf != 0) {
|
||||
ubus->ifp = NULL;
|
||||
return;
|
||||
}
|
||||
|
||||
ifp = ubus->ifp = if_alloc(IFT_USB);
|
||||
if (ifp == NULL) {
|
||||
device_printf(ubus->parent, "usbpf: Could not allocate "
|
||||
"instance\n");
|
||||
return;
|
||||
}
|
||||
|
||||
if_initname(ifp, "usbus", device_get_unit(ubus->bdev));
|
||||
ifp->if_flags = IFF_CANTCONFIG;
|
||||
if_attach(ifp);
|
||||
if_up(ifp);
|
||||
|
||||
KASSERT(sizeof(struct usbpf_pkthdr) == USBPF_HDR_LEN,
|
||||
("wrong USB pf header length (%zd)", sizeof(struct usbpf_pkthdr)));
|
||||
|
||||
/*
|
||||
* XXX According to the specification of DLT_USB, it indicates packets
|
||||
* beginning with USB setup header. But not sure all packets would be.
|
||||
* XXX According to the specification of DLT_USB, it indicates
|
||||
* packets beginning with USB setup header. But not sure all
|
||||
* packets would be.
|
||||
*/
|
||||
bpfattach(ifp, DLT_USB, USBPF_HDR_LEN);
|
||||
|
||||
if (bootverbose)
|
||||
device_printf(ubus->parent, "usbpf attached\n");
|
||||
device_printf(ubus->parent, "usbpf: Attached\n");
|
||||
}
|
||||
|
||||
void
|
||||
@ -172,79 +189,204 @@ usbpf_aggregate_status(struct usb_xfer_flags_int *flags)
|
||||
return (val);
|
||||
}
|
||||
|
||||
static int
|
||||
usbpf_xfer_frame_is_read(struct usb_xfer *xfer, uint32_t frame)
|
||||
{
|
||||
int isread;
|
||||
|
||||
if ((frame == 0) && (xfer->flags_int.control_xfr != 0) &&
|
||||
(xfer->flags_int.control_hdr != 0)) {
|
||||
/* special case */
|
||||
if (xfer->flags_int.usb_mode == USB_MODE_DEVICE) {
|
||||
/* The device controller writes to memory */
|
||||
isread = 1;
|
||||
} else {
|
||||
/* The host controller reads from memory */
|
||||
isread = 0;
|
||||
}
|
||||
} else {
|
||||
isread = USB_GET_DATA_ISREAD(xfer);
|
||||
}
|
||||
return (isread);
|
||||
}
|
||||
|
||||
static uint32_t
|
||||
usbpf_xfer_precompute_size(struct usb_xfer *xfer, int type)
|
||||
{
|
||||
uint32_t totlen;
|
||||
uint32_t x;
|
||||
uint32_t nframes;
|
||||
|
||||
if (type == USBPF_XFERTAP_SUBMIT)
|
||||
nframes = xfer->nframes;
|
||||
else
|
||||
nframes = xfer->aframes;
|
||||
|
||||
totlen = USBPF_HDR_LEN + (USBPF_FRAME_HDR_LEN * nframes);
|
||||
|
||||
/* precompute all trace lengths */
|
||||
for (x = 0; x != nframes; x++) {
|
||||
if (usbpf_xfer_frame_is_read(xfer, x)) {
|
||||
if (type != USBPF_XFERTAP_SUBMIT) {
|
||||
totlen += USBPF_FRAME_ALIGN(
|
||||
xfer->frlengths[x]);
|
||||
}
|
||||
} else {
|
||||
if (type == USBPF_XFERTAP_SUBMIT) {
|
||||
totlen += USBPF_FRAME_ALIGN(
|
||||
xfer->frlengths[x]);
|
||||
}
|
||||
}
|
||||
}
|
||||
return (totlen);
|
||||
}
|
||||
|
||||
void
|
||||
usbpf_xfertap(struct usb_xfer *xfer, int type)
|
||||
{
|
||||
struct usb_endpoint *ep = xfer->endpoint;
|
||||
struct usb_page_search res;
|
||||
struct usb_xfer_root *info = xfer->xroot;
|
||||
struct usb_bus *bus = info->bus;
|
||||
struct usb_bus *bus;
|
||||
struct usbpf_pkthdr *up;
|
||||
usb_frlength_t isoc_offset = 0;
|
||||
int i;
|
||||
char *buf, *ptr, *end;
|
||||
struct usbpf_framehdr *uf;
|
||||
usb_frlength_t offset;
|
||||
uint32_t totlen;
|
||||
uint32_t frame;
|
||||
uint32_t temp;
|
||||
uint32_t nframes;
|
||||
uint32_t x;
|
||||
uint8_t *buf;
|
||||
uint8_t *ptr;
|
||||
|
||||
bus = xfer->xroot->bus;
|
||||
|
||||
/* sanity checks */
|
||||
if (usb_no_pf != 0)
|
||||
return;
|
||||
if (bus->ifp == NULL)
|
||||
return;
|
||||
if (!bpf_peers_present(bus->ifp->if_bpf))
|
||||
return;
|
||||
|
||||
totlen = usbpf_xfer_precompute_size(xfer, type);
|
||||
|
||||
if (type == USBPF_XFERTAP_SUBMIT)
|
||||
nframes = xfer->nframes;
|
||||
else
|
||||
nframes = xfer->aframes;
|
||||
|
||||
/*
|
||||
* XXX TODO
|
||||
* Allocating the buffer here causes copy operations twice what's
|
||||
* really inefficient. Copying usbpf_pkthdr and data is for USB packet
|
||||
* read filter to pass a virtually linear buffer.
|
||||
* XXX TODO XXX
|
||||
*
|
||||
* When BPF supports it we could pass a fragmented array of
|
||||
* buffers avoiding the data copy operation here.
|
||||
*/
|
||||
buf = ptr = malloc(sizeof(struct usbpf_pkthdr) + (USB_PAGE_SIZE * 5),
|
||||
M_TEMP, M_NOWAIT);
|
||||
buf = ptr = malloc(totlen, M_TEMP, M_NOWAIT);
|
||||
if (buf == NULL) {
|
||||
printf("usbpf_xfertap: out of memory\n"); /* XXX */
|
||||
device_printf(bus->parent, "usbpf: Out of memory\n");
|
||||
return;
|
||||
}
|
||||
end = buf + sizeof(struct usbpf_pkthdr) + (USB_PAGE_SIZE * 5);
|
||||
|
||||
bzero(ptr, sizeof(struct usbpf_pkthdr));
|
||||
up = (struct usbpf_pkthdr *)ptr;
|
||||
up->up_busunit = htole32(device_get_unit(bus->bdev));
|
||||
ptr += USBPF_HDR_LEN;
|
||||
|
||||
/* fill out header */
|
||||
temp = device_get_unit(bus->bdev);
|
||||
up->up_totlen = htole32(totlen);
|
||||
up->up_busunit = htole32(temp);
|
||||
up->up_address = xfer->xroot->udev->device_index;
|
||||
if (xfer->flags_int.usb_mode == USB_MODE_DEVICE)
|
||||
up->up_mode = USBPF_MODE_DEVICE;
|
||||
else
|
||||
up->up_mode = USBPF_MODE_HOST;
|
||||
up->up_type = type;
|
||||
up->up_xfertype = ep->edesc->bmAttributes & UE_XFERTYPE;
|
||||
up->up_address = xfer->address;
|
||||
up->up_endpoint = xfer->endpointno;
|
||||
up->up_flags = htole32(usbpf_aggregate_xferflags(&xfer->flags));
|
||||
up->up_status = htole32(usbpf_aggregate_status(&xfer->flags_int));
|
||||
switch (type) {
|
||||
case USBPF_XFERTAP_SUBMIT:
|
||||
up->up_length = htole32(xfer->sumlen);
|
||||
up->up_frames = htole32(xfer->nframes);
|
||||
break;
|
||||
case USBPF_XFERTAP_DONE:
|
||||
up->up_length = htole32(xfer->actlen);
|
||||
up->up_frames = htole32(xfer->aframes);
|
||||
break;
|
||||
default:
|
||||
panic("wrong usbpf type (%d)", type);
|
||||
up->up_xfertype = xfer->endpoint->edesc->bmAttributes & UE_XFERTYPE;
|
||||
temp = usbpf_aggregate_xferflags(&xfer->flags);
|
||||
up->up_flags = htole32(temp);
|
||||
temp = usbpf_aggregate_status(&xfer->flags_int);
|
||||
up->up_status = htole32(temp);
|
||||
temp = xfer->error;
|
||||
up->up_error = htole32(temp);
|
||||
temp = xfer->interval;
|
||||
up->up_interval = htole32(temp);
|
||||
up->up_frames = htole32(nframes);
|
||||
temp = xfer->max_packet_size;
|
||||
up->up_packet_size = htole32(temp);
|
||||
temp = xfer->max_packet_count;
|
||||
up->up_packet_count = htole32(temp);
|
||||
temp = xfer->endpointno;
|
||||
up->up_endpoint = htole32(temp);
|
||||
up->up_speed = xfer->xroot->udev->speed;
|
||||
|
||||
/* clear reserved area */
|
||||
memset(up->up_reserved, 0, sizeof(up->up_reserved));
|
||||
|
||||
/* init offset and frame */
|
||||
offset = 0;
|
||||
frame = 0;
|
||||
|
||||
/* iterate all the USB frames and copy data, if any */
|
||||
for (x = 0; x != nframes; x++) {
|
||||
uint32_t length;
|
||||
int isread;
|
||||
|
||||
/* get length */
|
||||
length = xfer->frlengths[x];
|
||||
|
||||
/* get frame header pointer */
|
||||
uf = (struct usbpf_framehdr *)ptr;
|
||||
ptr += USBPF_FRAME_HDR_LEN;
|
||||
|
||||
/* fill out packet header */
|
||||
uf->length = htole32(length);
|
||||
uf->flags = 0;
|
||||
|
||||
/* get information about data read/write */
|
||||
isread = usbpf_xfer_frame_is_read(xfer, x);
|
||||
|
||||
/* check if we need to copy any data */
|
||||
if (isread) {
|
||||
if (type == USBPF_XFERTAP_SUBMIT)
|
||||
length = 0;
|
||||
else {
|
||||
uf->flags |= htole32(
|
||||
USBPF_FRAMEFLAG_DATA_FOLLOWS);
|
||||
}
|
||||
} else {
|
||||
if (type != USBPF_XFERTAP_SUBMIT)
|
||||
length = 0;
|
||||
else {
|
||||
uf->flags |= htole32(
|
||||
USBPF_FRAMEFLAG_DATA_FOLLOWS);
|
||||
}
|
||||
}
|
||||
|
||||
/* check if data is read direction */
|
||||
if (isread)
|
||||
uf->flags |= htole32(USBPF_FRAMEFLAG_READ);
|
||||
|
||||
/* copy USB data, if any */
|
||||
if (length != 0) {
|
||||
/* copy data */
|
||||
usbd_copy_out(&xfer->frbuffers[frame],
|
||||
offset, ptr, length);
|
||||
|
||||
/* align length */
|
||||
temp = USBPF_FRAME_ALIGN(length);
|
||||
|
||||
/* zero pad */
|
||||
if (temp != length)
|
||||
memset(ptr + length, 0, temp - length);
|
||||
|
||||
ptr += temp;
|
||||
}
|
||||
|
||||
if (xfer->flags_int.isochronous_xfr) {
|
||||
offset += usbd_xfer_old_frame_length(xfer, x);
|
||||
} else {
|
||||
frame ++;
|
||||
}
|
||||
}
|
||||
|
||||
up->up_error = htole32(xfer->error);
|
||||
up->up_interval = htole32(xfer->interval);
|
||||
ptr += sizeof(struct usbpf_pkthdr);
|
||||
bpf_tap(bus->ifp->if_bpf, buf, totlen);
|
||||
|
||||
for (i = 0; i < up->up_frames; i++) {
|
||||
if (ptr + sizeof(uint32_t) >= end)
|
||||
goto done;
|
||||
*((uint32_t *)ptr) = htole32(xfer->frlengths[i]);
|
||||
ptr += sizeof(uint32_t);
|
||||
|
||||
if (ptr + xfer->frlengths[i] >= end)
|
||||
goto done;
|
||||
if (xfer->flags_int.isochronous_xfr == 1) {
|
||||
usbd_get_page(&xfer->frbuffers[0], isoc_offset, &res);
|
||||
isoc_offset += xfer->frlengths[i];
|
||||
} else
|
||||
usbd_get_page(&xfer->frbuffers[i], 0, &res);
|
||||
bcopy(res.buffer, ptr, xfer->frlengths[i]);
|
||||
ptr += xfer->frlengths[i];
|
||||
}
|
||||
|
||||
bpf_tap(bus->ifp->if_bpf, buf, ptr - buf);
|
||||
done:
|
||||
free(buf, M_TEMP);
|
||||
}
|
||||
|
||||
@ -38,11 +38,14 @@
|
||||
#define _DEV_USB_PF_H
|
||||
|
||||
struct usbpf_pkthdr {
|
||||
uint32_t up_totlen; /* Total length including all headers */
|
||||
uint32_t up_busunit; /* Host controller unit number */
|
||||
uint8_t up_address; /* USB device address */
|
||||
uint8_t up_endpoint; /* USB endpoint */
|
||||
uint8_t up_address; /* USB device index */
|
||||
uint8_t up_mode; /* Mode of transfer */
|
||||
#define USBPF_MODE_HOST 0
|
||||
#define USBPF_MODE_DEVICE 1
|
||||
uint8_t up_type; /* points SUBMIT / DONE */
|
||||
uint8_t up_xfertype; /* Transfer type */
|
||||
uint8_t up_xfertype; /* Transfer type, see USB2.0 spec. */
|
||||
uint32_t up_flags; /* Transfer flags */
|
||||
#define USBPF_FLAG_FORCE_SHORT_XFER (1 << 0)
|
||||
#define USBPF_FLAG_SHORT_XFER_OK (1 << 1)
|
||||
@ -67,24 +70,43 @@ struct usbpf_pkthdr {
|
||||
#define USBPF_STATUS_CONTROL_STALL (1 << 10)
|
||||
#define USBPF_STATUS_SHORT_FRAMES_OK (1 << 11)
|
||||
#define USBPF_STATUS_SHORT_XFER_OK (1 << 12)
|
||||
#if USB_HAVE_BUSDMA
|
||||
#define USBPF_STATUS_BDMA_ENABLE (1 << 13)
|
||||
#define USBPF_STATUS_BDMA_NO_POST_SYNC (1 << 14)
|
||||
#define USBPF_STATUS_BDMA_SETUP (1 << 15)
|
||||
#endif
|
||||
#define USBPF_STATUS_ISOCHRONOUS_XFR (1 << 16)
|
||||
#define USBPF_STATUS_CURR_DMA_SET (1 << 17)
|
||||
#define USBPF_STATUS_CAN_CANCEL_IMMED (1 << 18)
|
||||
#define USBPF_STATUS_DOING_CALLBACK (1 << 19)
|
||||
uint32_t up_length; /* Total data length (submit/actual) */
|
||||
uint32_t up_frames; /* USB frame number (submit/actual) */
|
||||
uint32_t up_error; /* usb_error_t */
|
||||
uint32_t up_interval; /* for interrupt and isoc */
|
||||
uint32_t up_error; /* USB error, see USB_ERR_XXX */
|
||||
uint32_t up_interval; /* For interrupt and isoc (ms) */
|
||||
uint32_t up_frames; /* Number of following frames */
|
||||
uint32_t up_packet_size; /* Packet size used */
|
||||
uint32_t up_packet_count; /* Packet count used */
|
||||
uint32_t up_endpoint; /* USB endpoint / stream ID */
|
||||
uint8_t up_speed; /* USB speed, see USB_SPEED_XXX */
|
||||
/* sizeof(struct usbpf_pkthdr) == 128 bytes */
|
||||
uint8_t up_reserved[96];
|
||||
uint8_t up_reserved[83];
|
||||
};
|
||||
|
||||
#define USBPF_HDR_LEN 128
|
||||
struct usbpf_framehdr {
|
||||
/*
|
||||
* The frame length field excludes length of frame header and
|
||||
* any alignment.
|
||||
*/
|
||||
uint32_t length;
|
||||
#define USBPF_FRAME_ALIGN(x) (((x) + 3) & ~3)
|
||||
uint32_t flags;
|
||||
#define USBPF_FRAMEFLAG_READ (1 << 0)
|
||||
#define USBPF_FRAMEFLAG_DATA_FOLLOWS (1 << 1)
|
||||
};
|
||||
|
||||
#define USBPF_HDR_LEN 128 /* bytes */
|
||||
#define USBPF_FRAME_HDR_LEN 8 /* bytes */
|
||||
|
||||
extern uint8_t usbpf_pkthdr_size_ok[
|
||||
(sizeof(struct usbpf_pkthdr) == USBPF_HDR_LEN) ? 1 : -1];
|
||||
extern uint8_t usbpf_framehdr_size_ok[
|
||||
(sizeof(struct usbpf_framehdr) == USBPF_FRAME_HDR_LEN) ? 1 : -1];
|
||||
|
||||
#define USBPF_XFERTAP_SUBMIT 0
|
||||
#define USBPF_XFERTAP_DONE 1
|
||||
|
||||
@ -664,9 +664,13 @@ usbd_transfer_setup_sub(struct usb_setup_params *parm)
|
||||
}
|
||||
xfer->max_data_length -= REQ_SIZE;
|
||||
}
|
||||
/* setup "frlengths" */
|
||||
/*
|
||||
* Setup "frlengths" and shadow "frlengths" for keeping the
|
||||
* initial frame lengths when a USB transfer is complete. This
|
||||
* information is useful when computing isochronous offsets.
|
||||
*/
|
||||
xfer->frlengths = parm->xfer_length_ptr;
|
||||
parm->xfer_length_ptr += n_frlengths;
|
||||
parm->xfer_length_ptr += 2 * n_frlengths;
|
||||
|
||||
/* setup "frbuffers" */
|
||||
xfer->frbuffers = parm->xfer_page_cache_ptr;
|
||||
@ -1579,9 +1583,12 @@ usbd_transfer_submit(struct usb_xfer *xfer)
|
||||
USB_BUS_UNLOCK(bus);
|
||||
return;
|
||||
}
|
||||
/* compute total transfer length */
|
||||
/* compute some variables */
|
||||
|
||||
for (x = 0; x != xfer->nframes; x++) {
|
||||
/* make a copy of the frlenghts[] */
|
||||
xfer->frlengths[x + xfer->max_frame_count] = xfer->frlengths[x];
|
||||
/* compute total transfer length */
|
||||
xfer->sumlen += xfer->frlengths[x];
|
||||
if (xfer->sumlen < xfer->frlengths[x]) {
|
||||
/* length wrapped around */
|
||||
@ -1970,6 +1977,22 @@ usbd_xfer_frame_data(struct usb_xfer *xfer, usb_frcount_t frindex,
|
||||
*len = xfer->frlengths[frindex];
|
||||
}
|
||||
|
||||
/*------------------------------------------------------------------------*
|
||||
* usbd_xfer_old_frame_length
|
||||
*
|
||||
* This function returns the framelength of the given frame at the
|
||||
* time the transfer was submitted. This function can be used to
|
||||
* compute the starting data pointer of the next isochronous frame
|
||||
* when an isochronous transfer has completed.
|
||||
*------------------------------------------------------------------------*/
|
||||
usb_frlength_t
|
||||
usbd_xfer_old_frame_length(struct usb_xfer *xfer, usb_frcount_t frindex)
|
||||
{
|
||||
KASSERT(frindex < xfer->max_frame_count, ("frame index overflow"));
|
||||
|
||||
return (xfer->frlengths[frindex + xfer->max_frame_count]);
|
||||
}
|
||||
|
||||
void
|
||||
usbd_xfer_status(struct usb_xfer *xfer, int *actlen, int *sumlen, int *aframes,
|
||||
int *nframes)
|
||||
|
||||
@ -496,6 +496,8 @@ void usbd_set_power_mode(struct usb_device *udev, uint8_t power_mode);
|
||||
uint8_t usbd_filter_power_mode(struct usb_device *udev, uint8_t power_mode);
|
||||
uint8_t usbd_device_attached(struct usb_device *udev);
|
||||
|
||||
usb_frlength_t
|
||||
usbd_xfer_old_frame_length(struct usb_xfer *xfer, usb_frcount_t frindex);
|
||||
void usbd_xfer_status(struct usb_xfer *xfer, int *actlen, int *sumlen,
|
||||
int *aframes, int *nframes);
|
||||
struct usb_page_cache *usbd_xfer_get_frame(struct usb_xfer *xfer,
|
||||
|
||||
@ -52,8 +52,8 @@
|
||||
|
||||
struct usbcap {
|
||||
int fd; /* fd for /dev/usbpf */
|
||||
u_int bufsize;
|
||||
char *buffer;
|
||||
uint32_t bufsize;
|
||||
uint8_t *buffer;
|
||||
|
||||
/* for -w option */
|
||||
int wfd;
|
||||
@ -62,11 +62,11 @@ struct usbcap {
|
||||
};
|
||||
|
||||
struct usbcap_filehdr {
|
||||
u_int magic;
|
||||
uint32_t magic;
|
||||
#define USBCAP_FILEHDR_MAGIC 0x9a90000e
|
||||
u_char major;
|
||||
u_char minor;
|
||||
u_char reserved[26];
|
||||
uint8_t major;
|
||||
uint8_t minor;
|
||||
uint8_t reserved[26];
|
||||
} __packed;
|
||||
|
||||
static int doexit = 0;
|
||||
@ -76,7 +76,7 @@ static const char *i_arg = "usbus0";
|
||||
static const char *r_arg = NULL;
|
||||
static const char *w_arg = NULL;
|
||||
static const char *errstr_table[USB_ERR_MAX] = {
|
||||
[USB_ERR_NORMAL_COMPLETION] = "NORMAL_COMPLETION",
|
||||
[USB_ERR_NORMAL_COMPLETION] = "0",
|
||||
[USB_ERR_PENDING_REQUESTS] = "PENDING_REQUESTS",
|
||||
[USB_ERR_NOT_STARTED] = "NOT_STARTED",
|
||||
[USB_ERR_INVAL] = "INVAL",
|
||||
@ -107,13 +107,21 @@ static const char *errstr_table[USB_ERR_MAX] = {
|
||||
[USB_ERR_NOT_LOCKED] = "NOT_LOCKED",
|
||||
};
|
||||
|
||||
static const char *xfertype_table[] = {
|
||||
static const char *xfertype_table[4] = {
|
||||
[UE_CONTROL] = "CTRL",
|
||||
[UE_ISOCHRONOUS] = "ISOC",
|
||||
[UE_BULK] = "BULK",
|
||||
[UE_INTERRUPT] = "INTR"
|
||||
};
|
||||
|
||||
static const char *speed_table[USB_SPEED_MAX] = {
|
||||
[USB_SPEED_FULL] = "FULL",
|
||||
[USB_SPEED_HIGH] = "HIGH",
|
||||
[USB_SPEED_LOW] = "LOW",
|
||||
[USB_SPEED_VARIABLE] = "VARI",
|
||||
[USB_SPEED_SUPER] = "SUPER",
|
||||
};
|
||||
|
||||
static void
|
||||
handle_sigint(int sig)
|
||||
{
|
||||
@ -122,182 +130,282 @@ handle_sigint(int sig)
|
||||
doexit = 1;
|
||||
}
|
||||
|
||||
static void
|
||||
print_flags(u_int32_t flags)
|
||||
#define FLAGS(x, name) \
|
||||
(((x) & USBPF_FLAG_##name) ? #name "|" : "")
|
||||
|
||||
#define STATUS(x, name) \
|
||||
(((x) & USBPF_STATUS_##name) ? #name "|" : "")
|
||||
|
||||
static const char *
|
||||
usb_errstr(uint32_t error)
|
||||
{
|
||||
#define PRINTFLAGS(name) \
|
||||
if ((flags & USBPF_FLAG_##name) != 0) \
|
||||
printf("%s ", #name);
|
||||
printf(" flags %#x", flags);
|
||||
printf(" < ");
|
||||
PRINTFLAGS(FORCE_SHORT_XFER);
|
||||
PRINTFLAGS(SHORT_XFER_OK);
|
||||
PRINTFLAGS(SHORT_FRAMES_OK);
|
||||
PRINTFLAGS(PIPE_BOF);
|
||||
PRINTFLAGS(PROXY_BUFFER);
|
||||
PRINTFLAGS(EXT_BUFFER);
|
||||
PRINTFLAGS(MANUAL_STATUS);
|
||||
PRINTFLAGS(NO_PIPE_OK);
|
||||
PRINTFLAGS(STALL_PIPE);
|
||||
printf(">\n");
|
||||
#undef PRINTFLAGS
|
||||
if (error >= USB_ERR_MAX || errstr_table[error] == NULL)
|
||||
return ("UNKNOWN");
|
||||
else
|
||||
return (errstr_table[error]);
|
||||
}
|
||||
|
||||
static const char *
|
||||
usb_speedstr(uint8_t speed)
|
||||
{
|
||||
if (speed >= USB_SPEED_MAX || speed_table[speed] == NULL)
|
||||
return ("UNKNOWN");
|
||||
else
|
||||
return (speed_table[speed]);
|
||||
}
|
||||
|
||||
static void
|
||||
print_status(u_int32_t status)
|
||||
print_flags(uint32_t flags)
|
||||
{
|
||||
#define PRINTSTATUS(name) \
|
||||
if ((status & USBPF_STATUS_##name) != 0) \
|
||||
printf("%s ", #name);
|
||||
printf(" flags %#x <%s%s%s%s%s%s%s%s%s0>\n",
|
||||
flags,
|
||||
FLAGS(flags, FORCE_SHORT_XFER),
|
||||
FLAGS(flags, SHORT_XFER_OK),
|
||||
FLAGS(flags, SHORT_FRAMES_OK),
|
||||
FLAGS(flags, PIPE_BOF),
|
||||
FLAGS(flags, PROXY_BUFFER),
|
||||
FLAGS(flags, EXT_BUFFER),
|
||||
FLAGS(flags, MANUAL_STATUS),
|
||||
FLAGS(flags, NO_PIPE_OK),
|
||||
FLAGS(flags, STALL_PIPE));
|
||||
}
|
||||
|
||||
printf(" status %#x", status);
|
||||
printf(" < ");
|
||||
PRINTSTATUS(OPEN);
|
||||
PRINTSTATUS(TRANSFERRING);
|
||||
PRINTSTATUS(DID_DMA_DELAY);
|
||||
PRINTSTATUS(DID_CLOSE);
|
||||
PRINTSTATUS(DRAINING);
|
||||
PRINTSTATUS(STARTED);
|
||||
PRINTSTATUS(BW_RECLAIMED);
|
||||
PRINTSTATUS(CONTROL_XFR);
|
||||
PRINTSTATUS(CONTROL_HDR);
|
||||
PRINTSTATUS(CONTROL_ACT);
|
||||
PRINTSTATUS(CONTROL_STALL);
|
||||
PRINTSTATUS(SHORT_FRAMES_OK);
|
||||
PRINTSTATUS(SHORT_XFER_OK);
|
||||
#if USB_HAVE_BUSDMA
|
||||
PRINTSTATUS(BDMA_ENABLE);
|
||||
PRINTSTATUS(BDMA_NO_POST_SYNC);
|
||||
PRINTSTATUS(BDMA_SETUP);
|
||||
#endif
|
||||
PRINTSTATUS(ISOCHRONOUS_XFR);
|
||||
PRINTSTATUS(CURR_DMA_SET);
|
||||
PRINTSTATUS(CAN_CANCEL_IMMED);
|
||||
PRINTSTATUS(DOING_CALLBACK);
|
||||
printf(">\n");
|
||||
#undef PRINTSTATUS
|
||||
static void
|
||||
print_status(uint32_t status)
|
||||
{
|
||||
printf(" status %#x <%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s%s0>\n",
|
||||
status,
|
||||
STATUS(status, OPEN),
|
||||
STATUS(status, TRANSFERRING),
|
||||
STATUS(status, DID_DMA_DELAY),
|
||||
STATUS(status, DID_CLOSE),
|
||||
STATUS(status, DRAINING),
|
||||
STATUS(status, STARTED),
|
||||
STATUS(status, BW_RECLAIMED),
|
||||
STATUS(status, CONTROL_XFR),
|
||||
STATUS(status, CONTROL_HDR),
|
||||
STATUS(status, CONTROL_ACT),
|
||||
STATUS(status, CONTROL_STALL),
|
||||
STATUS(status, SHORT_FRAMES_OK),
|
||||
STATUS(status, SHORT_XFER_OK),
|
||||
STATUS(status, BDMA_ENABLE),
|
||||
STATUS(status, BDMA_NO_POST_SYNC),
|
||||
STATUS(status, BDMA_SETUP),
|
||||
STATUS(status, ISOCHRONOUS_XFR),
|
||||
STATUS(status, CURR_DMA_SET),
|
||||
STATUS(status, CAN_CANCEL_IMMED),
|
||||
STATUS(status, DOING_CALLBACK));
|
||||
}
|
||||
|
||||
/*
|
||||
* Dump a byte into hex format.
|
||||
*/
|
||||
static void
|
||||
hexbyte(char *buf, uint8_t temp)
|
||||
{
|
||||
uint8_t lo;
|
||||
uint8_t hi;
|
||||
|
||||
lo = temp & 0xF;
|
||||
hi = temp >> 4;
|
||||
|
||||
if (hi < 10)
|
||||
buf[0] = '0' + hi;
|
||||
else
|
||||
buf[0] = 'A' + hi - 10;
|
||||
|
||||
if (lo < 10)
|
||||
buf[1] = '0' + lo;
|
||||
else
|
||||
buf[1] = 'A' + lo - 10;
|
||||
}
|
||||
|
||||
/*
|
||||
* Display a region in traditional hexdump format.
|
||||
*/
|
||||
static void
|
||||
hexdump(const char *region, size_t len)
|
||||
hexdump(const uint8_t *region, uint32_t len)
|
||||
{
|
||||
const char *line;
|
||||
const uint8_t *line;
|
||||
char linebuf[128];
|
||||
int i;
|
||||
int x;
|
||||
int c;
|
||||
#define EMIT(fmt, ...) do { \
|
||||
printf(fmt,## __VA_ARGS__); \
|
||||
} while (0)
|
||||
|
||||
for (line = region; line < (region + len); line += 16) {
|
||||
EMIT(" %04lx ", (long) (line - region));
|
||||
|
||||
i = 0;
|
||||
|
||||
linebuf[i] = ' ';
|
||||
hexbyte(linebuf + i + 1, ((line - region) >> 8) & 0xFF);
|
||||
hexbyte(linebuf + i + 3, (line - region) & 0xFF);
|
||||
linebuf[i + 5] = ' ';
|
||||
linebuf[i + 6] = ' ';
|
||||
i += 7;
|
||||
|
||||
for (x = 0; x < 16; x++) {
|
||||
if ((line + x) < (region + len))
|
||||
EMIT("%02x ", *(const u_int8_t *)(line + x));
|
||||
else
|
||||
EMIT("-- ");
|
||||
if (x == 7)
|
||||
EMIT(" ");
|
||||
if ((line + x) < (region + len)) {
|
||||
hexbyte(linebuf + i,
|
||||
*(const u_int8_t *)(line + x));
|
||||
} else {
|
||||
linebuf[i] = '-';
|
||||
linebuf[i + 1] = '-';
|
||||
}
|
||||
linebuf[i + 2] = ' ';
|
||||
if (x == 7) {
|
||||
linebuf[i + 3] = ' ';
|
||||
i += 4;
|
||||
} else {
|
||||
i += 3;
|
||||
}
|
||||
}
|
||||
EMIT(" |");
|
||||
linebuf[i] = ' ';
|
||||
linebuf[i + 1] = '|';
|
||||
i += 2;
|
||||
for (x = 0; x < 16; x++) {
|
||||
if ((line + x) < (region + len)) {
|
||||
c = *(const u_int8_t *)(line + x);
|
||||
/* !isprint(c) */
|
||||
if ((c < ' ') || (c > '~'))
|
||||
c = '.';
|
||||
EMIT("%c", c);
|
||||
} else
|
||||
EMIT(" ");
|
||||
linebuf[i] = c;
|
||||
} else {
|
||||
linebuf[i] = ' ';
|
||||
}
|
||||
i++;
|
||||
}
|
||||
EMIT("|\n");
|
||||
linebuf[i] = '|';
|
||||
linebuf[i + 1] = 0;
|
||||
i += 2;
|
||||
puts(linebuf);
|
||||
}
|
||||
#undef EMIT
|
||||
}
|
||||
|
||||
static void
|
||||
print_apacket(const struct bpf_xhdr *hdr, struct usbpf_pkthdr *up,
|
||||
const char *payload)
|
||||
print_apacket(const struct bpf_xhdr *hdr, const uint8_t *ptr, int ptr_len)
|
||||
{
|
||||
struct tm *tm;
|
||||
struct usbpf_pkthdr up_temp;
|
||||
struct usbpf_pkthdr *up;
|
||||
struct timeval tv;
|
||||
size_t len;
|
||||
u_int32_t framelen, x;
|
||||
const char *ptr = payload;
|
||||
uint32_t x;
|
||||
char buf[64];
|
||||
|
||||
/* A packet from the kernel is based on little endian byte order. */
|
||||
ptr += USBPF_HDR_LEN;
|
||||
ptr_len -= USBPF_HDR_LEN;
|
||||
if (ptr_len < 0)
|
||||
return;
|
||||
|
||||
/* make sure we don't change the source buffer */
|
||||
memcpy(&up_temp, ptr - USBPF_HDR_LEN, sizeof(up_temp));
|
||||
up = &up_temp;
|
||||
|
||||
/*
|
||||
* A packet from the kernel is based on little endian byte
|
||||
* order.
|
||||
*/
|
||||
up->up_totlen = le32toh(up->up_totlen);
|
||||
up->up_busunit = le32toh(up->up_busunit);
|
||||
up->up_address = le32toh(up->up_address);
|
||||
up->up_flags = le32toh(up->up_flags);
|
||||
up->up_status = le32toh(up->up_status);
|
||||
up->up_length = le32toh(up->up_length);
|
||||
up->up_frames = le32toh(up->up_frames);
|
||||
up->up_error = le32toh(up->up_error);
|
||||
up->up_interval = le32toh(up->up_interval);
|
||||
up->up_frames = le32toh(up->up_frames);
|
||||
up->up_packet_size = le32toh(up->up_packet_size);
|
||||
up->up_packet_count = le32toh(up->up_packet_count);
|
||||
up->up_endpoint = le32toh(up->up_endpoint);
|
||||
|
||||
tv.tv_sec = hdr->bh_tstamp.bt_sec;
|
||||
tv.tv_usec = hdr->bh_tstamp.bt_frac;
|
||||
tm = localtime(&tv.tv_sec);
|
||||
|
||||
len = strftime(buf, sizeof(buf), "%H:%M:%S", tm);
|
||||
printf("%.*s.%06ju", (int)len, buf, tv.tv_usec);
|
||||
printf(" usbus%d.%d 0x%02x %s %s", up->up_busunit, up->up_address,
|
||||
up->up_endpoint,
|
||||
|
||||
printf("%.*s.%06ju usbus%d.%d %s-%s-EP=%08x,SPD=%s,NFR=%d,SLEN=%d,IVAL=%d%s%s\n",
|
||||
(int)len, buf, tv.tv_usec,
|
||||
(int)up->up_busunit, (int)up->up_address,
|
||||
(up->up_type == USBPF_XFERTAP_SUBMIT) ? "SUBM" : "DONE",
|
||||
xfertype_table[up->up_xfertype],
|
||||
up->up_type == USBPF_XFERTAP_SUBMIT ? "S" : "D");
|
||||
printf(" (%d/%d)", up->up_frames, up->up_length);
|
||||
if (up->up_type == USBPF_XFERTAP_DONE)
|
||||
printf(" %s", errstr_table[up->up_error]);
|
||||
if (up->up_xfertype == UE_BULK || up->up_xfertype == UE_ISOCHRONOUS)
|
||||
printf(" %d", up->up_interval);
|
||||
printf("\n");
|
||||
(unsigned int)up->up_endpoint,
|
||||
usb_speedstr(up->up_speed),
|
||||
(int)up->up_frames,
|
||||
(int)(up->up_totlen - USBPF_HDR_LEN -
|
||||
(USBPF_FRAME_HDR_LEN * up->up_frames)),
|
||||
(int)up->up_interval,
|
||||
(up->up_type == USBPF_XFERTAP_DONE) ? ",ERR=" : "",
|
||||
(up->up_type == USBPF_XFERTAP_DONE) ?
|
||||
usb_errstr(up->up_error) : "");
|
||||
|
||||
if (verbose >= 1) {
|
||||
for (x = 0; x < up->up_frames; x++) {
|
||||
framelen = le32toh(*((const u_int32_t *)ptr));
|
||||
ptr += sizeof(u_int32_t);
|
||||
printf(" frame[%u] len %d\n", x, framelen);
|
||||
assert(framelen < (1024 * 4));
|
||||
hexdump(ptr, framelen);
|
||||
ptr += framelen;
|
||||
for (x = 0; x != up->up_frames; x++) {
|
||||
const struct usbpf_framehdr *uf;
|
||||
uint32_t framelen;
|
||||
uint32_t flags;
|
||||
|
||||
uf = (const struct usbpf_framehdr *)ptr;
|
||||
ptr += USBPF_FRAME_HDR_LEN;
|
||||
ptr_len -= USBPF_FRAME_HDR_LEN;
|
||||
if (ptr_len < 0)
|
||||
return;
|
||||
|
||||
framelen = le32toh(uf->length);
|
||||
flags = le32toh(uf->flags);
|
||||
|
||||
printf(" frame[%u] %s %d bytes\n",
|
||||
(unsigned int)x,
|
||||
(flags & USBPF_FRAMEFLAG_READ) ? "READ" : "WRITE",
|
||||
(int)framelen);
|
||||
|
||||
if (flags & USBPF_FRAMEFLAG_DATA_FOLLOWS) {
|
||||
|
||||
int tot_frame_len;
|
||||
|
||||
tot_frame_len = USBPF_FRAME_ALIGN(framelen);
|
||||
|
||||
ptr_len -= tot_frame_len;
|
||||
|
||||
if (tot_frame_len < 0 ||
|
||||
(int)framelen < 0 || (int)ptr_len < 0)
|
||||
break;
|
||||
|
||||
hexdump(ptr, framelen);
|
||||
|
||||
ptr += tot_frame_len;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (verbose >= 2) {
|
||||
if (verbose >= 2)
|
||||
print_flags(up->up_flags);
|
||||
if (verbose >= 3)
|
||||
print_status(up->up_status);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
print_packets(char *data, const int datalen)
|
||||
print_packets(uint8_t *data, const int datalen)
|
||||
{
|
||||
struct usbpf_pkthdr *up;
|
||||
const struct bpf_xhdr *hdr;
|
||||
u_int32_t framelen, x;
|
||||
char *ptr, *next;
|
||||
uint8_t *ptr;
|
||||
uint8_t *next;
|
||||
|
||||
for (ptr = data; ptr < (data + datalen); ptr = next) {
|
||||
hdr = (const struct bpf_xhdr *)ptr;
|
||||
up = (struct usbpf_pkthdr *)(ptr + hdr->bh_hdrlen);
|
||||
next = ptr + BPF_WORDALIGN(hdr->bh_hdrlen + hdr->bh_caplen);
|
||||
|
||||
ptr = ((char *)up) + sizeof(struct usbpf_pkthdr);
|
||||
if (w_arg == NULL)
|
||||
print_apacket(hdr, up, ptr);
|
||||
pkt_captured++;
|
||||
for (x = 0; x < up->up_frames; x++) {
|
||||
framelen = le32toh(*((const u_int32_t *)ptr));
|
||||
ptr += sizeof(u_int32_t) + framelen;
|
||||
if (w_arg == NULL) {
|
||||
print_apacket(hdr, ptr +
|
||||
hdr->bh_hdrlen, hdr->bh_caplen);
|
||||
}
|
||||
pkt_captured++;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
write_packets(struct usbcap *p, const char *data, const int datalen)
|
||||
write_packets(struct usbcap *p, const uint8_t *data, const int datalen)
|
||||
{
|
||||
int len = htole32(datalen), ret;
|
||||
int len = htole32(datalen);
|
||||
int ret;
|
||||
|
||||
ret = write(p->wfd, &len, sizeof(int));
|
||||
assert(ret == sizeof(int));
|
||||
@ -308,8 +416,9 @@ write_packets(struct usbcap *p, const char *data, const int datalen)
|
||||
static void
|
||||
read_file(struct usbcap *p)
|
||||
{
|
||||
int datalen, ret;
|
||||
char *data;
|
||||
int datalen;
|
||||
int ret;
|
||||
uint8_t *data;
|
||||
|
||||
while ((ret = read(p->rfd, &datalen, sizeof(int))) == sizeof(int)) {
|
||||
datalen = le32toh(datalen);
|
||||
@ -330,7 +439,7 @@ do_loop(struct usbcap *p)
|
||||
int cc;
|
||||
|
||||
while (doexit == 0) {
|
||||
cc = read(p->fd, (char *)p->buffer, p->bufsize);
|
||||
cc = read(p->fd, (uint8_t *)p->buffer, p->bufsize);
|
||||
if (cc < 0) {
|
||||
switch (errno) {
|
||||
case EINTR:
|
||||
@ -364,7 +473,7 @@ init_rfile(struct usbcap *p)
|
||||
assert(ret == sizeof(uf));
|
||||
assert(le32toh(uf.magic) == USBCAP_FILEHDR_MAGIC);
|
||||
assert(uf.major == 0);
|
||||
assert(uf.minor == 1);
|
||||
assert(uf.minor == 2);
|
||||
}
|
||||
|
||||
static void
|
||||
@ -381,7 +490,7 @@ init_wfile(struct usbcap *p)
|
||||
bzero(&uf, sizeof(uf));
|
||||
uf.magic = htole32(USBCAP_FILEHDR_MAGIC);
|
||||
uf.major = 0;
|
||||
uf.minor = 1;
|
||||
uf.minor = 2;
|
||||
ret = write(p->wfd, (const void *)&uf, sizeof(uf));
|
||||
assert(ret == sizeof(uf));
|
||||
}
|
||||
@ -412,7 +521,7 @@ main(int argc, char *argv[])
|
||||
struct usbcap uc, *p = &uc;
|
||||
struct ifreq ifr;
|
||||
long snapshot = 192;
|
||||
u_int v;
|
||||
uint32_t v;
|
||||
int fd, o;
|
||||
const char *optstring;
|
||||
|
||||
@ -471,9 +580,13 @@ main(int argc, char *argv[])
|
||||
return (EXIT_FAILURE);
|
||||
}
|
||||
|
||||
if ((ioctl(fd, BIOCGBLEN, (caddr_t)&v) < 0) || v < 4096)
|
||||
v = 4096;
|
||||
for ( ; v != 0; v >>= 1) {
|
||||
/* USB transfers can be greater than 64KByte */
|
||||
v = 1U << 16;
|
||||
|
||||
/* clear ifr structure */
|
||||
memset(&ifr, 0, sizeof(ifr));
|
||||
|
||||
for ( ; v >= USBPF_HDR_LEN; v >>= 1) {
|
||||
(void)ioctl(fd, BIOCSBLEN, (caddr_t)&v);
|
||||
(void)strncpy(ifr.ifr_name, i_arg, sizeof(ifr.ifr_name));
|
||||
if (ioctl(fd, BIOCSETIF, (caddr_t)&ifr) >= 0)
|
||||
@ -490,7 +603,7 @@ main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
p->bufsize = v;
|
||||
p->buffer = (u_char *)malloc(p->bufsize);
|
||||
p->buffer = (uint8_t *)malloc(p->bufsize);
|
||||
if (p->buffer == NULL) {
|
||||
fprintf(stderr, "malloc: %s", strerror(errno));
|
||||
return (EXIT_FAILURE);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user